In [1]:

    

import time
print time.ctime()
%reload_ext displaytools


    

    




Fri Jul  1 14:50:22 2016

In [2]:

    

import sympy as sp
from sympy import sin, cos, tan, pi, Matrix, Q
from sympy.interactive import printing
import scipy as sc
import scipy.optimize

import symbtools as st
import symbtools.modeltools as mt
from symbtools.modeltools import Rz
import symbtools.noncommutativetools as nct

import pycartan as pc

printing.init_printing(1)


    

In [3]:

    

t = sp.Symbol('t')
np = 1 # passiv
na = 2 # aktiv
n = np + na

qq = st.symb_vector("q1:{0}".format(n+1)) ##
qqdot = st.time_deriv(qq, qq) ##
qqddot = st.time_deriv(qq, qq, order=2) ##

st.make_global(qq)
st.make_global(qqdot)
st.make_global(qqddot)


    

    






$$\left[\begin{matrix}q_{1}\\q_{2}\\q_{3}\end{matrix}\right]$$






    




---






    






$$\left[\begin{matrix}\dot{q}_{1}\\\dot{q}_{2}\\\dot{q}_{3}\end{matrix}\right]$$






    




---






    






$$\left[\begin{matrix}\ddot{q}_{1}\\\ddot{q}_{2}\\\ddot{q}_{3}\end{matrix}\right]$$






    




---

In [4]:

    

params = sp.symbols('m1, m2, g')
st.make_global(params)

ttau = st.symb_vector("tau1:{0}".format(na+1)) ##
st.make_global(ttau)


    

    






$$\left[\begin{matrix}\tau_{1}\\\tau_{2}\end{matrix}\right]$$






    




---

In [5]:

    

#Einheitsvektoren
ex = sp.Matrix([1,0])
ey = sp.Matrix([0,1])

# Koordinaten der Schwerpunkte und Gelenke
S1 = q2*ex
S2 = S1 + Rz(q1)*(-ey)*q3

# Zeitableitungen der Schwerpunktskoordinaten
Sd1, Sd2 = st.col_split(st.time_deriv(st.col_stack(S1, S2), qq)) ##


    

    






$$\left[\begin{matrix}\dot{q}_{2}\\0\end{matrix}\right]$$






    






$$\left[\begin{matrix}q_{3} \dot{q}_{1} \cos{\left (q_{1} \right )} + \dot{q}_{2} + \dot{q}_{3} \sin{\left (q_{1} \right )}\\q_{3} \dot{q}_{1} \sin{\left (q_{1} \right )} - \dot{q}_{3} \cos{\left (q_{1} \right )}\end{matrix}\right]$$






    




---

In [6]:

    

# Energie
T_rot = 0
T_trans = (m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2)/2

T = T_rot + T_trans[0] ##
V = m2*g*S2[1]


    

    






$$\frac{m_{1} \dot{q}_{2}^{2}}{2} + \frac{m_{2}}{2} \left(q_{3} \dot{q}_{1} \sin{\left (q_{1} \right )} - \dot{q}_{3} \cos{\left (q_{1} \right )}\right)^{2} + \frac{m_{2}}{2} \left(q_{3} \dot{q}_{1} \cos{\left (q_{1} \right )} + \dot{q}_{2} + \dot{q}_{3} \sin{\left (q_{1} \right )}\right)^{2}$$






    




---

In [7]:

    

external_forces = [0, tau1, tau2]
assert not any(external_forces[:np])
mod = mt.generate_symbolic_model(T, V, qq, external_forces)


    

In [8]:

    

MM = sp.simplify(mod.MM) ##


    

    






$$\left[\begin{matrix}m_{2} q_{3}^{2} & m_{2} q_{3} \cos{\left (q_{1} \right )} & 0\\m_{2} q_{3} \cos{\left (q_{1} \right )} & m_{1} + m_{2} & m_{2} \sin{\left (q_{1} \right )}\\0 & m_{2} \sin{\left (q_{1} \right )} & m_{2}\end{matrix}\right]$$






    




---

In [9]:

    

# Implizite Systemgleichung 0=FF
FF = sp.simplify(mod.eqns[:np, :]) ##
# Vereinfachung
FF = FF/(m2*q3) ##


    

    






$$\left[\begin{matrix}m_{2} q_{3} \left(g \sin{\left (q_{1} \right )} + q_{3} \ddot{q}_{1} + \ddot{q}_{2} \cos{\left (q_{1} \right )} + 2 \dot{q}_{1} \dot{q}_{3}\right)\end{matrix}\right]$$






    




---






    






$$\left[\begin{matrix}g \sin{\left (q_{1} \right )} + q_{3} \ddot{q}_{1} + \ddot{q}_{2} \cos{\left (q_{1} \right )} + 2 \dot{q}_{1} \dot{q}_{3}\end{matrix}\right]$$






    




---

In [10]:

    

s = sp.Symbol('s', commutative=False)


    

In [11]:

    

vec_x = st.symb_vector('x1:7', commutative=False)
vec_xdot = st.time_deriv(vec_x, vec_x)
st.make_global(vec_x)
st.make_global(vec_xdot)


    

In [12]:

    

FF_zustand = sp.Matrix([
        [xdot1-x4],
        [xdot2-x5],
        [xdot3-x6],
        [g*sin(x1)+x3*xdot4+2*x4*x6+xdot5*cos(x1)]
    ])
FF_zustand


    

    
Out[12]:





$$\left[\begin{matrix}- x_{4} + \dot{x}_{1}\\- x_{5} + \dot{x}_{2}\\- x_{6} + \dot{x}_{3}\\g \sin{\left (x_{1} \right )} + x_{3} \dot{x}_{4} + 2 x_{4} x_{6} + \dot{x}_{5} \cos{\left (x_{1} \right )}\end{matrix}\right]$$

In [13]:

    

P1 = FF_zustand.jacobian(vec_xdot) ##
P0 = FF_zustand.jacobian(vec_x) ##


    

    






$$\left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & x_{3} & \cos{\left (x_{1} \right )} & 0\end{matrix}\right]$$






    




---






    






$$\left[\begin{matrix}0 & 0 & 0 & -1 & 0 & 0\\0 & 0 & 0 & 0 & -1 & 0\\0 & 0 & 0 & 0 & 0 & -1\\g \cos{\left (x_{1} \right )} - \dot{x}_{5} \sin{\left (x_{1} \right )} & 0 & \dot{x}_{4} & 2 x_{6} & 0 & 2 x_{4}\end{matrix}\right]$$






    




---

In [14]:

    

basis_vec = st.row_stack(vec_x[:3,:], vec_xdot[:3,:], st.time_deriv(vec_xdot[:3,:],vec_xdot[:3,:])) ##


    

    






$$\left[\begin{matrix}x_{1}\\x_{2}\\x_{3}\\\dot{x}_{1}\\\dot{x}_{2}\\\dot{x}_{3}\\\ddot{x}_{1}\\\ddot{x}_{2}\\\ddot{x}_{3}\end{matrix}\right]$$






    




---

In [15]:

    

# Lade Ergebnis aus uc_algorithm.py Ergebnis-Datei
data = st.pickle_full_load("brueckenkran.pcl")


    

In [16]:

    

data.F_eq


    

    
Out[16]:





$$\left[\begin{matrix}- x_{4} + \dot{x}_{1}\\- x_{5} + \dot{x}_{2}\\- x_{6} + \dot{x}_{3}\\g \sin{\left (x_{1} \right )} + 2 x_{4} x_{6} + \dot{x}_{4} x_{3} + \cos{\left (x_{1} \right )} \dot{x}_{5}\end{matrix}\right]$$

In [17]:

    

# P = P1*s + P0
P = data.P ##


    

    






$$\left[\begin{matrix}s & 0 & 0 & -1 & 0 & 0\\0 & s & 0 & 0 & -1 & 0\\0 & 0 & s & 0 & 0 & -1\\g \cos{\left (x_{1} \right )} - \dot{x}_{5} \sin{\left (x_{1} \right )} & 0 & \dot{x}_{4} & x_{3} s + 2 x_{6} & \cos{\left (x_{1} \right )} s & 2 x_{4}\end{matrix}\right]$$






    




---

In [18]:

    

H = data.H ##


    

    






$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0 & 0 & 0 & 0\\2 x_{6} - \dot{x}_{1} \tan{\left (x_{1} \right )} x_{3} - \dot{x}_{3} & 0 & 2 x_{4} & x_{3} & \cos{\left (x_{1} \right )} & 0\end{matrix}\right]$$






    




---

In [19]:

    

M1 = sp.Matrix([[1, 0], [-s, 1]]) ##


    

    






$$\left[\begin{matrix}1 & 0\\- s & 1\end{matrix}\right]$$






    




---

In [20]:

    

H_ = st.col_stack(H, sp.zeros(2,3))
Hdx = pc.VectorDifferentialForm(1, basis_vec, coeff=H_)
Hdx.coeff


    

    
Out[20]:





$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 x_{6} - \dot{x}_{1} \tan{\left (x_{1} \right )} x_{3} - \dot{x}_{3} & 0 & 2 x_{4} & x_{3} & \cos{\left (x_{1} \right )} & 0 & 0 & 0 & 0\end{matrix}\right]$$

In [21]:

    

H1dx = Hdx.left_mul_by(M1,s)
H1dx.coeff


    

    
Out[21]:





$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 x_{6} - \dot{x}_{1} \tan{\left (x_{1} \right )} x_{3} - 2 \dot{x}_{3} & \sin{\left (x_{1} \right )} \dot{x}_{1} & 2 x_{4} & 0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]$$

In [22]:

    

# Substitution mit definitorischen Gleichungen: x4=xdot1, x6=xdot3
H2_ = H1dx.coeff.subs(x4, xdot1).subs(x6, xdot3)
H2dx = pc.VectorDifferentialForm(1, basis_vec, coeff=H2_)
H2dx.coeff


    

    
Out[22]:





$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\- \dot{x}_{1} \tan{\left (x_{1} \right )} x_{3} & \sin{\left (x_{1} \right )} \dot{x}_{1} & 2 \dot{x}_{1} & 0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]$$

In [23]:

    

# Betrachten der ersten 3 Spalten
H2_2 = st.col_select(H2dx.coeff, 0, 1, 2) ##


    

    






$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0\\- \dot{x}_{1} \tan{\left (x_{1} \right )} x_{3} & \sin{\left (x_{1} \right )} \dot{x}_{1} & 2 \dot{x}_{1}\end{matrix}\right]$$






    




---

In [24]:

    

# Nichtkommutativität nicht mehr benötigt: Symbole in kommutative umwandeln
H2 = nct.make_all_symbols_commutative(H2_2, appendix='')[0] ##
M2 = nct.make_all_symbols_commutative(sp.Matrix([[1,0],[0,1/xdot1]]), appendix='')[0] ##


    

    






$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0\\- x_{3} \dot{x}_{1} \tan{\left (x_{1} \right )} & \dot{x}_{1} \sin{\left (x_{1} \right )} & 2 \dot{x}_{1}\end{matrix}\right]$$






    




---






    






$$\left[\begin{matrix}1 & 0\\0 & \frac{1}{\dot{x}_{1}}\end{matrix}\right]$$






    




---

In [25]:

    

H3 = sp.simplify(M2*H2) ##


    

    






$$\left[\begin{matrix}x_{3} & \cos{\left (x_{1} \right )} & 0\\- x_{3} \tan{\left (x_{1} \right )} & \sin{\left (x_{1} \right )} & 2\end{matrix}\right]$$






    




---

In [26]:

    

M3_nct = sp.Matrix([[1/(2*x3),-1/(2*x3*tan(x1))],[1/(2*cos(x1)),1/(2*sin(x1))]])
M3 = nct.make_all_symbols_commutative(M3_nct, appendix='')[0] ##


    

    






$$\left[\begin{matrix}\frac{1}{2 x_{3}} & - \frac{1}{2 x_{3} \tan{\left (x_{1} \right )}}\\\frac{1}{2 \cos{\left (x_{1} \right )}} & \frac{1}{2 \sin{\left (x_{1} \right )}}\end{matrix}\right]$$






    




---

In [27]:

    

H4 = sp.simplify(M3*H3) ##


    

    






$$\left[\begin{matrix}1 & 0 & - \frac{1}{x_{3} \tan{\left (x_{1} \right )}}\\0 & 1 & \frac{1}{\sin{\left (x_{1} \right )}}\end{matrix}\right]$$






    




---

In [28]:

    

M4_nct = sp.Matrix([[x3*sin(x1),0],[x3*cos(x1),1]])
M4 = nct.make_all_symbols_commutative(M4_nct, appendix='')[0] ##


    

    






$$\left[\begin{matrix}x_{3} \sin{\left (x_{1} \right )} & 0\\x_{3} \cos{\left (x_{1} \right )} & 1\end{matrix}\right]$$






    




---

In [29]:

    

H5 = sp.simplify(M4*H4) ##


    

    






$$\left[\begin{matrix}x_{3} \sin{\left (x_{1} \right )} & 0 & - \cos{\left (x_{1} \right )}\\x_{3} \cos{\left (x_{1} \right )} & 1 & \sin{\left (x_{1} \right )}\end{matrix}\right]$$






    




---

In [30]:

    

basis_vec_nct = nct.make_all_symbols_commutative(basis_vec, appendix='')[0]


    

In [31]:

    

H5_ = st.col_stack(H5, sp.zeros(2,6))
H5_coeff_omega1 = st.row_split(H5_, 0)[0]
H5_coeff_omega2 = st.row_split(H5_, 1)[0]
omega1 = pc.DifferentialForm(1, basis_vec_nct, coeff=H5_coeff_omega1) ##
omega2 = pc.DifferentialForm(1, basis_vec_nct, coeff=H5_coeff_omega2) ##


    

    






(x3*sin(x1))dx1  +  (-cos(x1))dx3






    




---






    






(x3*cos(x1))dx1  +  (1)dx2  +  (sin(x1))dx3






    




---

In [32]:

    

omega1.d


    

    
Out[32]:





(0)dx1^dx2

In [33]:

    

omega2.d


    

    
Out[33]:





(0)dx1^dx2

In [34]:

    

y1 = omega1.integrate() ##


    

    






$$- x_{3} \cos{\left (x_{1} \right )}$$






    




---

In [35]:

    

y2 = omega2.integrate() ##


    

    






$$x_{2} + x_{3} \sin{\left (x_{1} \right )}$$






    




---

In [ ]: