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).simplify()
alpha_B_N
a_Pab_N = Pab.acc(N).subs(qdots).simplify()
a_Pab_N
a_Bo_N = Bo.acc(N).express(B).simplify()
a_Bo_N
a_Pc_N = Pc.acc(N).express(C).subs(qdots).simplify()
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.trigsimp(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 = sm.trigsimp(Frstar)
Frstar
f = Fr + Frstar
me.find_dynamicsymbols(f)
f.free_symbols
u = sm.Matrix(u)
u
u.diff()
M = f.jacobian(u.diff())
M = sm.trigsimp(M)
M
G = -(f - M * u.diff())
udots = M.LUsolve(G)
# udots # no need to print, too long, may eat up your computer's memory