Contents

Building Constraints in SymPy

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

  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\)

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.

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])

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

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

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

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}\).

C = sympy.Matrix([q1  - q4 + L2/2*sympy.cos(q6),
                 q2 - q5 + L2/2*sympy.sin(q6),
                 q2,
                 q3])
C
\[\begin{split}\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]\end{split}\]

Next, take the Jacobian, \(\mathbf{C_q}\).

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

Cq dimensions: (4, 6)
\[\begin{split}\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]\end{split}\]

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}\)

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

Qd
\[\begin{split}\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]\end{split}\]

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\).

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

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

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. ]])

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

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

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

previous

Embedding technique: solution of link pinned to ground at origin

next

Augmented solution for one-bar linkage