import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting()
q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')
u1, u2, u3 = me.dynamicsymbols('u1, u2, u3')
F, T = me.dynamicsymbols('F, T')
k, c, ma, mb, mc, IB_bo, l, kT, g = sm.symbols('k, c, m_a, m_b, m_c, I_{B_bo}, l, k_T, g')
k, c, ma, mb, mc, IB_bo, l, kT, g
N = me.ReferenceFrame('N')
B = N.orientnew('B', 'Axis', (q2, N.z))
C = B.orientnew('C', 'Axis', (q3, N.z))
O = me.Point('O')
Pab = O.locatenew('P_{ab}', q1 * N.x)
Bo = Pab.locatenew('B_o', - 2 * l / 3 * B.y)
Pbc = Pab.locatenew('P_{bc}', -l * B.y)
Pc = Pbc.locatenew('P_c', -l * C.y)
Pc.pos_from(O)
Pab.set_vel(N, Pab.pos_from(O).dt(N))
Pab.vel(N)
Bo.v2pt_theory(Pab, N, B)
Pbc.v2pt_theory(Pab, N, B)
Pc.v2pt_theory(Pbc, N, C)
u1_eq = sm.Eq(u1, Pab.vel(N).dot(N.x))
u2_eq = sm.Eq(u2, Bo.vel(N).dot(B.x))
u3_eq = sm.Eq(u3, C.ang_vel_in(B).dot(B.z))
qdots = sm.solve([u1_eq, u2_eq, u3_eq], q1.diff(), q2.diff(), q3.diff())
qdots
Substitute expressions for the $\dot{q}$'s.
Pab.set_vel(N, Pab.vel(N).subs(qdots).simplify())
Pab.vel(N)
Bo.set_vel(N, Bo.vel(N).subs(qdots).express(B).simplify())
Bo.vel(N)
Pc.set_vel(N, Pc.vel(N).subs(qdots).simplify())
Pc.vel(N)
B.set_ang_vel(N, B.ang_vel_in(N).subs(qdots).simplify())
B.ang_vel_in(N)
C.set_ang_vel(B, u3 * N.z)
Each partial velocity can be calculated manually like so:
Pab.vel(N).diff(u1, N)
Or you can use this convenience function:
Pab.partial_velocity(N, u1)
All partial velocities can be calculated at once with:
Pab.partial_velocity(N, u1, u2, u3)
Bo.partial_velocity(N, u1, u2, u3)
Pc.partial_velocity(N, u1, u2, u3)
This also works for angular partial velocities:
B.partial_velocity(N, u1, u2, u3)
The convenience function is doing this behind the scenes:
[B.ang_vel_in(N).diff(ur, N) for ur in [u1, u2, u3]]
alpha_B_N = B.ang_acc_in(N).subs(qdots)
alpha_B_N
a_Pab_N = Pab.acc(N).subs(qdots)
a_Pab_N
a_Bo_N = Bo.acc(N).express(B).subs(qdots)
a_Bo_N
a_Pc_N = Pc.acc(N).express(C).subs(qdots)
a_Pc_N
ma, mc
IB = me.inertia(B, 0, 0, IB_bo)
IB
Rab = (F - k*q1 - c*qdots[q1.diff()]) * N.x
Rab
Rbo = -(mb*g)*N.y
Rbo
Rc = -(mc*g)*N.y
Rc
TB = (T + kT*q3)*N.z
TB
Pab.partial_velocity(N, u1).dot(Rab)
Pab.partial_velocity(N, u2).dot(Rab)
Bo.partial_velocity(N, u2).dot(Rbo) + B.partial_velocity(N, u2).dot(TB)
u = [u1, u2, u3]
Fr = []
for ur in u:
Fr.append(Pab.partial_velocity(N, ur).dot(Rab) + # particle
Pc.partial_velocity(N, ur).dot(Rc) + # particle
Bo.partial_velocity(N, ur).dot(Rbo) + B.partial_velocity(N, ur).dot(TB)) # rigid body
Fr
Fr = sm.Matrix(Fr)
Fr
Frstar = []
for ur in u:
Frstar.append(Pab.partial_velocity(N, ur).dot(-ma*a_Pab_N) +
Pc.partial_velocity(N, ur).dot(-mc*a_Pc_N) +
Bo.partial_velocity(N, ur).dot(-mb*a_Bo_N) +
B.partial_velocity(N, ur).dot(-alpha_B_N.dot(IB) +
B.ang_vel_in(N).cross(IB.dot(B.ang_vel_in(N)))))
Frstar = sm.Matrix(Frstar)
Frstar = Frstar
Frstar
f = Fr + Frstar
me.find_dynamicsymbols(f)
f.free_symbols
Create a state vector with all of the u's and q's.
x = sm.Matrix([u1, u2, u3, q1, q2, q3])
x
For this model, an equilibrium point happens to be when $\bar{x}=0$, so create that vector:
xeq = sm.zeros(6, 1)
xeq
Make sure the dynamical equations are functions of the correct variables:
me.find_dynamicsymbols(f), f.shape
Express the kinematical equations in the form $\bar{g}(\bar{x},\dot{\bar{x}}, t)=0$
g = sm.Matrix([q1.diff() - u1,
q2.diff() - qdots[q2.diff()],
q3.diff() - u3])
g
Form the vector function of both the dynamical and kinematical equations:
h = f.col_join(g)
For a holomic system h should be 2n in length:
h.shape
This vector is just there to help us avoid an incorrect substitution in the following steps. We don't want to sub in for things inside derivatives.
q1d, q2d, q3d, u1d, u2d, u3d = me.dynamicsymbols('q_{1d}, q_{2d}, q_{3d}, u_{1d}, u_{2d}, u_{3d}')
xd = sm.Matrix([u1d, u2d, u3d, q1d, q2d, q3d])
xd
{k: v for k, v in zip(x.diff(), xd)}
h = h.subs({k: v for k, v in zip(x.diff(), xd)})
me.find_dynamicsymbols(h)
xeq_sub = {k: v for k, v in zip(x, xeq)}
xeq_sub
Here is the linearization using the first two terms of the Taylor Series
$\bar{h}_{lin} = \bar{h}(\bar{x}_{eq}) + J_h(\bar{x}_{eq})(\bar{x} - \bar{x}_{eq})$
h_lin = h.subs(xeq_sub) + h.jacobian(x).subs(xeq_sub)*(x - xeq)
h_lin.simplify()
h_lin
Now, solve for $\dot{\bar{x}}$ by forming $\mathbf{M}\dot{\bar{x}} = \mathbf{G}$
M = h_lin.jacobian(xd)
M
xdot = M.LUsolve(-(h_lin - M*xd))
xdot.simplify()
You can put it into state space form now: $\dot{\bar{x}}=\mathbf{A}\bar{x}+\mathbf{B}\bar{u}$
A = xdot.jacobian(x)
A
B = xdot.jacobian([F, T])
B