In [1]:

import sympy
import numpy as np

Building Constraints in SymPy ¶

It can be difficult to build constraints for general dynamic solutions. Each constraint is realtively straight forward, but keeping track of Jacobian terms or acceleration constraints

$\mathbf{C_q}$
$\mathbf{Q_d} = -(\mathbf{C_q \dot{q}})_\mathbf{q}\mathbf{\dot{q}} - 2\mathbf{C}_{\mathbf{q}t}\dot{\mathbf{q}}- \mathbf{C}_{tt}$

can be cumbersome and confusing. In this notebook, you will use SymPy to

define $\mathbf{q}~and~\dot{\mathbf{q}}$
take a Jacobian and find the constraints on acceleration
lambdify the constraints, Jacobian, and $\mathbf{Q}_d$

1. define $\mathbf{q}~and~\dot{\mathbf{q}}$¶

First, define the variables using sympy.Matrix. SymPy uses var to define variables, here you create

$q = \left[\begin{matrix}q_{1}\\q_{2}\\q_{3}\\q_{4}\\q_{5}\\q_{6}\end{matrix}\right],~ \dot{q} = \left[\begin{matrix}dq_{1}\\dq_{2}\\dq_{3}\\dq_{4}\\dq_{5}\\dq_{6}\end{matrix}\right],~t,~and~L$

as SymPy variables.

In [2]:

sympy.var('q1, q2, q3, q4, q5, q6')
sympy.var('dq1, dq2, dq3, dq4, dq5, dq6')
sympy.var('t, L2')
q = sympy.Matrix([q1, q2, q3, q4, q5, q6])
dq = sympy.Matrix([dq1, dq2, dq3, dq4, dq5, dq6])

In [19]:

Out[19]:

$\displaystyle \left[\begin{matrix}q_{1}\\q_{2}\\q_{3}\\q_{4}\\q_{5}\\q_{6}\end{matrix}\right]$

$\mathbf{q} = \left[\begin{matrix}q_{1}\\q_{2}\\q_{3}\\q_{4}\\q_{5}\\q_{6}\end{matrix}\right]$

In [4]:

dq

Out[4]:

$\displaystyle \left[\begin{matrix}dq_{1}\\dq_{2}\\dq_{3}\\dq_{4}\\dq_{5}\\dq_{6}\end{matrix}\right]$

2. take a Jacobian and find the constraints on acceleration¶

two bodies: sliding block and compound pendulum

In this example, you have two rigid bodies. Body one $[q_1,~q_2,~q_3]$ slides on the x-axis. Body 1 and body 2, $[q_4,~q_5,~q_6]$ are connected by a pin in the center of body 1, $\mathbf{R}_{pin} = q_1\hat{i}+q_2\hat{j}$.

In [22]:

C = sympy.Matrix([q1  - q4 + L2/2*sympy.cos(q6),
                 q2 - q5 + L2/2*sympy.sin(q6),
                 q2,
                 q3])
C

Out[22]:

$\displaystyle \left[\begin{matrix}\frac{L_{2} \cos{\left(q_{6} \right)}}{2} + q_{1} - q_{4}\\\frac{L_{2} \sin{\left(q_{6} \right)}}{2} + q_{2} - q_{5}\\q_{2}\\q_{3}\end{matrix}\right]$

Next, take the Jacobian, $\mathbf{C_q}$.

In [27]:

Cq = C.jacobian(q)
print('Cq dimensions:', Cq.shape)
Cq

Cq dimensions: (4, 6)

Out[27]:

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

Now, you can calculate

$\mathbf{Q_d} = -(\mathbf{C_q \dot{q}})_\mathbf{q}\mathbf{\dot{q}} - 2\mathbf{C}_{\mathbf{q}t}\dot{\mathbf{q}}- \mathbf{C}_{tt}$

In [28]:

Qd = -(C.jacobian(q)@dq).jacobian(q)@dq\
    - 2*sympy.diff(Cq,t)@dq\
    - sympy.diff(C, t, 2)

Qd

Out[28]:

$\displaystyle \left[\begin{matrix}\frac{L_{2} dq_{6}^{2} \cos{\left(q_{6} \right)}}{2}\\\frac{L_{2} dq_{6}^{2} \sin{\left(q_{6} \right)}}{2}\\0\\0\end{matrix}\right]$

3. `lambdify` the constraints, Jacobian, and $\mathbf{Q}_d$¶

Finally, you can create functions that return NumPy arrays with sympy.lambdify. The inputs are

sympy.lambdify( inputs, function, "numpy" )

where the inputs are $\mathbf{q}$, $\dot{\mathbf{q}}$, and other parameters like time or dimensions, $L^2$.

In [32]:

Cq_sys = sympy.lambdify((q, t, L2), Cq, 'numpy')

In [35]:

Cq_sys(np.zeros(6), 1, L2 = 1)

Out[35]:

array([[ 1. ,  0. ,  0. , -1. ,  0. , -0. ],
       [ 0. ,  1. ,  0. ,  0. , -1. ,  0.5],
       [ 0. ,  1. ,  0. ,  0. ,  0. ,  0. ],
       [ 0. ,  0. ,  1. ,  0. ,  0. ,  0. ]])

In [36]:

Q = sympy.lambdify((q, dq, t, L2), Qd, "numpy")

In [39]:

Q(np.ones(6), np.ones(6), 0, L2 = 1)

Out[39]:

array([[0.27015115],
       [0.42073549],
       [0.        ],
       [0.        ]])

In [ ]:

Building Constraints in SymPy¶

1. define $\mathbf{q}~and~\dot{\mathbf{q}}$¶

2. take a Jacobian and find the constraints on acceleration¶

3. lambdify the constraints, Jacobian, and $\mathbf{Q}_d$¶

Building Constraints in SymPy ¶

3. `lambdify` the constraints, Jacobian, and $\mathbf{Q}_d$¶