Unconstrained Equations of Motion

Note

You can download this example as a Python script: eom.py or Jupyter Notebook: eom.ipynb.

import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting(use_latex='mathjax')

Learning Objectives

After completing this chapter readers will be able to:

  • Form the dynamical differential equations for a multibody system where \(n=p\).

  • Calculate the dynamical differential equations of motion for a single rigid body.

  • Form the equations of motion for a multibody system where \(n=p\).

  • Write the equations of motion in implicit and explicit forms.

Dynamical Differential Equations

In the previous chapter, we introduced the generalized active forces \(\bar{F}_r\) and the generalized inertia forces \(\bar{F}_r^*\). Together, these two forces give us the dynamical differential equations. The dynamical differential equations for a holonomic system with \(p=n\) degrees of freedom in an inertial reference frame. They are a function of the generalized coordinates, the generalized speeds, the time derivatives of the generalized speeds, and time. The dynamical differential equations take this form:

(188)\[\bar{F}_r + \bar{F}^*_r = \bar{f}_d(\dot{\bar{u}}, \bar{u}, \bar{q}, t) = 0\]

These are the Newton-Euler equations for a multibody system in the form presented in [Kane1985] pg. 158, thus we also call these equations Kane’s Equations. The dynamical differential equations can only be formed when motion is viewed from an inertial reference frame, because an inertial reference frame is one where Newton’s First Law holds, i.e. objects at rest stay at rest unless an external force acts on them. An inertial reference frame is one that is not accelerating, or can be assumed not to be with respect to the motion of the bodies of interest.

\(\bar{F}^*_r\) is always linear in the time derivatives of the generalized speeds and contains velocity dependent terms such as the centripetal and Coriolis forces and the rotational velocity coupling terms. These forces are sometimes called fictitious forces. \(\bar{F}_r\) are contributing forces due to body and environment interactions. Texts about dynamics will often present the dynamical differential equations in this form:

(189)\[-\bar{F}^*_r = \bar{F}_r \rightarrow \mathbf{M}(\bar{q}, t) \dot{\bar{u}} + \bar{C}(\bar{u}, \bar{q}, t) = \bar{F}(\bar{u}, \bar{q}, t)\]

where \(\mathbf{M}\) is called the mass matrix, \(\bar{C}\) is are the forces due to the various velocity effects, and \(\bar{F}\) are the contributing externally applied forces.

Body Fixed Newton-Euler Equations

To show that Kane’s Equations are equivalent to the Newton-Euler equations you may have seen before, we can find the dynamical differential equations for a single rigid body using Kane’s method and then show the results in the canonical form. For a rigid body \(B\) moving in an inertial reference frame \(A\) with its velocity and angular velocity expressed in body fixed coordinates and acted upon by a resultant force \(\bar{F}\) at the mass center \(B_o\) and a moment about the mass center \(\bar{M}\) we need these variables, reference frames, and points:

m, Ixx, Iyy, Izz = sm.symbols('m, I_{xx}, I_{yy}, I_{zz}')
Ixy, Iyz, Ixz = sm.symbols('I_{xy}, I_{yz}, I_{xz}')
Fx, Fy, Fz, Mx, My, Mz = me.dynamicsymbols('F_x, F_y, F_z, M_x, M_y, M_z')
u1, u2, u3, u4, u5, u6 = me.dynamicsymbols('u1, u2, u3, u4, u5, u6')

A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')

Bo = me.Point('Bo')

Now define the angular velocity of the body and the velocity of the mass center in terms of six generalized coordinates expressed in body fixed coordinates.

A_w_B = u4*B.x + u5*B.y + u6*B.z
B.set_ang_vel(A, A_w_B)

A_v_Bo = u1*B.x + u2*B.y + u3*B.z
Bo.set_vel(A, A_v_Bo)

Now we can find the six partial velocities and partial angular velocities. Note that we use the var_in_dcm=False keyword argument. We do this because the generalized speeds are not present in the unspecified direction cosine matrix relating \(A\) and \(B\). This allows the derivative in \(A\) to be formed without use of a direction cosine matrix. Generalized speeds will never be present in a direction cosine matrix.

v_Bo_1 = A_v_Bo.diff(u1, A, var_in_dcm=False)
v_Bo_2 = A_v_Bo.diff(u2, A, var_in_dcm=False)
v_Bo_3 = A_v_Bo.diff(u3, A, var_in_dcm=False)
v_Bo_4 = A_v_Bo.diff(u4, A, var_in_dcm=False)
v_Bo_5 = A_v_Bo.diff(u5, A, var_in_dcm=False)
v_Bo_6 = A_v_Bo.diff(u6, A, var_in_dcm=False)

v_Bo_1, v_Bo_2, v_Bo_3, v_Bo_4, v_Bo_5, v_Bo_6
\[\displaystyle \left( \hat{b}_x, \ \hat{b}_y, \ \hat{b}_z, \ 0, \ 0, \ 0\right)\]
w_B_1 = A_w_B.diff(u1, A, var_in_dcm=False)
w_B_2 = A_w_B.diff(u2, A, var_in_dcm=False)
w_B_3 = A_w_B.diff(u3, A, var_in_dcm=False)
w_B_4 = A_w_B.diff(u4, A, var_in_dcm=False)
w_B_5 = A_w_B.diff(u5, A, var_in_dcm=False)
w_B_6 = A_w_B.diff(u6, A, var_in_dcm=False)

w_B_1, w_B_2, w_B_3, w_B_4, w_B_5, w_B_6
\[\displaystyle \left( 0, \ 0, \ 0, \ \hat{b}_x, \ \hat{b}_y, \ \hat{b}_z\right)\]

The partial_velocity() function does this same thing. Notice that due to our velocity definitions, we get a very simple set of partial velocities.

par_vels = me.partial_velocity([A_v_Bo, A_w_B], [u1, u2, u3, u4, u5, u6], A)

par_vels
\[\displaystyle \left[ \left[ \hat{b}_x, \ \hat{b}_y, \ \hat{b}_z, \ 0, \ 0, \ 0\right], \ \left[ 0, \ 0, \ 0, \ \hat{b}_x, \ \hat{b}_y, \ \hat{b}_z\right]\right]\]

Now form the generalized active forces:

T = Mx*B.x + My*B.y + Mz*B.z
R = Fx*B.x + Fy*B.y + Fz*B.z

F1 = v_Bo_1.dot(R) + w_B_1.dot(T)
F2 = v_Bo_2.dot(R) + w_B_2.dot(T)
F3 = v_Bo_3.dot(R) + w_B_3.dot(T)
F4 = v_Bo_4.dot(R) + w_B_4.dot(T)
F5 = v_Bo_5.dot(R) + w_B_5.dot(T)
F6 = v_Bo_6.dot(R) + w_B_6.dot(T)

Fr = sm.Matrix([F1, F2, F3, F4, F4, F6])
Fr
\[\begin{split}\displaystyle \left[\begin{matrix}F_{x}\\F_{y}\\F_{z}\\M_{x}\\M_{x}\\M_{z}\end{matrix}\right]\end{split}\]

and the generalized inertia forces:

I = me.inertia(B, Ixx, Iyy, Izz, Ixy, Iyz, Ixz)

Rs = -m*Bo.acc(A)
Ts = -(B.ang_acc_in(A).dot(I) + me.cross(A_w_B, I).dot(A_w_B))

F1s = v_Bo_1.dot(Rs) + w_B_1.dot(Ts)
F2s = v_Bo_2.dot(Rs) + w_B_2.dot(Ts)
F3s = v_Bo_3.dot(Rs) + w_B_3.dot(Ts)
F4s = v_Bo_4.dot(Rs) + w_B_4.dot(Ts)
F5s = v_Bo_5.dot(Rs) + w_B_5.dot(Ts)
F6s = v_Bo_6.dot(Rs) + w_B_6.dot(Ts)

Frs = sm.Matrix([F1s, F2s, F3s, F4s, F5s, F6s])
Frs
\[\begin{split}\displaystyle \left[\begin{matrix}- m \left(- u_{2} u_{6} + u_{3} u_{5} + \dot{u}_{1}\right)\\- m \left(u_{1} u_{6} - u_{3} u_{4} + \dot{u}_{2}\right)\\- m \left(- u_{1} u_{5} + u_{2} u_{4} + \dot{u}_{3}\right)\\- I_{xx} \dot{u}_{4} - I_{xy} \dot{u}_{5} - I_{xz} \dot{u}_{6} - \left(- I_{xy} u_{6} + I_{xz} u_{5}\right) u_{4} - \left(- I_{yy} u_{6} + I_{yz} u_{5}\right) u_{5} - \left(- I_{yz} u_{6} + I_{zz} u_{5}\right) u_{6}\\- I_{xy} \dot{u}_{4} - I_{yy} \dot{u}_{5} - I_{yz} \dot{u}_{6} - \left(I_{xx} u_{6} - I_{xz} u_{4}\right) u_{4} - \left(I_{xy} u_{6} - I_{yz} u_{4}\right) u_{5} - \left(I_{xz} u_{6} - I_{zz} u_{4}\right) u_{6}\\- I_{xz} \dot{u}_{4} - I_{yz} \dot{u}_{5} - I_{zz} \dot{u}_{6} - \left(- I_{xx} u_{5} + I_{xy} u_{4}\right) u_{4} - \left(- I_{xy} u_{5} + I_{yy} u_{4}\right) u_{5} - \left(- I_{xz} u_{5} + I_{yz} u_{4}\right) u_{6}\end{matrix}\right]\end{split}\]

and finally Kane’s Equations:

Fr + Frs
\[\begin{split}\displaystyle \left[\begin{matrix}- m \left(- u_{2} u_{6} + u_{3} u_{5} + \dot{u}_{1}\right) + F_{x}\\- m \left(u_{1} u_{6} - u_{3} u_{4} + \dot{u}_{2}\right) + F_{y}\\- m \left(- u_{1} u_{5} + u_{2} u_{4} + \dot{u}_{3}\right) + F_{z}\\- I_{xx} \dot{u}_{4} - I_{xy} \dot{u}_{5} - I_{xz} \dot{u}_{6} - \left(- I_{xy} u_{6} + I_{xz} u_{5}\right) u_{4} - \left(- I_{yy} u_{6} + I_{yz} u_{5}\right) u_{5} - \left(- I_{yz} u_{6} + I_{zz} u_{5}\right) u_{6} + M_{x}\\- I_{xy} \dot{u}_{4} - I_{yy} \dot{u}_{5} - I_{yz} \dot{u}_{6} - \left(I_{xx} u_{6} - I_{xz} u_{4}\right) u_{4} - \left(I_{xy} u_{6} - I_{yz} u_{4}\right) u_{5} - \left(I_{xz} u_{6} - I_{zz} u_{4}\right) u_{6} + M_{x}\\- I_{xz} \dot{u}_{4} - I_{yz} \dot{u}_{5} - I_{zz} \dot{u}_{6} - \left(- I_{xx} u_{5} + I_{xy} u_{4}\right) u_{4} - \left(- I_{xy} u_{5} + I_{yy} u_{4}\right) u_{5} - \left(- I_{xz} u_{5} + I_{yz} u_{4}\right) u_{6} + M_{z}\end{matrix}\right]\end{split}\]

We can put Kane’s Equations in canonical form (Eq. (189)) by extracting the mass matrix, which is the linear coefficient matrix of \(\dot{\bar{u}}\):

u = sm.Matrix([u1, u2, u3, u4, u5, u6])
t = me.dynamicsymbols._t
ud = u.diff(t)

The mass matrix is:

M = -Frs.jacobian(ud)
M
\[\begin{split}\displaystyle \left[\begin{matrix}m & 0 & 0 & 0 & 0 & 0\\0 & m & 0 & 0 & 0 & 0\\0 & 0 & m & 0 & 0 & 0\\0 & 0 & 0 & I_{xx} & I_{xy} & I_{xz}\\0 & 0 & 0 & I_{xy} & I_{yy} & I_{yz}\\0 & 0 & 0 & I_{xz} & I_{yz} & I_{zz}\end{matrix}\right]\end{split}\]

The velocity forces vector is:

C = -Frs.xreplace({udi: 0 for udi in ud})
C
\[\begin{split}\displaystyle \left[\begin{matrix}m \left(- u_{2} u_{6} + u_{3} u_{5}\right)\\m \left(u_{1} u_{6} - u_{3} u_{4}\right)\\m \left(- u_{1} u_{5} + u_{2} u_{4}\right)\\\left(- I_{xy} u_{6} + I_{xz} u_{5}\right) u_{4} + \left(- I_{yy} u_{6} + I_{yz} u_{5}\right) u_{5} + \left(- I_{yz} u_{6} + I_{zz} u_{5}\right) u_{6}\\\left(I_{xx} u_{6} - I_{xz} u_{4}\right) u_{4} + \left(I_{xy} u_{6} - I_{yz} u_{4}\right) u_{5} + \left(I_{xz} u_{6} - I_{zz} u_{4}\right) u_{6}\\\left(- I_{xx} u_{5} + I_{xy} u_{4}\right) u_{4} + \left(- I_{xy} u_{5} + I_{yy} u_{4}\right) u_{5} + \left(- I_{xz} u_{5} + I_{yz} u_{4}\right) u_{6}\end{matrix}\right]\end{split}\]

And the forcing vector is:

F = Fr
F
\[\begin{split}\displaystyle \left[\begin{matrix}F_{x}\\F_{y}\\F_{z}\\M_{x}\\M_{x}\\M_{z}\end{matrix}\right]\end{split}\]

This example may seem overly complicated when using Kane’s method, but it is a systematic method that works for any number of rigid bodies and particles in a system.

Equations of Motion

The kinematical and dynamical differential equations constitute the equations of motion for a holonomic multibody system. These equations are ordinary differential equations in the generalized speeds and generalized coordinates.

(190)\[\begin{split}\bar{f}_d(\dot{\bar{u}}, \bar{u}, \bar{q}, t) = 0 \\ \bar{f}_k(\dot{\bar{q}}, \bar{u}, \bar{q}, t) = 0\end{split}\]

and since they are both linear in \(\dot{\bar{u}}\) and \(\dot{\bar{q}}\), respectively, they can be written in a combined form:

(191)\[\begin{split}\begin{bmatrix} \mathbf{M}_k && 0 \\ 0 && \mathbf{M}_d \\ \end{bmatrix} \begin{bmatrix} \dot{\bar{q}} \\ \dot{\bar{u}} \end{bmatrix} + \begin{bmatrix} \bar{g}_k(\bar{u}, \bar{q}, t) \\ \bar{g}_d(\bar{u}, \bar{q}, t) \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}\end{split}\]

which we write as:

(192)\[\mathbf{M}_m \dot{\bar{x}} + \bar{g}_m = \bar{0}\]

where \(\bar{x}=[\bar{q} \quad \bar{u}]^T\) is called the state of the system and is comprised of the generalized coordinates and generalized speeds.

Example of Kane’s Equations

Returning to the example from the previous chapter, I will add an additional particle of mass \(m/4\) at point \(Q\) that can slide along the rod \(B\) and is attached to point \(B_o\) via a linear translational spring with stiffness \(k_l\) and located by generalized coordinate \(q_3\). The torsional spring stiffness has been renamed to \(k_t\). See Fig. 47 for a visual description.

_images/eom-double-rod-pendulum.svg

Fig. 47 Three dimensional pendulum made up of two pinned rods and a sliding mass on rod \(B\). Each degree of freedom is resisted by a linear spring. When the generalized coordinates are all zero, the two rods are perpendicular to each other.

The following code is reproduced from the prior chapter and gives the velocities and angular velocities of \(A_o\), \(B_o\), \(A\), and \(B\) in the inertial reference frame \(N\).

m, g, kt, kl, l = sm.symbols('m, g, k_t, k_l, l')
q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')
u1, u2, u3 = me.dynamicsymbols('u1, u2, u3')

N = me.ReferenceFrame('N')
A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')

A.orient_axis(N, q1, N.z)
B.orient_axis(A, q2, A.x)

A.set_ang_vel(N, u1*N.z)
B.set_ang_vel(A, u2*A.x)

O = me.Point('O')
Ao = me.Point('A_O')
Bo = me.Point('B_O')

Ao.set_pos(O, l/2*A.x)
Bo.set_pos(O, l*A.x)

O.set_vel(N, 0)
Ao.v2pt_theory(O, N, A)
Bo.v2pt_theory(O, N, A)

Ao.vel(N), Bo.vel(N), A.ang_vel_in(N), B.ang_vel_in(N)
\[\displaystyle \left( \frac{l u_{1}}{2}\hat{a}_y, \ l u_{1}\hat{a}_y, \ u_{1}\hat{n}_z, \ u_{2}\hat{a}_x + u_{1}\hat{n}_z\right)\]

We now have the particle at \(Q\) so we need its velocity for its contribution to \(F_r\) and \(F_r^*\). \(Q\) is moving in \(B\) so the one point velocity theorem can be used.

Q = me.Point('Q')
Q.set_pos(Bo, q3*B.y)
Q.set_vel(B, u3*B.y)
Q.v1pt_theory(Bo, N, B)

Q.vel(N)
\[\displaystyle - q_{3} u_{1} \cos{\left(q_{2} \right)}\hat{b}_x + u_{3}\hat{b}_y + q_{3} u_{2}\hat{b}_z + l u_{1}\hat{a}_y\]

We will also need the accelerations of the points and frames for the generalized inertia forces. For points \(A_o\), \(B_o\) and frames \(A\) and \(B\) these are nicely expressed in terms of \(\dot{\bar{u}}, \bar{u}, \bar{q}\):

Ao.acc(N), Bo.acc(N), A.ang_acc_in(N), B.ang_acc_in(N)
\[\displaystyle \left( - \frac{l u_{1}^{2}}{2}\hat{a}_x + \frac{l \dot{u}_{1}}{2}\hat{a}_y, \ - l u_{1}^{2}\hat{a}_x + l \dot{u}_{1}\hat{a}_y, \ \dot{u}_{1}\hat{n}_z, \ \dot{u}_{2}\hat{a}_x + u_{1} u_{2}\hat{a}_y + \dot{u}_{1}\hat{n}_z\right)\]

but the acceleration of point \(Q\) contains \(\dot{\bar{q}}\) terms, so we need to eliminate those with the kinematical differential equations:

Q.acc(N)
\[\displaystyle (q_{3} u_{1} u_{2} \sin{\left(q_{2} \right)} + q_{3} u_{1} \sin{\left(q_{2} \right)} \dot{q}_{2} - q_{3} \cos{\left(q_{2} \right)} \dot{u}_{1} - u_{1} u_{3} \cos{\left(q_{2} \right)} - u_{1} \cos{\left(q_{2} \right)} \dot{q}_{3})\hat{b}_x + (- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2} + \dot{u}_{3})\hat{b}_y + (q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + q_{3} \dot{u}_{2} + u_{2} u_{3} + u_{2} \dot{q}_{3})\hat{b}_z - l u_{1}^{2}\hat{a}_x + l \dot{u}_{1}\hat{a}_y\]
t = me.dynamicsymbols._t

qdot_repl = {q1.diff(t): u1,
             q2.diff(t): u2,
             q3.diff(t): u3}

Q.set_acc(N, Q.acc(N).xreplace(qdot_repl))
Q.acc(N)
\[\displaystyle (2 q_{3} u_{1} u_{2} \sin{\left(q_{2} \right)} - q_{3} \cos{\left(q_{2} \right)} \dot{u}_{1} - 2 u_{1} u_{3} \cos{\left(q_{2} \right)})\hat{b}_x + (- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2} + \dot{u}_{3})\hat{b}_y + (q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + q_{3} \dot{u}_{2} + 2 u_{2} u_{3})\hat{b}_z - l u_{1}^{2}\hat{a}_x + l \dot{u}_{1}\hat{a}_y\]

Warning

Be careful when making substitutions when expressions contain derivatives and double derivatives. The order in which you make the substitutions matter and the printer that SymPy is using may not show you what you think you are looking at. Take this expression:

expr = m*q1.diff(t, 2) + kt*q1.diff(t) + kl*q1
expr
\[\displaystyle k_{l} q_{1} + k_{t} \dot{q}_{1} + m \ddot{q}_{1}\]

Let’s say you need to make these substitutions: \(q_1=\frac{q_2}{q_1},\dot{q}_1=u_1,\ddot{q}_1=\dot{u}_1\). It may seem obvious that the \(\ddot{q}_1\) substitution should be done before \(q_1\), but care may be needed to help the computer realize this. If the highest derivatives are substituted first with successive calls to .xreplace() then you get:

expr1 = expr.xreplace({q1.diff(t, 2): u1.diff(t)}).xreplace({q1.diff(t): u1}).xreplace({q1: q2/q1})
expr1
\[\displaystyle \frac{k_{l} q_{2}}{q_{1}} + k_{t} u_{1} + m \dot{u}_{1}\]

But if you substitute in the opposite order you get:

expr2 = expr.xreplace({q1: q2/q1}).xreplace({q1.diff(t): u1}).xreplace({q1.diff(t, 2): u1.diff(t)})
expr2
\[\displaystyle \frac{k_{l} q_{2}}{q_{1}} + k_{t} \left(\frac{\dot{q}_{2}}{q_{1}} - \frac{q_{2} \dot{q}_{1}}{q_{1}^{2}}\right) + m \left(\frac{- \frac{\left(\ddot{q}_{1} - \frac{2 \dot{q}_{1}^{2}}{q_{1}}\right) q_{2}}{q_{1}} + \ddot{q}_{2} - \frac{2 \dot{q}_{1} \dot{q}_{2}}{q_{1}}}{q_{1}}\right)\]

which is a very different answer.

Checking the str() or srepr() versions of the expressions can help diagnose what is going on. The string representation of the first expression is as expected:

print(expr1)

k_l*q2(t)/q1(t) + k_t*u1(t) + m*Derivative(u1(t), t)

The string representation of the second expression shows that the \(q_1\) symbol was substituted into each derivative term.

print(expr2)

k_l*q2(t)/q1(t) + k_t*Derivative(q2(t)/q1(t), t) + m*Derivative(q2(t)/q1(t), (t, 2))

expr2 shows different results depending on how you print it! The typeset math evaluates the derivatives and the string representation does not.

If you put all of the substitutions in the same dictionary, SymPy should substitute the terms in the expected order:

expr.xreplace({q1: q2/q1, q1.diff(t): u1, q1.diff(t, 2): u1.diff(t)})
\[\displaystyle \frac{k_{l} q_{2}}{q_{1}} + k_{t} u_{1} + m \dot{u}_{1}\]
expr.xreplace({q1.diff(t, 2): u1.diff(t), q1.diff(t): u1, q1: q2/q1})
\[\displaystyle \frac{k_{l} q_{2}}{q_{1}} + k_{t} u_{1} + m \dot{u}_{1}\]

Now we formulate the resultant forces and torques on each relevant point and frame:

R_Ao = m*g*N.x
R_Bo = m*g*N.x + kl*q3*B.y
R_Q = m/4*g*N.x - kl*q3*B.y
T_A = -kt*q1*N.z + kt*q2*A.x
T_B = -kt*q2*A.x

Note the equal and opposite spring forces that act on the pairs of points and pairs of reference frames. We ignored the reaction torque on \(N\) from \(A\) because \(N\) is our inertial reference frame.

The inertia dyadics of the two rods are:

I = m*l**2/12
I_A_Ao = I*me.outer(A.y, A.y) + I*me.outer(A.z, A.z)
I_B_Bo = I*me.outer(B.x, B.x) + I*me.outer(B.z, B.z)

To form the equations of motion, start by finding all of the partial velocities of the two mass centers \(A_o,B_o\), one particle \(Q\), and two bodies \(A,B\):

v_Ao_1 = Ao.vel(N).diff(u1, N)
v_Bo_1 = Bo.vel(N).diff(u1, N)
v_Q_1 = Q.vel(N).diff(u1, N)

v_Ao_2 = Ao.vel(N).diff(u2, N)
v_Bo_2 = Bo.vel(N).diff(u2, N)
v_Q_2 = Q.vel(N).diff(u2, N)

v_Ao_3 = Ao.vel(N).diff(u3, N)
v_Bo_3 = Bo.vel(N).diff(u3, N)
v_Q_3 = Q.vel(N).diff(u3, N)

w_A_1 = A.ang_vel_in(N).diff(u1, N)
w_B_1 = B.ang_vel_in(N).diff(u1, N)

w_A_2 = A.ang_vel_in(N).diff(u2, N)
w_B_2 = B.ang_vel_in(N).diff(u2, N)

w_A_3 = A.ang_vel_in(N).diff(u3, N)
w_B_3 = B.ang_vel_in(N).diff(u3, N)

The three generalized active forces are then formed by dotting the partial velocities with the associated load:

F1 = v_Ao_1.dot(R_Ao) + v_Bo_1.dot(R_Bo) + v_Q_1.dot(R_Q) + w_A_1.dot(T_A) + w_B_1.dot(T_B)
F2 = v_Ao_2.dot(R_Ao) + v_Bo_2.dot(R_Bo) + v_Q_2.dot(R_Q) + w_A_2.dot(T_A) + w_B_2.dot(T_B)
F3 = v_Ao_3.dot(R_Ao) + v_Bo_3.dot(R_Bo) + v_Q_3.dot(R_Q) + w_A_3.dot(T_A) + w_B_3.dot(T_B)

The generalized force vector \(\bar{F}_r\) is then:

Fr = sm.Matrix([F1, F2, F3])
Fr
\[\begin{split}\displaystyle \left[\begin{matrix}- \frac{7 g l m \sin{\left(q_{1} \right)}}{4} - \frac{g m q_{3} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{t} q_{1}\\\frac{g m q_{3} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)}}{4} - k_{t} q_{2}\\- \frac{g m \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{l} q_{3}\end{matrix}\right]\end{split}\]

The three generalized inertia forces are similarly formed but with the resultant inertial forces:

TAs = -(A.ang_acc_in(N).dot(I_A_Ao) + me.cross(A.ang_vel_in(N), I_A_Ao).dot(A.ang_vel_in(N)))
TBs = -(B.ang_acc_in(N).dot(I_B_Bo) + me.cross(B.ang_vel_in(N), I_B_Bo).dot(B.ang_vel_in(N)))

F1s = v_Ao_1.dot(-m*Ao.acc(N)) + v_Bo_1.dot(-m*Bo.acc(N)) + v_Q_1.dot(-m/4*Q.acc(N))
F1s += w_A_1.dot(TAs) + w_B_1.dot(TBs)

F2s = v_Ao_2.dot(-m*Ao.acc(N)) + v_Bo_2.dot(-m*Bo.acc(N)) + v_Q_2.dot(-m/4*Q.acc(N))
F2s += w_A_2.dot(TAs) + w_B_2.dot(TBs)

F3s = v_Ao_3.dot(-m*Ao.acc(N)) + v_Bo_3.dot(-m*Bo.acc(N)) + v_Q_3.dot(-m/4*Q.acc(N))
F3s += w_A_3.dot(TAs) + w_B_3.dot(TBs)

Finally the generalized inertia force vector is:

Frs = sm.Matrix([F1s, F2s, F3s])
Frs
\[\begin{split}\displaystyle \left[\begin{matrix}- \frac{19 l^{2} m \dot{u}_{1}}{12} - \frac{l m q_{3} u_{1}^{2} \cos{\left(q_{2} \right)}}{4} + l \left(- \frac{m \left(- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2} + \dot{u}_{3}\right) \cos{\left(q_{2} \right)}}{4} + \frac{m \left(q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + q_{3} \dot{u}_{2} + 2 u_{2} u_{3}\right) \sin{\left(q_{2} \right)}}{4}\right) + \frac{m \left(2 q_{3} u_{1} u_{2} \sin{\left(q_{2} \right)} - q_{3} \cos{\left(q_{2} \right)} \dot{u}_{1} - 2 u_{1} u_{3} \cos{\left(q_{2} \right)}\right) q_{3} \cos{\left(q_{2} \right)}}{4} + \left(- \frac{l^{2} m \left(- u_{1} u_{2} \sin{\left(q_{2} \right)} + \cos{\left(q_{2} \right)} \dot{u}_{1}\right)}{12} + \frac{l^{2} m u_{1} u_{2} \sin{\left(q_{2} \right)}}{12}\right) \cos{\left(q_{2} \right)}\\- \frac{l^{2} m u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)}}{12} - \frac{l^{2} m \dot{u}_{2}}{12} + \frac{l m q_{3} \sin{\left(q_{2} \right)} \dot{u}_{1}}{4} - \frac{m \left(q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + q_{3} \dot{u}_{2} + 2 u_{2} u_{3}\right) q_{3}}{4}\\- \frac{l m \cos{\left(q_{2} \right)} \dot{u}_{1}}{4} - \frac{m \left(- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2} + \dot{u}_{3}\right)}{4}\end{matrix}\right]\end{split}\]

Notice that the dynamical differential equations are only functions of the time varying variables \(\dot{\bar{u}},\bar{u},\bar{q}\):

me.find_dynamicsymbols(Fr)
\[\displaystyle \left\{q_{1}, q_{2}, q_{3}\right\}\]
me.find_dynamicsymbols(Frs)
\[\displaystyle \left\{q_{2}, q_{3}, u_{1}, u_{2}, u_{3}, \dot{u}_{1}, \dot{u}_{2}, \dot{u}_{3}\right\}\]

Implicit and Explicit Form

Eq. (192) is written in an implicit form, meaning that the derivatives are not explicitly solved for. The explicit form is found by inverting \(\mathbf{M}_m\):

(193)\[\dot{\bar{x}} = -\mathbf{M}_m^{-1} \bar{g}_m =\bar{f}_m(\bar{x}, t)\]

To determine how the state changes over time, these explicit differential equations can be solved by integrating them with respect to time:

(194)\[\bar{x}(t) = \int^{t_f}_{t_0} \bar{f}_m(\bar{x}, t) dt\]

\(\bar{f}_m\) is, in general, nonlinear in time, thus analytical solutions are impossible to find. To solve this integral we must numerically integrate \(\bar{f}_m\). To do so, it will be useful to extract the symbolic forms of \(\mathbf{M}_k\), \(\bar{g}_k\), \(\mathbf{M}_d\), and \(\bar{g}_d\).

Our example problem has a simple definition of the kinematical differential equations:

(195)\[\begin{split}\begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{bmatrix} = \begin{bmatrix} u_1 \\ u_2 \\ u_3 \end{bmatrix}\end{split}\]

so \(\mathbf{M}_k\) is the identity matrix and need not be formed:

(196)\[\begin{split}\mathbf{M}_k \dot{\bar{q}} + \bar{g}_k = 0 \rightarrow - \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{bmatrix} + \begin{bmatrix} u_1 \\ u_2 \\ u_3 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}\end{split}\]

But we will need \(\mathbf{M}_d\) to solve explicitly for \(\dot{\bar{u}}\). Recall that we can use the Jacobian to extract the linear coefficients of \(\dot{\bar{u}}\) and then find the terms that aren’t functions of \(\dot{\bar{u}}\) by substitution (See Sec. Solving Linear Systems).

Form the column vector \(\dot{\bar{u}}\):

u = sm.Matrix([u1, u2, u3])
ud = u.diff(t)
ud
\[\begin{split}\displaystyle \left[\begin{matrix}\dot{u}_{1}\\\dot{u}_{2}\\\dot{u}_{3}\end{matrix}\right]\end{split}\]

Extract the coefficients of \(\dot{\bar{u}}\):

Md = Frs.jacobian(ud)
Md
\[\begin{split}\displaystyle \left[\begin{matrix}- \frac{l^{2} m \cos^{2}{\left(q_{2} \right)}}{12} - \frac{19 l^{2} m}{12} - \frac{m q_{3}^{2} \cos^{2}{\left(q_{2} \right)}}{4} & \frac{l m q_{3} \sin{\left(q_{2} \right)}}{4} & - \frac{l m \cos{\left(q_{2} \right)}}{4}\\\frac{l m q_{3} \sin{\left(q_{2} \right)}}{4} & - \frac{l^{2} m}{12} - \frac{m q_{3}^{2}}{4} & 0\\- \frac{l m \cos{\left(q_{2} \right)}}{4} & 0 & - \frac{m}{4}\end{matrix}\right]\end{split}\]

Make a substitution dictionary to set \(\dot{\bar{u}}=\bar{0}\):

ud_zerod = {udr: 0 for udr in ud}
ud_zerod
\[\displaystyle \left\{ \dot{u}_{1} : 0, \ \dot{u}_{2} : 0, \ \dot{u}_{3} : 0\right\}\]

Find \(\bar{g}_d\) with \(\bar{g}_d = \bar{F}_r^* |_{\dot{\bar{u}}=\bar{0}} + \bar{F}_r\):

gd = Frs.xreplace(ud_zerod) + Fr
gd
\[\begin{split}\displaystyle \left[\begin{matrix}- \frac{7 g l m \sin{\left(q_{1} \right)}}{4} - \frac{g m q_{3} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{t} q_{1} + \frac{l^{2} m u_{1} u_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)}}{6} - \frac{l m q_{3} u_{1}^{2} \cos{\left(q_{2} \right)}}{4} + l \left(- \frac{m \left(- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2}\right) \cos{\left(q_{2} \right)}}{4} + \frac{m \left(q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + 2 u_{2} u_{3}\right) \sin{\left(q_{2} \right)}}{4}\right) + \frac{m \left(2 q_{3} u_{1} u_{2} \sin{\left(q_{2} \right)} - 2 u_{1} u_{3} \cos{\left(q_{2} \right)}\right) q_{3} \cos{\left(q_{2} \right)}}{4}\\\frac{g m q_{3} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)}}{4} - k_{t} q_{2} - \frac{l^{2} m u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)}}{12} - \frac{m \left(q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + 2 u_{2} u_{3}\right) q_{3}}{4}\\- \frac{g m \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{l} q_{3} - \frac{m \left(- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2}\right)}{4}\end{matrix}\right]\end{split}\]

Check that neither are functions of \(\dot{\bar{u}}\):

me.find_dynamicsymbols(Md)
\[\displaystyle \left\{q_{2}, q_{3}\right\}\]
me.find_dynamicsymbols(gd)
\[\displaystyle \left\{q_{1}, q_{2}, q_{3}, u_{1}, u_{2}, u_{3}\right\}\]