Rocket_Car.rst

##

\mathbf{\text{Modeling of a Variable-Mass Nonholonomic Gyrostatic Rocket Car Using Extended Kane's Equations}}

---

\text{Ge, Z.M. and Cheng, Y.H., 1982. Extended Kane’s equations for nonholonomic variable mass system.}

---

\text{Figure 1: Idealized model of a jet racing car that is propelled by a rocket engine at point P, the rocket engine being treated as a variable mass particle at P}

---

\text{Here}, \{a_x: g_3, a_y: g_1, a_z: g_2\}

---

\text{Leimanis, E., 2013. The general problem of the motion of coupled rigid bodies about a fixed point (Vol. 7). Springer Science & Business Media. (Page 207)}

import sympy as sm
import sympy.physics.mechanics as me
from pydy.system import System
import numpy as np
from sympy.simplify.fu import TR2
import matplotlib.pyplot as plt
from scipy.integrate import odeint
me.init_vprinting()

N = me.ReferenceFrame('N')

q1, q2, q3, q4, q5, q6, q7, q8 = me.dynamicsymbols('q1:9')

A2 = N.orientnew('A_2', 'Axis', [q3, N.z])
A1 = A2.orientnew('A_1', 'Axis', [q4, A2.z])

B1 = A1.orientnew('B_1', 'Axis', [q5, A1.y])
B2 = A1.orientnew('B_2', 'Axis', [q6, A1.y])
B3 = A2.orientnew('B_3', 'Axis', [q7, A2.y])
B4 = A2.orientnew('B_4', 'Axis', [q8, A2.y])

t = me.dynamicsymbols._t

O = me.Point('O') # fixed point in the inertial reference frame
O.set_vel(N, 0)

L, l , a, b, r1, r2 = sm.symbols('L, l , a, b, r_1, r_2')

Q = O.locatenew('Q', q1 * N.x + q2 * N.y)

P = Q.locatenew('P', L * -A2.x)

C = P.locatenew('C', l * A2.x)

Q.set_vel(N, Q.pos_from(O).dt(N))
Q.vel(N)

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y}

P.v2pt_theory(Q, N, A2)
P.vel(N)

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} -  L \dot{q}_{3}\mathbf{\hat{a_2}_y}

C.v2pt_theory(P, N, A2)
# C.vel(N)

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} + (- L \dot{q}_{3} + l \dot{q}_{3})\mathbf{\hat{a_2}_y}

A1.ang_vel_in(A2).express(A1)

\displaystyle \dot{q}_{4}\mathbf{\hat{a_1}_z}

u1, u2 = me.dynamicsymbols('u_1:3')

z1 = sm.Eq(u1, A1.ang_vel_in(A2).dot(A1.z))
z2 = sm.Eq(u2, Q.vel(N).dot(A1.x))

u = sm.trigsimp(sm.solve([z1, z2], A1.ang_vel_in(A2).dot(A1.z), Q.vel(N).dot(A1.x)))
u

\displaystyle \left\{ \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{1} : u_{2}, \  \dot{q}_{4} : u_{1}\right\}

\text{Nonholonomic Constraints:}\ B_1

B1_center = Q.locatenew('B_1_center', a * A1.y)
B1_center.pos_from(Q)

\displaystyle a\mathbf{\hat{a_1}_y}

B1_center.v2pt_theory(Q, N, A1)
B1_center.vel(N).express(A1).simplify()

\displaystyle (- a \left(\dot{q}_{3} + \dot{q}_{4}\right) + \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{1})\mathbf{\hat{a_1}_x} + (- \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{2})\mathbf{\hat{a_1}_y}

B1_ground = B1_center.locatenew('B_1_ground', r1 * -A1.z)
B1_ground.pos_from(B1_center)

\displaystyle -  r_{1}\mathbf{\hat{a_1}_z}

B1_ground.v2pt_theory(B1_center, N, B1)
B1_ground.vel(N).simplify()

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} + (- a \left(\dot{q}_{3} + \dot{q}_{4}\right) - r_{1} \dot{q}_{5})\mathbf{\hat{a_1}_x}

B1_cons = [me.dot(B1_ground.vel(N).simplify(), uv) for uv in A1]
sm.trigsimp(B1_cons)

\displaystyle \left[ - a \left(\dot{q}_{3} + \dot{q}_{4}\right) - r_{1} \dot{q}_{5} + \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right)\right) \dot{q}_{1} + \left(\operatorname{sin}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right) + \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{3}\right)\right) \dot{q}_{2}, \  \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right)\right) \dot{q}_{2} + \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right) - \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{3}\right)\right) \dot{q}_{1}, \  0\right]

eq1 = sm.Eq(B1_cons[0].simplify().subs(u), 0)
eq1

\displaystyle - a \left(u_{1} + \dot{q}_{3}\right) - r_{1} \dot{q}_{5} + u_{2} = 0

eq2 = sm.Eq(B1_cons[1].simplify().subs(u), 0)
eq2

\displaystyle - \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{2} = 0

\text{Nonholonomic Constraints:}\ B_2

B2_center = Q.locatenew('B_1_center', a * -A1.y)
B2_center.pos_from(Q)

\displaystyle -  a\mathbf{\hat{a_1}_y}

B2_center.v2pt_theory(Q, N, A1)
B2_center.vel(N).express(A1).simplify()

\displaystyle (a \left(\dot{q}_{3} + \dot{q}_{4}\right) + \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{1})\mathbf{\hat{a_1}_x} + (- \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{2})\mathbf{\hat{a_1}_y}

B2_ground = B2_center.locatenew('B_2_ground', r1 * -A1.z)
B2_ground.pos_from(B2_center)

\displaystyle -  r_{1}\mathbf{\hat{a_1}_z}

B2_ground.v2pt_theory(B2_center, N, B2)
B2_ground.vel(N).simplify()

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} + (a \left(\dot{q}_{3} + \dot{q}_{4}\right) - r_{1} \dot{q}_{6})\mathbf{\hat{a_1}_x}

B2_cons = [me.dot(B2_ground.vel(N).simplify(), uv) for uv in A1]
sm.trigsimp(B2_cons)

\displaystyle \left[ a \left(\dot{q}_{3} + \dot{q}_{4}\right) - r_{1} \dot{q}_{6} + \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right)\right) \dot{q}_{1} + \left(\operatorname{sin}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right) + \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{3}\right)\right) \dot{q}_{2}, \  \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right)\right) \dot{q}_{2} + \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right) - \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{3}\right)\right) \dot{q}_{1}, \  0\right]

eq3 = sm.Eq(B2_cons[0].simplify().subs(u), 0)
eq3

\displaystyle a \left(u_{1} + \dot{q}_{3}\right) - r_{1} \dot{q}_{6} + u_{2} = 0

eq4 = sm.Eq(B2_cons[1].simplify().subs(u), 0)
eq4

\displaystyle - \operatorname{sin}\left(q_{3} + q_{4}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3} + q_{4}\right) \dot{q}_{2} = 0

\text{Nonholonomic Constraints:}\ B_3

B3_center = P.locatenew('B_3_center', b * A2.y)
B3_center.pos_from(P)

\displaystyle b\mathbf{\hat{a_2}_y}

B3_center.v2pt_theory(P, N, A2)
B3_center.vel(N).express(A2).simplify()

\displaystyle (- b \dot{q}_{3} + \operatorname{sin}\left(q_{3}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{1})\mathbf{\hat{a_2}_x} + (- L \dot{q}_{3} - \operatorname{sin}\left(q_{3}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{2})\mathbf{\hat{a_2}_y}

B3_ground = B3_center.locatenew('B_3_ground', r2 * -A2.z)
B3_ground.pos_from(B3_center)

\displaystyle -  r_{2}\mathbf{\hat{a_2}_z}

B3_ground.v2pt_theory(B3_center, N, B3)
B3_ground.vel(N).simplify()

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} + (- b \dot{q}_{3} - r_{2} \dot{q}_{7})\mathbf{\hat{a_2}_x} -  L \dot{q}_{3}\mathbf{\hat{a_2}_y}

B3_cons = [me.dot(B3_ground.vel(N).simplify(), uv) for uv in A2]
sm.trigsimp(B3_cons)

\displaystyle \left[ - b \dot{q}_{3} - r_{2} \dot{q}_{7} + \operatorname{sin}\left(q_{3}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{1}, \  - L \dot{q}_{3} - \operatorname{sin}\left(q_{3}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{2}, \  0\right]

eq5 = sm.Eq(B3_cons[0].simplify().subs(u), 0)
eq5

\displaystyle - b \dot{q}_{3} - r_{2} \dot{q}_{7} + \operatorname{sin}\left(q_{3}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{1} = 0

eq6 = sm.Eq(B3_cons[1].simplify().subs(u), 0)
eq6

\displaystyle - L \dot{q}_{3} - \operatorname{sin}\left(q_{3}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{2} = 0

\text{Nonholonomic Constraints:}\ B_4

B4_center = P.locatenew('B_4_center', b * -A2.y)
B4_center.pos_from(P)

\displaystyle -  b\mathbf{\hat{a_2}_y}

B4_center.v2pt_theory(P, N, A2)
# B4_center.vel(N).express(A2).simplify() # Invalid Json

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} + b \dot{q}_{3}\mathbf{\hat{a_2}_x} -  L \dot{q}_{3}\mathbf{\hat{a_2}_y}

B4_ground = B4_center.locatenew('B_4_ground', r2 * -A2.z)
B4_ground.pos_from(B4_center)

\displaystyle -  r_{2}\mathbf{\hat{a_2}_z}

B4_ground.v2pt_theory(B4_center, N, B4)
B4_ground.vel(N).simplify()

\displaystyle \dot{q}_{1}\mathbf{\hat{n}_x} + \dot{q}_{2}\mathbf{\hat{n}_y} + (b \dot{q}_{3} - r_{2} \dot{q}_{8})\mathbf{\hat{a_2}_x} -  L \dot{q}_{3}\mathbf{\hat{a_2}_y}

B4_cons = [me.dot(B4_ground.vel(N).simplify(), uv) for uv in A2]
sm.trigsimp(B4_cons)

\displaystyle \left[ b \dot{q}_{3} - r_{2} \dot{q}_{8} + \operatorname{sin}\left(q_{3}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{1}, \  - L \dot{q}_{3} - \operatorname{sin}\left(q_{3}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{2}, \  0\right]

eq7 = sm.Eq(B4_cons[0].simplify().subs(u), 0)
eq7

\displaystyle b \dot{q}_{3} - r_{2} \dot{q}_{8} + \operatorname{sin}\left(q_{3}\right) \dot{q}_{2} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{1} = 0

eq8 = sm.Eq(B4_cons[1].simplify().subs(u), 0)
eq8

\displaystyle - L \dot{q}_{3} - \operatorname{sin}\left(q_{3}\right) \dot{q}_{1} + \operatorname{cos}\left(q_{3}\right) \dot{q}_{2} = 0

\text{LHS} \Longleftrightarrow \text{RHS}\ \text{in}\ z_1, z_2 \rightarrow \text{Equations}\ 9, 10

eq9 = sm.Eq(A1.ang_vel_in(A2).dot(A1.z), u1)
eq9

\displaystyle \dot{q}_{4} = u_{1}

eq10 = sm.Eq(Q.vel(N).dot(A1.x), u2)
eq10

\displaystyle \left(- \operatorname{sin}\left(q_{3}\right) \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right)\right) \dot{q}_{1} + \left(\operatorname{sin}\left(q_{3}\right) \operatorname{cos}\left(q_{4}\right) + \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{3}\right)\right) \dot{q}_{2} = u_{2}

\text{Solution of the System of Linear (in}\ \dot{q}_1,...\text{) Equations}

solution = sm.solve([eq1, eq2, eq3, eq4, eq5, eq6, eq7, eq8, eq9, eq10], q1.diff(), q2.diff(), q3.diff(),  q4.diff(), q5.diff(), q6.diff(), q7.diff(), q8.diff())

solution

\displaystyle \left\{ \dot{q}_{1} : u_{2} \operatorname{cos}\left(q_{3} + q_{4}\right), \  \dot{q}_{2} : u_{2} \operatorname{sin}\left(q_{3} + q_{4}\right), \  \dot{q}_{3} : \frac{\left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2} \operatorname{tan}\left(q_{4}\right)}{L \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  \dot{q}_{4} : u_{1}, \  \dot{q}_{5} : \frac{- L \left(L + b \operatorname{tan}\left(q_{4}\right)\right) \left(a u_{1} + u_{2}\right) + 2 L \left(L + b \operatorname{tan}\left(q_{4}\right)\right) u_{2} - a \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2} \operatorname{tan}\left(q_{4}\right)}{L r_{1} \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  \dot{q}_{6} : \frac{L \left(L + b \operatorname{tan}\left(q_{4}\right)\right) \left(a u_{1} + u_{2}\right) + a \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2} \operatorname{tan}\left(q_{4}\right)}{L r_{1} \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  \dot{q}_{7} : \frac{\left(L - b \operatorname{tan}\left(q_{4}\right)\right) \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2} \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  \dot{q}_{8} : \frac{\left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2}}\right\}

sollist_keys = list(solution.keys())
sollist_keys

\displaystyle \left[ \dot{q}_{8}, \  \dot{q}_{4}, \  \dot{q}_{7}, \  \dot{q}_{6}, \  \dot{q}_{1}, \  \dot{q}_{5}, \  \dot{q}_{2}, \  \dot{q}_{3}\right]

sollist_values = list(solution.values())
sollist_values

\displaystyle \left[ \frac{\left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2}}, \  u_{1}, \  \frac{\left(L - b \operatorname{tan}\left(q_{4}\right)\right) \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2} \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  \frac{L \left(L + b \operatorname{tan}\left(q_{4}\right)\right) \left(a u_{1} + u_{2}\right) + a \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2} \operatorname{tan}\left(q_{4}\right)}{L r_{1} \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  u_{2} \operatorname{cos}\left(q_{3} + q_{4}\right), \  \frac{- L \left(L + b \operatorname{tan}\left(q_{4}\right)\right) \left(a u_{1} + u_{2}\right) + 2 L \left(L + b \operatorname{tan}\left(q_{4}\right)\right) u_{2} - a \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2} \operatorname{tan}\left(q_{4}\right)}{L r_{1} \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}, \  u_{2} \operatorname{sin}\left(q_{3} + q_{4}\right), \  \frac{\left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2} \operatorname{tan}\left(q_{4}\right)}{L \left(L + b \operatorname{tan}\left(q_{4}\right)\right)}\right]

sollist_values_simple = []
for i in range(8):
   sollist_values_simple.append(sm.factor(TR2(sollist_values[i]).simplify()))

sollist_values_simple

\displaystyle \left[ \frac{\left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2}}, \  u_{1}, \  - \frac{\left(- L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2}}, \  \frac{L a u_{1} + L u_{2} + a u_{2} \operatorname{sin}\left(q_{4}\right)}{L r_{1}}, \  u_{2} \operatorname{cos}\left(q_{3} + q_{4}\right), \  - \frac{L a u_{1} - L u_{2} + a u_{2} \operatorname{sin}\left(q_{4}\right)}{L r_{1}}, \  u_{2} \operatorname{sin}\left(q_{3} + q_{4}\right), \  \frac{u_{2} \operatorname{sin}\left(q_{4}\right)}{L}\right]

soldict = dict(zip(sollist_keys, sollist_values_simple))
soldict

\displaystyle \left\{ \dot{q}_{1} : u_{2} \operatorname{cos}\left(q_{3} + q_{4}\right), \  \dot{q}_{2} : u_{2} \operatorname{sin}\left(q_{3} + q_{4}\right), \  \dot{q}_{3} : \frac{u_{2} \operatorname{sin}\left(q_{4}\right)}{L}, \  \dot{q}_{4} : u_{1}, \  \dot{q}_{5} : - \frac{L a u_{1} - L u_{2} + a u_{2} \operatorname{sin}\left(q_{4}\right)}{L r_{1}}, \  \dot{q}_{6} : \frac{L a u_{1} + L u_{2} + a u_{2} \operatorname{sin}\left(q_{4}\right)}{L r_{1}}, \  \dot{q}_{7} : - \frac{\left(- L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2}}, \  \dot{q}_{8} : \frac{\left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) u_{2}}{L r_{2}}\right\}

\text{Reformulated Velocity and Angular Velocity Expressions}

N_v_Q = Q.vel(N).subs(soldict).express(A1).simplify()
N_v_Q

\displaystyle u_{2}\mathbf{\hat{a_1}_x}

N_v_P = P.vel(N).subs(soldict).express(A2).simplify()
N_v_P

\displaystyle u_{2} \operatorname{cos}\left(q_{4}\right)\mathbf{\hat{a_2}_x}

N_v_C = C.vel(N).subs(soldict).express(A2).simplify()
N_v_C

\displaystyle u_{2} \operatorname{cos}\left(q_{4}\right)\mathbf{\hat{a_2}_x} + \frac{l u_{2} \operatorname{sin}\left(q_{4}\right)}{L}\mathbf{\hat{a_2}_y}

N_w_A1 = A1.ang_vel_in(N).subs(soldict).express(A1).simplify()
N_w_A1

\displaystyle (u_{1} + \frac{u_{2} \operatorname{sin}\left(q_{4}\right)}{L})\mathbf{\hat{a_1}_z}

N_w_A2 = A2.ang_vel_in(N).subs(soldict).express(A2).simplify()
N_w_A2

\displaystyle \frac{u_{2} \operatorname{sin}\left(q_{4}\right)}{L}\mathbf{\hat{a_2}_z}

\text{Partial Velocities and Partial Angular Velocities}

V_1_Q = N_v_Q.diff(u1, N)
V_1_Q

\displaystyle 0

V_2_Q = N_v_Q.diff(u2, N)
V_2_Q

\displaystyle \mathbf{\hat{a_1}_x}

V_1_C = N_v_C.diff(u1, N)
V_1_C

\displaystyle 0

V_2_C = N_v_C.diff(u2, N)
V_2_C

\displaystyle \operatorname{cos}\left(q_{4}\right)\mathbf{\hat{a_2}_x} + \frac{l \operatorname{sin}\left(q_{4}\right)}{L}\mathbf{\hat{a_2}_y}

V_1_P = N_v_P.diff(u1, N)
V_1_P

\displaystyle 0

V_2_P = N_v_P.diff(u2, N)
V_2_P

\displaystyle \operatorname{cos}\left(q_{4}\right)\mathbf{\hat{a_2}_x}

w_1_A1 = N_w_A1.diff(u1, N)
w_1_A1

\displaystyle \mathbf{\hat{a_1}_z}

w_2_A1 = N_w_A1.diff(u2, N)
w_2_A1

\displaystyle \frac{\operatorname{sin}\left(q_{4}\right)}{L}\mathbf{\hat{a_1}_z}

w_1_A2 = N_w_A2.diff(u1, N)
w_1_A2

\displaystyle 0

w_2_A2 = N_w_A2.diff(u2, N)
w_2_A2

\displaystyle \frac{\operatorname{sin}\left(q_{4}\right)}{L}\mathbf{\hat{a_2}_z}

\text{Accelerations and Angular Accelerations}

a_1__P, a_2__P, a_3__P, a_1__C, a_2__C, a_3__C, a__Q, alpha__A1, alpha__A2 = sm.symbols('a_1__P, a_2__P, a_3__P, a_1__C, a_2__C, a_3__C, a__Q, alpha__A1, alpha__A2')

N_a_P = N_v_P.dt(N).subs(soldict)
N_a_P

\displaystyle (- u_{1} u_{2} \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{4}\right) \dot{u}_{2})\mathbf{\hat{a_2}_x} + \frac{u^{2}_{2} \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{4}\right)}{L}\mathbf{\hat{a_2}_y}

N_a_C = N_v_C.dt(N).subs(soldict)
N_a_C

\displaystyle (- u_{1} u_{2} \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{4}\right) \dot{u}_{2} - \frac{l u^{2}_{2} \operatorname{sin}^{2}\left(q_{4}\right)}{L^{2}})\mathbf{\hat{a_2}_x} + (\frac{l u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{l \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L} + \frac{u^{2}_{2} \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{4}\right)}{L})\mathbf{\hat{a_2}_y}

N_a_Q = N_v_Q.dt(N).subs(soldict)
N_a_Q

\displaystyle \dot{u}_{2}\mathbf{\hat{a_1}_x} + \left(u_{1} + \frac{u_{2} \operatorname{sin}\left(q_{4}\right)}{L}\right) u_{2}\mathbf{\hat{a_1}_y}

N_aa_A1 = N_w_A1.dt(N).subs(soldict)
N_aa_A1

\displaystyle (\dot{u}_{1} + \frac{u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{\operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L})\mathbf{\hat{a_1}_z}

N_aa_A2 = N_w_A2.dt(N).subs(soldict)
N_aa_A2

\displaystyle (\frac{u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{\operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L})\mathbf{\hat{a_2}_z}

\text{Forces}

(F_r^*)_G = (F_r^*)_{GR} + (F_r^*)_{GI}

where,

(F_r^*)_{GR} = {V_r}^G \cdot {F_G}^* + \omega_r^A \cdot {T_G}^*

F_G^* = -m_G {a^G}^*

T_G^* \overset{\Delta}{=} -[\alpha_A \cdot I_G + \omega_r^A \times (I_G \cdot \omega_r^A)]

({F_r}^*)_{GI} = -J\{\omega_r^A [\ddot{q_k} g_1 + \dot{q_k} (\omega_3^A g_2 - \omega_2^A g_3)] + C_{kr} (\dot{\omega_1^A} + \ddot{q_k}) \}

---

\text{Kane, T.R., 1978. Nonholonomic multibody systems containing gyrostats. In Dynamics of Multibody Systems (pp. 97-107). Springer, Berlin, Heidelberg.}

\text{Naming Convention:}

({F_r}^*)_{G_n R}\ \text{(rigid)}

({F_r}^*)_{G_n I}\ \text{(internal)}

\text{Masses and Moments of Inertia}

M1, M2 = sm.symbols('M_1, M_2')
m = me.dynamicsymbols('m')

I1x, I1y, I1z = sm.symbols('I_{1_x}, I_{1_y}, I_{1_z}')
I2x, I2y, I2z = sm.symbols('I_{2_x}, I_{2_y}, I_{2_z}')
J1, J2 = sm.symbols('J_1, J_2')

I1 = me.inertia(A1, I1x, I1y, I1z)
I1

\displaystyle I_{1_x}\mathbf{\hat{a_1}_x}\otimes \mathbf{\hat{a_1}_x} + I_{1_y}\mathbf{\hat{a_1}_y}\otimes \mathbf{\hat{a_1}_y} + I_{1_z}\mathbf{\hat{a_1}_z}\otimes \mathbf{\hat{a_1}_z}

I2 = me.inertia(A2, I2x, I2y, I2z)
I2

\displaystyle I_{2_x}\mathbf{\hat{a_2}_x}\otimes \mathbf{\hat{a_2}_x} + I_{2_y}\mathbf{\hat{a_2}_y}\otimes \mathbf{\hat{a_2}_y} + I_{2_z}\mathbf{\hat{a_2}_z}\otimes \mathbf{\hat{a_2}_z}

\text{Gyrostat}\ G_1

\rightarrow {F_G}^* = -m_G {a^G}^*

Fstar_G1 = -M1 * N_a_Q
Fstar_G1

\displaystyle -  M_{1} \dot{u}_{2}\mathbf{\hat{a_1}_x} -  M_{1} \left(u_{1} + \frac{u_{2} \operatorname{sin}\left(q_{4}\right)}{L}\right) u_{2}\mathbf{\hat{a_1}_y}

\rightarrow {T_G}^* \overset{\Delta}{=} -[\alpha_A \cdot I_G + {\omega_r}^A \times (I_G \cdot {\omega_r}^A)]

Tstar_G1 = -(N_aa_A1.dot(I1) + me.cross(N_w_A1, I1.dot(N_w_A1)))
Tstar_G1

\displaystyle -  I_{1_z} \left(\dot{u}_{1} + \frac{u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{\operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}\right)\mathbf{\hat{a_1}_z}

\rightarrow ({F_r}^*)_{GR} = {V_r}^G \cdot {F_G}^* + {\omega_r}^A \cdot {T_G}^*

Fstar_1_G1_R = V_1_Q.dot(Fstar_G1) + w_1_A1.dot(Tstar_G1).subs(soldict)
Fstar_1_G1_R.subs({N_w_A1.dt(N).subs(soldict).dot(A1.z): alpha__A1})

\displaystyle - I_{1_z} \alpha^{A1}

Fstar_2_G1_R = V_2_Q.dot(Fstar_G1) + w_2_A1.dot(Tstar_G1).subs(soldict)
Fstar_2_G1_R.subs({N_w_A1.dt(N).subs(soldict).dot(A1.z): alpha__A1})

\displaystyle - \frac{I_{1_z} \alpha^{A1} \operatorname{sin}\left(q_{4}\right)}{L} - M_{1} \dot{u}_{2}

\rightarrow (F_r^*)_{GI} = -J\{\omega_r^A \cdot [\ddot{q_k} g_1 + \dot{q_k} (\omega_3^A g_2 - \omega_2^A g_3)] + C_{kr} (\dot{\omega}_1^A + \ddot{q_k}) \} \qquad (r=1,...,n-m)

\text{Here}, \{\omega_1^A: \omega_2^A,\ \omega_2^A: \omega_3^A,\ \omega_3^A: \omega_1^A\}

\rightarrow \dot{q_k} = \sum_{s = 1}^{n - m} C_{ks} u_s + D_k \quad \text{(Generalized Speeds)}

\omega_i^A \overset{\Delta}{=} \omega^A \cdot \hat{g}_i \quad (i = 1, 2, 3)

# C_kr
C51, C61 = sm.symbols('C_51, C_61')
C_51 = soldict[q5.diff()].diff(u1)
C_61 = soldict[q6.diff()].diff(u1)
Fstar_1_G1_I = -J1 * (N_w_A1.dot(q5.diff().diff() * A1.y + q5.diff()*(N_w_A1.dot(A1.x)*A1.z - N_w_A1.dot(A1.z)*A1.x)) + C_51 * (N_w_A1.dot(A1.y).diff() + q5.diff().diff())) \
               -J1 * (N_w_A1.dot(q6.diff().diff() * A1.y + q6.diff()*(N_w_A1.dot(A1.x)*A1.z - N_w_A1.dot(A1.z)*A1.x)) + C_61 * (N_w_A1.dot(A1.y).diff() + q6.diff().diff()))   # B1 \ B2

Fstar_1_G1_I, C_51, C_61, Fstar_1_G1_I.subs({-C_51: -C51, -C_61: -C61}).simplify()

\displaystyle \left( \frac{J_{1} a \ddot{q}_{5}}{r_{1}} - \frac{J_{1} a \ddot{q}_{6}}{r_{1}}, \  - \frac{a}{r_{1}}, \  \frac{a}{r_{1}}, \  - J_{1} \left(C_{51} \ddot{q}_{5} + C_{61} \ddot{q}_{6}\right)\right)

# C_kr
C52, C62 = sm.symbols('C_52, C_62')
C_52 = soldict[q5.diff()].diff(u2)
C_62 = soldict[q6.diff()].diff(u2)
Fstar_2_G1_I = -J1 * (N_w_A1.dot(q5.diff().diff() * A1.y + q5.diff()*(N_w_A1.dot(A1.x)*A1.z - N_w_A1.dot(A1.z)*A1.x)) + C_52 * (N_w_A1.dot(A1.y).diff() + q5.diff().diff())) \
               -J1 * (N_w_A1.dot(q6.diff().diff() * A1.y + q6.diff()*(N_w_A1.dot(A1.x)*A1.z - N_w_A1.dot(A1.z)*A1.x)) + C_62 * (N_w_A1.dot(A1.y).diff() + q6.diff().diff()))   # B1 \ B2

Fstar_2_G1_I, C_52, C_62, Fstar_2_G1_I.subs({-C_52: -C52, -C_62: -C62}).simplify()

\displaystyle \left( \frac{J_{1} \left(- L + a \operatorname{sin}\left(q_{4}\right)\right) \ddot{q}_{5}}{L r_{1}} - \frac{J_{1} \left(L + a \operatorname{sin}\left(q_{4}\right)\right) \ddot{q}_{6}}{L r_{1}}, \  - \frac{- L + a \operatorname{sin}\left(q_{4}\right)}{L r_{1}}, \  \frac{L + a \operatorname{sin}\left(q_{4}\right)}{L r_{1}}, \  - J_{1} \left(C_{52} \ddot{q}_{5} + C_{62} \ddot{q}_{6}\right)\right)

\rightarrow (F_r^*)_G = (F_r^*)_{GR} + (F_r^*)_{GI}

Fstar_1_G1 = Fstar_1_G1_R + Fstar_1_G1_I
Fstar_1_G1.subs({N_w_A1.dt(N).subs(soldict).dot(A1.z): alpha__A1}).subs({-C_51: -C51, -C_61: -C61}).simplify()

\displaystyle - C_{51} J_{1} \ddot{q}_{5} - C_{61} J_{1} \ddot{q}_{6} - I_{1_z} \alpha^{A1}

Fstar_2_G1 = Fstar_2_G1_R + Fstar_2_G1_I
Fstar_2_G1.subs({N_w_A1.dt(N).subs(soldict).dot(A1.z): alpha__A1}).subs({-C_52: -C52, -C_62: -C62}).simplify()

\displaystyle - C_{52} J_{1} \ddot{q}_{5} - C_{62} J_{1} \ddot{q}_{6} - \frac{I_{1_z} \alpha^{A1} \operatorname{sin}\left(q_{4}\right)}{L} - M_{1} \dot{u}_{2}

\text{Gyrostat}\ G_2

\rightarrow {F_G}^* = -m_G {a^G}^*

Fstar_G2 = -M2 * N_a_C
Fstar_G2

\displaystyle -  M_{2} \left(- u_{1} u_{2} \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{4}\right) \dot{u}_{2} - \frac{l u^{2}_{2} \operatorname{sin}^{2}\left(q_{4}\right)}{L^{2}}\right)\mathbf{\hat{a_2}_x} -  M_{2} \left(\frac{l u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{l \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L} + \frac{u^{2}_{2} \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{4}\right)}{L}\right)\mathbf{\hat{a_2}_y}

\rightarrow {T_G}^* \overset{\Delta}{=} -[\alpha_A \cdot I_G + {\omega_r}^A \times (I_G \cdot {\omega_r}^A)]

Tstar_G2 = -(N_aa_A2.dot(I2) + me.cross(N_w_A2, I2.dot(N_w_A2)))
Tstar_G2

\displaystyle -  I_{2_z} \left(\frac{u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{\operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}\right)\mathbf{\hat{a_2}_z}

\rightarrow ({F_r}^*)_{GR} = {V_r}^G \cdot {F_G}^* + {\omega_r}^A \cdot {T_G}^*

Fstar_1_G2_R = V_1_C.dot(Fstar_G2) + w_1_A2.dot(Tstar_G2).subs(soldict)
Fstar_1_G2_R.subs({N_w_A2.dt(N).subs(soldict).dot(A2.z): alpha__A2})

\displaystyle 0

Fstar_2_G2_R = V_2_C.dot(Fstar_G2) + w_2_A1.dot(Tstar_G2).subs(soldict)
Fstar_2_G2_R.subs({N_w_A2.dt(N).subs(soldict).dot(A2.z): alpha__A2})

\displaystyle - \frac{I_{2_z} \alpha^{A2} \operatorname{sin}\left(q_{4}\right)}{L} - M_{2} \left(- u_{1} u_{2} \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{4}\right) \dot{u}_{2} - \frac{l u^{2}_{2} \operatorname{sin}^{2}\left(q_{4}\right)}{L^{2}}\right) \operatorname{cos}\left(q_{4}\right) - \frac{M_{2} l \left(\frac{l u_{1} u_{2} \operatorname{cos}\left(q_{4}\right)}{L} + \frac{l \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L} + \frac{u^{2}_{2} \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{4}\right)}{L}\right) \operatorname{sin}\left(q_{4}\right)}{L}

\rightarrow (F_r^*)_{GI} = -J\{\omega_r^A \cdot [\ddot{q_k} g_1 + \dot{q_k} (\omega_3^A g_2 - \omega_2^A g_3)] + C_{kr} (\dot{\omega}_1^A + \ddot{q_k}) \} \qquad (r=1,...,n-m)

\text{Here}, \{\omega_1^A: \omega_2^A,\ \omega_2^A: \omega_3^A,\ \omega_3^A: \omega_1^A\}

\rightarrow \dot{q_k} = \sum_{s = 1}^{n - m} C_{ks} u_s + D_k \quad \text{(Generalized Speeds)}

\omega_i^A \overset{\Delta}{=} \omega^A \cdot \hat{g}_i \quad (i = 1, 2, 3)

# C_kr
C71, C81 = sm.symbols('C_71, C_81')
C_71 = soldict[q7.diff()].diff(u1)
C_81 = soldict[q8.diff()].diff(u1)
Fstar_1_G2_I = -J2 * (N_w_A2.dot(q7.diff().diff() * A2.y + q7.diff()*(N_w_A2.dot(A2.x)*A2.z - N_w_A2.dot(A2.z)*A2.x)) + C_71 * (N_w_A2.dot(A2.y).diff() + q7.diff().diff())) \
               -J2 * (N_w_A2.dot(q8.diff().diff() * A2.y + q8.diff()*(N_w_A2.dot(A2.x)*A2.z - N_w_A2.dot(A2.z)*A2.x)) + C_81 * (N_w_A2.dot(A2.y).diff() + q8.diff().diff()))   # B1 \ B2

Fstar_1_G2_I, C_71, C_81, # Fstar_1_G2_I.subs({-C_71: -C71, -C_81: -C81}).simplify()

\displaystyle \left( 0, \  0, \  0\right)

# C_kr
C72, C82 = sm.symbols('C_72, C_82')
C_72 = soldict[q7.diff()].diff(u2)
C_82 = soldict[q8.diff()].diff(u2)
Fstar_2_G2_I = -J2 * (N_w_A2.dot(q7.diff().diff() * A2.y + q7.diff()*(N_w_A2.dot(A2.x)*A2.z - N_w_A2.dot(A2.z)*A2.x)) + C_72 * (N_w_A2.dot(A2.y).diff() + q7.diff().diff())) \
               -J2 * (N_w_A2.dot(q8.diff().diff() * A2.y + q8.diff()*(N_w_A2.dot(A2.x)*A2.z - N_w_A2.dot(A2.z)*A2.x)) + C_82 * (N_w_A2.dot(A2.y).diff() + q8.diff().diff()))   # B1 \ B2

Fstar_2_G2_I, C_72, C_82, Fstar_2_G2_I.subs({-C_72: -C72, -C_82: -C82}).simplify()

\displaystyle \left( \frac{J_{2} \left(- L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) \ddot{q}_{7}}{L r_{2}} - \frac{J_{2} \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) \ddot{q}_{8}}{L r_{2}}, \  - \frac{- L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)}{L r_{2}}, \  \frac{L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)}{L r_{2}}, \  - J_{2} \left(C_{72} \ddot{q}_{7} + C_{82} \ddot{q}_{8}\right)\right)

\rightarrow (F_r^*)_G = (F_r^*)_{GR} + (F_r^*)_{GI}

Fstar_1_G2 = Fstar_1_G2_R + Fstar_1_G2_I
# Fstar_1_G2.subs({N_w_A2.dt(N).subs(soldict).dot(A2.z): alpha__A2}) # .subs({-C_71: -C71, -C_81: -C81}).simplify()
Fstar_1_G2 = 0

\text{Here}, \{a_1^C: a_2^C,\ a_2^C: a_3^C,\ a_3^C: a_1^C\}

Fstar_2_G2 = Fstar_2_G2_R + Fstar_2_G2_I
Fstar_2_G2.subs({N_w_A2.dt(N).subs(soldict).dot(A2.z): alpha__A2}).subs({N_v_C.dt(N).subs(soldict).dot(A2.x): a_3__C}).subs({N_v_C.dt(N).subs(soldict).dot(A2.y): a_1__C}).subs({-C_72: -C72, -C_82: -C82}).simplify()

\displaystyle - C_{72} J_{2} \ddot{q}_{7} - C_{82} J_{2} \ddot{q}_{8} - \frac{I_{2_z} \alpha^{A2} \operatorname{sin}\left(q_{4}\right)}{L} - M_{2} a^{C}_{3} \operatorname{cos}\left(q_{4}\right) - \frac{M_{2} a^{C}_{1} l \operatorname{sin}\left(q_{4}\right)}{L}

\text{Variable-Mass Particle}\ P

\rightarrow {F_G}^* = -m_G {a^G}^*

Fstar_P = -m * N_a_P
Fstar_P

\displaystyle -  \left(- u_{1} u_{2} \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{4}\right) \dot{u}_{2}\right) m\mathbf{\hat{a_2}_x} -  \frac{m u^{2}_{2} \operatorname{sin}\left(q_{4}\right) \operatorname{cos}\left(q_{4}\right)}{L}\mathbf{\hat{a_2}_y}

\rightarrow ({F_r}^*)_{GR} = {V_r}^G \cdot {F_G}^*

Fstar_1_P_R = V_1_P.dot(Fstar_P)
Fstar_1_P_R

\displaystyle 0

Fstar_2_P_R = V_2_P.dot(Fstar_P)
Fstar_2_P_R

\displaystyle - \left(- u_{1} u_{2} \operatorname{sin}\left(q_{4}\right) + \operatorname{cos}\left(q_{4}\right) \dot{u}_{2}\right) m \operatorname{cos}\left(q_{4}\right)

\rightarrow (F_r^*)_G = (F_r^*)_{GR}

Fstar_1_P = Fstar_1_P_R
Fstar_1_P

\displaystyle 0

\text{Here}, \{a_1^P: a_2^P,\ a_2^P: a_3^P,\ a_3^P: a_1^P\}

Fstar_2_P = Fstar_2_P_R
Fstar_2_P.subs({N_v_P.dt(N).subs(soldict).dot(A2.x): a_3__P}).subs({N_v_P.dt(N).subs(soldict).dot(A2.y): a_1__P}).simplify()

\displaystyle - a^{P}_{3} m \operatorname{cos}\left(q_{4}\right)

\text{Generalized Inertia Forces}

\rightarrow F_r^* = (F_r^*)_{G_1} + (F_r^*)_{G_2} + (F_r^*)_{P} \quad (r = 1, 2)

Fstar_1 = Fstar_1_G1 + Fstar_1_G2 + Fstar_1_P
Fstar_1.subs(soldict).simplify()

\displaystyle \frac{- I_{1_z} r_{1} \left(L \dot{u}_{1} + u_{1} u_{2} \operatorname{cos}\left(q_{4}\right) + \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}\right) + J_{1} L a \left(\frac{- a \dot{u}_{1} + \dot{u}_{2} - \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} - \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}}{r_{1}} - \frac{a \dot{u}_{1} + \dot{u}_{2} + \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} + \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}}{r_{1}}\right)}{L r_{1}}

Fstar_2 = Fstar_2_G1 + Fstar_2_G2 + Fstar_2_P
Fstar_2.subs(soldict).simplify()

\displaystyle \frac{- J_{1} L r_{2} \left(\frac{\left(L - a \operatorname{sin}\left(q_{4}\right)\right) \left(- a \dot{u}_{1} + \dot{u}_{2} - \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} - \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}\right)}{r_{1}} + \frac{\left(L + a \operatorname{sin}\left(q_{4}\right)\right) \left(a \dot{u}_{1} + \dot{u}_{2} + \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} + \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}\right)}{r_{1}}\right) - J_{2} L r_{1} \left(\left(L \operatorname{cos}\left(q_{4}\right) - b \operatorname{sin}\left(q_{4}\right)\right) \left(\frac{\left(- \operatorname{sin}\left(q_{4}\right) \dot{q}_{4} - \frac{b \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L}\right) u_{2}}{r_{2}} + \frac{\left(\operatorname{cos}\left(q_{4}\right) - \frac{b \operatorname{sin}\left(q_{4}\right)}{L}\right) \dot{u}_{2}}{r_{2}}\right) + \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) \left(\frac{\left(- \operatorname{sin}\left(q_{4}\right) \dot{q}_{4} + \frac{b \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L}\right) u_{2}}{r_{2}} + \frac{\left(\operatorname{cos}\left(q_{4}\right) + \frac{b \operatorname{sin}\left(q_{4}\right)}{L}\right) \dot{u}_{2}}{r_{2}}\right)\right) + \frac{L^{2} r_{1} r_{2} \left(- 2 M_{1} \dot{u}_{2} + \left(u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) - \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - \dot{u}_{2}\right) m\right)}{2} + \frac{r_{1} r_{2} \left(- 2 I_{1_z} L \operatorname{sin}\left(q_{4}\right) \dot{u}_{1} - I_{1_z} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) + I_{1_z} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - I_{1_z} \dot{u}_{2} - I_{2_z} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) + I_{2_z} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - I_{2_z} \dot{u}_{2} + L^{2} M_{2} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) - L^{2} M_{2} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - L^{2} M_{2} \dot{u}_{2} - M_{2} l^{2} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) + M_{2} l^{2} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - M_{2} l^{2} \dot{u}_{2}\right)}{2}}{L^{2} r_{1} r_{2}}

\mathbf{\text{Velocity of material ejected at}\ P\ \text{relative to}\ A_2 \rightarrow -C(t)g_3^{'};\ C(t) \rightarrow \text{positive}}

C = me.dynamicsymbols('C')
C_t = -C*A2.x
C_t

\displaystyle -  C\mathbf{\hat{a_2}_x}

\text{Generalized Thrust}

\rightarrow F_r^{\prime} \triangleq \sum_{i=1}^{N} \mathbf{V}_{r}^{P i} \cdot \mathbf{C}^{P i} \dot{m}_{i} \quad (r=1, \ldots, k)

Fprime_1 = V_1_P.dot(C_t)*m.diff()
Fprime_1

\displaystyle 0

Fprime_2 = V_2_P.dot(C_t)*m.diff()
Fprime_2

\displaystyle - C \operatorname{cos}\left(q_{4}\right) \dot{m}

\text{Extended Kane's Equations for Variable-Mass Systems}

\rightarrow F_r + F_r^* + F_r^{\prime} = 0 \quad (r = 1,..., k)

Here, F_r = 0 \rightarrow no forces contributing to generalized active forces

kane_1 = Fstar_1.simplify() + Fprime_1.simplify()
kane_1.subs(soldict).simplify()

\displaystyle \frac{- I_{1_z} r_{1} \left(L \dot{u}_{1} + u_{1} u_{2} \operatorname{cos}\left(q_{4}\right) + \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}\right) + J_{1} L a \left(\frac{- a \dot{u}_{1} + \dot{u}_{2} - \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} - \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}}{r_{1}} - \frac{a \dot{u}_{1} + \dot{u}_{2} + \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} + \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}}{r_{1}}\right)}{L r_{1}}

kane_2 = Fstar_2 + Fprime_2
kane_2.subs(soldict).simplify()

\displaystyle \frac{- J_{1} L r_{2} \left(\frac{\left(L - a \operatorname{sin}\left(q_{4}\right)\right) \left(- a \dot{u}_{1} + \dot{u}_{2} - \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} - \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}\right)}{r_{1}} + \frac{\left(L + a \operatorname{sin}\left(q_{4}\right)\right) \left(a \dot{u}_{1} + \dot{u}_{2} + \frac{a u_{2} \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L} + \frac{a \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}}{L}\right)}{r_{1}}\right) - J_{2} L r_{1} \left(\left(L \operatorname{cos}\left(q_{4}\right) - b \operatorname{sin}\left(q_{4}\right)\right) \left(\frac{\left(- \operatorname{sin}\left(q_{4}\right) \dot{q}_{4} - \frac{b \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L}\right) u_{2}}{r_{2}} + \frac{\left(\operatorname{cos}\left(q_{4}\right) - \frac{b \operatorname{sin}\left(q_{4}\right)}{L}\right) \dot{u}_{2}}{r_{2}}\right) + \left(L \operatorname{cos}\left(q_{4}\right) + b \operatorname{sin}\left(q_{4}\right)\right) \left(\frac{\left(- \operatorname{sin}\left(q_{4}\right) \dot{q}_{4} + \frac{b \operatorname{cos}\left(q_{4}\right) \dot{q}_{4}}{L}\right) u_{2}}{r_{2}} + \frac{\left(\operatorname{cos}\left(q_{4}\right) + \frac{b \operatorname{sin}\left(q_{4}\right)}{L}\right) \dot{u}_{2}}{r_{2}}\right)\right) + \frac{L^{2} r_{1} r_{2} \left(- 2 M_{1} \dot{u}_{2} + \left(u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) - \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - \dot{u}_{2}\right) m - 2 C \operatorname{cos}\left(q_{4}\right) \dot{m}\right)}{2} + \frac{r_{1} r_{2} \left(- 2 I_{1_z} L \operatorname{sin}\left(q_{4}\right) \dot{u}_{1} - I_{1_z} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) + I_{1_z} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - I_{1_z} \dot{u}_{2} - I_{2_z} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) + I_{2_z} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - I_{2_z} \dot{u}_{2} + L^{2} M_{2} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) - L^{2} M_{2} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - L^{2} M_{2} \dot{u}_{2} - M_{2} l^{2} u_{1} u_{2} \operatorname{sin}\left(2 q_{4}\right) + M_{2} l^{2} \operatorname{cos}\left(2 q_{4}\right) \dot{u}_{2} - M_{2} l^{2} \dot{u}_{2}\right)}{2}}{L^{2} r_{1} r_{2}}

kane_1_eq = sm.Eq(kane_1.simplify().subs(soldict).simplify().subs(u).simplify(), 0)
kane_1_eq

\displaystyle - \frac{\left(I_{1_z} r_{1}^{2} + 2 J_{1} a^{2}\right) \left(L \dot{u}_{1} + u_{1} u_{2} \operatorname{cos}\left(q_{4}\right) + \operatorname{sin}\left(q_{4}\right) \dot{u}_{2}\right)}{L r_{1}^{2}} = 0

kane_2_eq = sm.Eq(kane_2.simplify().subs(soldict).simplify().subs(u).simplify(), 0)
# kane_2_eq