Wagen mit mathematischem Pendel, Bewegungsgleichung in Sympy

Mechanik
Python
Sympy
Autor:in

Johannes Kaisinger

Veröffentlichungsdatum

11. August 2021

In diesem Notebook wollen wir die Bewegungsgleichung des Systems Wagen mit mathematischem Pendel herleiten. Für die symbolische Mathematik steht Python-Bibliothek sympy zur Verfügung.

Der hier gezeigte Code entspricht dem Vorgehen des letzten Blogeintrags.

Code - Start

from sympy import init_printing
init_printing()

from sympy import *

Hilfsfunktionen

# vector 2 spintensor
def vec2spin(v):
    S = Matrix([[0,-v[2],v[1]],[v[2],0,-v[0]],[-v[1],v[0],0]]) 
    return S

# spintensor 2 vector
def spin2vec(S):
    s = Matrix([S[2,1],S[0,2],S[1,0]]) 
    return s

Parameter

# parameters
g = symbols('g', positive=True)
m1, s1, d1 = symbols('m1 s1 d1', positive=True)
m2, s2, d2 = symbols('m2 s2 d2', positive=True)

print(g.is_positive)
True

Kräfte und Momente

# Forces and Moments
F1 = symbols('F1')
u_vec = Matrix([F1])

Koordinaten

# coordinates
q1, q1d, q1dd = symbols('q_1, qdot_1 qddot_1')
q2, q2d, q2dd = symbols('q_2, qdot_2 qddot_2')
display(q1, q1d, q1dd)
display(q2, q2d, q2dd)

\(\displaystyle q_{1}\)

\(\displaystyle \dot{q}_{1}\)

\(\displaystyle \ddot{q}_{1}\)

\(\displaystyle q_{2}\)

\(\displaystyle \dot{q}_{2}\)

\(\displaystyle \ddot{q}_{2}\)

# states
x1, x2, x3, x4 = symbols('x1 x2 x3 x4')
# coordinates subs lists
list_q2x = [(q1, x1), (q2, x2), (q1d, x3), (q2d, x4)]
list_x2q = [(x1, q1), (x2, q2), (x3, q1d), (x4, q2d)]
q_vec =  Matrix([[q1],[q2]])
q_vec

\(\displaystyle \left[\begin{matrix}q_{1}\\q_{2}\end{matrix}\right]\)

qd_vec =  Matrix([[q1d],[q2d]])
qd_vec

\(\displaystyle \left[\begin{matrix}\dot{q}_{1}\\\dot{q}_{2}\end{matrix}\right]\)

qdd_vec =  Matrix([[q1dd],[q2dd]])
qdd_vec

\(\displaystyle \left[\begin{matrix}\ddot{q}_{1}\\\ddot{q}_{2}\end{matrix}\right]\)

x_vec =  Matrix([[x1],[x2],[x3],[x4]])
x_vec

\(\displaystyle \left[\begin{matrix}x_{1}\\x_{2}\\x_{3}\\x_{4}\end{matrix}\right]\)

x_vec =  Matrix([[x1],[x2],[x3],[x4]])
x_vec

\(\displaystyle \left[\begin{matrix}x_{1}\\x_{2}\\x_{3}\\x_{4}\end{matrix}\right]\)

q_and_qd = Matrix([[q_vec],[qd_vec]])
q_and_qd

\(\displaystyle \left[\begin{matrix}q_{1}\\q_{2}\\\dot{q}_{1}\\\dot{q}_{2}\end{matrix}\right]\)

q_and_qd.subs(list_q2x)

\(\displaystyle \left[\begin{matrix}x_{1}\\x_{2}\\x_{3}\\x_{4}\end{matrix}\right]\)

Lagrange II - Terme

Transformationsmatrizen

A_1I = eye(3)
A_1I

\(\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{matrix}\right]\)

A_I1 = A_1I.T
A_I1

\(\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{matrix}\right]\)

A_21 = Matrix([
    [cos(q2), sin(q2), 0],
    [-sin(q2), cos(q2), 0],
    [0, 0, 1]])
A_21

\(\displaystyle \left[\begin{matrix}\cos{\left(q_{2} \right)} & \sin{\left(q_{2} \right)} & 0\\- \sin{\left(q_{2} \right)} & \cos{\left(q_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]\)

A_12 = A_21.T
A_12

\(\displaystyle \left[\begin{matrix}\cos{\left(q_{2} \right)} & - \sin{\left(q_{2} \right)} & 0\\\sin{\left(q_{2} \right)} & \cos{\left(q_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]\)

A_2I = simplify(A_21@A_1I)
A_2I

\(\displaystyle \left[\begin{matrix}\cos{\left(q_{2} \right)} & \sin{\left(q_{2} \right)} & 0\\- \sin{\left(q_{2} \right)} & \cos{\left(q_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]\)

A_I2 = A_2I.T
A_I2

\(\displaystyle \left[\begin{matrix}\cos{\left(q_{2} \right)} & - \sin{\left(q_{2} \right)} & 0\\\sin{\left(q_{2} \right)} & \cos{\left(q_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]\)

Position und Geschwindigkeitsvektoren¶

Körper 1:

rs1_1 = Matrix([[q1],[0],[0]])
rs1_1

\(\displaystyle \left[\begin{matrix}q_{1}\\0\\0\end{matrix}\right]\)

vs1_1 = rs1_1.jacobian(q_vec)@qd_vec
vs1_1

\(\displaystyle \left[\begin{matrix}\dot{q}_{1}\\0\\0\end{matrix}\right]\)

Korper 2:

phi_I2_2 = Matrix([[0],[0],[q2]])
phi_I2_2

\(\displaystyle \left[\begin{matrix}0\\0\\q_{2}\end{matrix}\right]\)

w_I2_2 = phi_I2_2.jacobian(q_vec)@qd_vec
wspin_I2_2 = vec2spin(w_I2_2)
wspin_I2_2

\(\displaystyle \left[\begin{matrix}0 & - \dot{q}_{2} & 0\\\dot{q}_{2} & 0 & 0\\0 & 0 & 0\end{matrix}\right]\)

ws2_2 = w_I2_2
ws2_2

\(\displaystyle \left[\begin{matrix}0\\0\\\dot{q}_{2}\end{matrix}\right]\)

# relative coordinates
rs2_2 = Matrix([[0],[-s2],[0]])+A_21 @ rs1_1
rs2_2

\(\displaystyle \left[\begin{matrix}q_{1} \cos{\left(q_{2} \right)}\\- q_{1} \sin{\left(q_{2} \right)} - s_{2}\\0\end{matrix}\right]\)

vs2_2 = simplify(rs2_2.jacobian(q_vec)@qd_vec+wspin_I2_2@rs2_2)
vs2_2

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

Energie Terme

M1_diag = diag(m1,m1,m1)
M2_diag = diag(m2,m2,m2)
M2_diag

\(\displaystyle \left[\begin{matrix}m_{2} & 0 & 0\\0 & m_{2} & 0\\0 & 0 & m_{2}\end{matrix}\right]\)

T = nsimplify(simplify(1/2*vs1_1.T@M1_diag@vs1_1 + 1/2*vs2_2.T@M2_diag@vs2_2))
T

\(\displaystyle \left[\begin{matrix}\frac{m_{1} \dot{q}_{1}^{2}}{2} + \frac{m_{2} \dot{q}_{1}^{2}}{2} + m_{2} \dot{q}_{1} \dot{q}_{2} s_{2} \cos{\left(q_{2} \right)} + \frac{m_{2} \dot{q}_{2}^{2} s_{2}^{2}}{2}\end{matrix}\right]\)

g_vec_I = Matrix([[0],[-g],[0]])
A_2I@g_vec_I

\(\displaystyle \left[\begin{matrix}- g \sin{\left(q_{2} \right)}\\- g \cos{\left(q_{2} \right)}\\0\end{matrix}\right]\)

Vg2 = simplify(- m2 * (A_2I@g_vec_I).T@rs2_2)
Vg2

\(\displaystyle \left[\begin{matrix}- g m_{2} s_{2} \cos{\left(q_{2} \right)}\end{matrix}\right]\)

V = Vg2
V

\(\displaystyle \left[\begin{matrix}- g m_{2} s_{2} \cos{\left(q_{2} \right)}\end{matrix}\right]\)

R = nsimplify(Matrix([1/2*d1*q1d**2 + 1/2*d2*q2d**2]))
R

\(\displaystyle \left[\begin{matrix}\frac{d_{1} \dot{q}_{1}^{2}}{2} + \frac{d_{2} \dot{q}_{2}^{2}}{2}\end{matrix}\right]\)

Generalisierte Kräfte

F1_1 = Matrix([[F1],[0],[0]])
F1_1

\(\displaystyle \left[\begin{matrix}F_{1}\\0\\0\end{matrix}\right]\)

Qq = vs1_1.jacobian(qd_vec).T@F1_1
Qq

\(\displaystyle \left[\begin{matrix}F_{1}\\0\end{matrix}\right]\)

Bewegungsgleichung - Ableitungen

\[ \frac{d}{dt} \left( \frac{\partial T}{\partial \dot{\mathbf{q}}} \right)^T - \left( \frac{\partial T}{\partial \mathbf{q}} \right)^T + \left( \frac{\partial V}{\partial \mathbf{q}} \right)^T + \left( \frac{\partial R}{\partial \dot{\mathbf{q}}} \right)^T = \mathbf{Q} \]

T_qd = nsimplify(T.jacobian(qd_vec).T)
T_qd

\(\displaystyle \left[\begin{matrix}m_{1} \dot{q}_{1} + m_{2} \dot{q}_{1} + m_{2} \dot{q}_{2} s_{2} \cos{\left(q_{2} \right)}\\m_{2} \dot{q}_{1} s_{2} \cos{\left(q_{2} \right)} + m_{2} \dot{q}_{2} s_{2}^{2}\end{matrix}\right]\)

T_qd_dt = simplify(T_qd.jacobian(qd_vec)@qdd_vec + T_qd.jacobian(q_vec)@qd_vec)
T_qd_dt

\(\displaystyle \left[\begin{matrix}m_{2} \ddot{q}_{2} s_{2} \cos{\left(q_{2} \right)} - m_{2} \dot{q}_{2}^{2} s_{2} \sin{\left(q_{2} \right)} + \ddot{q}_{1} \left(m_{1} + m_{2}\right)\\m_{2} s_{2} \left(\ddot{q}_{1} \cos{\left(q_{2} \right)} + \ddot{q}_{2} s_{2} - \dot{q}_{1} \dot{q}_{2} \sin{\left(q_{2} \right)}\right)\end{matrix}\right]\)

T_q = simplify(T.jacobian(q_vec).T)
T_q

\(\displaystyle \left[\begin{matrix}0\\- m_{2} \dot{q}_{1} \dot{q}_{2} s_{2} \sin{\left(q_{2} \right)}\end{matrix}\right]\)

R_qd = nsimplify(R.jacobian(qd_vec).T)
R_qd

\(\displaystyle \left[\begin{matrix}d_{1} \dot{q}_{1}\\d_{2} \dot{q}_{2}\end{matrix}\right]\)

V_q = simplify(V.jacobian(q_vec).T)
V_q

\(\displaystyle \left[\begin{matrix}0\\g m_{2} s_{2} \sin{\left(q_{2} \right)}\end{matrix}\right]\)

EoM_1 = T_qd_dt - T_q + V_q + R_qd -Qq
EoM_1

\(\displaystyle \left[\begin{matrix}- F_{1} + d_{1} \dot{q}_{1} + m_{2} \ddot{q}_{2} s_{2} \cos{\left(q_{2} \right)} - m_{2} \dot{q}_{2}^{2} s_{2} \sin{\left(q_{2} \right)} + \ddot{q}_{1} \left(m_{1} + m_{2}\right)\\d_{2} \dot{q}_{2} + g m_{2} s_{2} \sin{\left(q_{2} \right)} + m_{2} \dot{q}_{1} \dot{q}_{2} s_{2} \sin{\left(q_{2} \right)} + m_{2} s_{2} \left(\ddot{q}_{1} \cos{\left(q_{2} \right)} + \ddot{q}_{2} s_{2} - \dot{q}_{1} \dot{q}_{2} \sin{\left(q_{2} \right)}\right)\end{matrix}\right]\)

Wir haben hier nun die Bewegungsgleichung. Jedoch ist diese Darstellung für Simulation und Regelung nicht geeignet.

Bewegungsgleichung

Üblicher ist die Darstellung mit Hilfe der Massenmatrix.

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{g}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{Q}(\mathbf{q}) = \mathbf{B}(\mathbf{q}) \mathbf{u} \]

Mq = T_qd_dt.jacobian(qdd_vec)
Mq

\(\displaystyle \left[\begin{matrix}m_{1} + m_{2} & m_{2} s_{2} \cos{\left(q_{2} \right)}\\m_{2} s_{2} \cos{\left(q_{2} \right)} & m_{2} s_{2}^{2}\end{matrix}\right]\)

gqqd = simplify(T_qd_dt-T_q+R_qd+V_q-Mq@qdd_vec)
gqqd

\(\displaystyle \left[\begin{matrix}d_{1} \dot{q}_{1} - m_{2} \dot{q}_{2}^{2} s_{2} \sin{\left(q_{2} \right)}\\d_{2} \dot{q}_{2} + g m_{2} s_{2} \sin{\left(q_{2} \right)}\end{matrix}\right]\)

Ergebnis

Nun haben wir eine übliche Darstellung der Bewegungsgleichung.

Mq

\(\displaystyle \left[\begin{matrix}m_{1} + m_{2} & m_{2} s_{2} \cos{\left(q_{2} \right)}\\m_{2} s_{2} \cos{\left(q_{2} \right)} & m_{2} s_{2}^{2}\end{matrix}\right]\)

gqqd

\(\displaystyle \left[\begin{matrix}d_{1} \dot{q}_{1} - m_{2} \dot{q}_{2}^{2} s_{2} \sin{\left(q_{2} \right)}\\d_{2} \dot{q}_{2} + g m_{2} s_{2} \sin{\left(q_{2} \right)}\end{matrix}\right]\)

Qq
Qq

\(\displaystyle \left[\begin{matrix}F_{1}\\0\end{matrix}\right]\)

Vergleich

Als letztes können wir noch die beiden Gleichungen vergleichen.

\[ \begin{split} & EoM_1 := \frac{d}{dt} \left( \frac{\partial T}{\partial \dot{\mathbf{q}}} \right)^T - \left( \frac{\partial T}{\partial \mathbf{q}} \right)^T + \left( \frac{\partial V}{\partial \mathbf{q}} \right)^T + \left( \frac{\partial R}{\partial \dot{\mathbf{q}}} \right)^T - \mathbf{Q} = \mathbf{0} \\ & EoM_2 := \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{g}(\mathbf{q},\dot{\mathbf{q}}) - \mathbf{Q}(\mathbf{q}) = \mathbf{0} \end{split} \]

EoM_2 = Mq@qdd_vec+gqqd-Qq
EoM_2

\(\displaystyle \left[\begin{matrix}- F_{1} + d_{1} \dot{q}_{1} + m_{2} \ddot{q}_{2} s_{2} \cos{\left(q_{2} \right)} - m_{2} \dot{q}_{2}^{2} s_{2} \sin{\left(q_{2} \right)} + \ddot{q}_{1} \left(m_{1} + m_{2}\right)\\d_{2} \dot{q}_{2} + g m_{2} s_{2} \sin{\left(q_{2} \right)} + m_{2} \ddot{q}_{1} s_{2} \cos{\left(q_{2} \right)} + m_{2} \ddot{q}_{2} s_{2}^{2}\end{matrix}\right]\)

simplify(EoM_1-EoM_2)

\(\displaystyle \left[\begin{matrix}0\\0\end{matrix}\right]\)

Substitution (Änderung der Parameternamen)

Tipp

Für eine bessere Lesbarkeit können wir den Parametern auch nachträglich neue Namen geben. In unserem Fall ersetzen wir die \(1\) mit \(c\) für cart und die \(2\) mit \(p\) für pole. Weiters ersetzen wir die Länge \(s_2\) mit \(l\).

mc, dc = symbols('m_c  d_c', positive=True)
Fc = symbols('F_c')
mp, l, dp = symbols('m_p l d_p', positive=True)
# number to name
list_num2name = [(m1, mc), (d1, dc), (F1, Fc), (m2, mp), (d2, dp), (s2,l)]
Mq.subs(list_num2name)

\(\displaystyle \left[\begin{matrix}m_{c} + m_{p} & l m_{p} \cos{\left(q_{2} \right)}\\l m_{p} \cos{\left(q_{2} \right)} & l^{2} m_{p}\end{matrix}\right]\)

gqqd.subs(list_num2name)

\(\displaystyle \left[\begin{matrix}d_{c} \dot{q}_{1} - l m_{p} \dot{q}_{2}^{2} \sin{\left(q_{2} \right)}\\d_{p} \dot{q}_{2} + g l m_{p} \sin{\left(q_{2} \right)}\end{matrix}\right]\)

Qq.subs(list_num2name)

\(\displaystyle \left[\begin{matrix}F_{c}\\0\end{matrix}\right]\)

Anmerkungen

Für größere Systeme sollten die Durchnummerierung der Parameter beibehalten werden. Bei mechanischen Systemen mit 4 oder mehr Körper wird sonst die Parameterbezeichnung unübersichtlich.

Fazit

In diesem Notebook haben wir eine Modellbildung mit Hilfe von sympy durchgeführt. sympy hat das Ziel ein vollständiges CAS Programm bereit zu stellen.