import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting()
A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')
theta = me.dynamicsymbols('theta')
B.orient_axis(A, theta, A.y)
Q = 3*me.outer(B.x, A.z) + 4*me.outer(A.x, A.x) + 6*me.outer(A.z, B.z)
Q
Q.express(A)
Q.express(B)
Q.to_matrix(A)
Q.to_matrix(B)
M1, M2, M3 = me.dynamicsymbols('M1, M2, M3')
A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')
w1, w2, w3 = me.dynamicsymbols('omega1, omega2, omega3')
A_w_B = w1*B.x + w2*B.y + w3*B.z
A_w_B
M = M1*B.x + M2*B.y + M3*B.z
M
I11, I22, I33 = sm.symbols('I_{11}, I_{22}, I_{33}')
I = I11*me.outer(B.x, B.x) + I22*me.outer(B.y, B.y) + I33*me.outer(B.z, B.z)
I
H = me.dot(I, A_w_B)
H
t = me.dynamicsymbols._t
Hdot = H.diff(t, B) + me.cross(A_w_B, H)
Hdot
M - Hdot
sm.Eq(M.to_matrix(B), Hdot.to_matrix(B))
A_w_B.diff(t, B)
B = me.ReferenceFrame('B')
H = me.ReferenceFrame('H')
B_C_H = sm.Matrix([[0, -1, 0],
[-1, 0, 0],
[0, 0, -1]])
B_C_H
B.orient_explicit(H, B_C_H.transpose())
mH = 84
mB = 9.9
r_O_Ho = 0.4*B.x - 1.1*B.z
r_O_Bo = 0.3*B.x - 0.5*B.z
r_O_Fo = (mB*r_O_Bo + mH*r_O_Ho)/(mB + mH)
r_O_Fo
I_H_Ho = me.inertia(H, 11.3, 11.0, 2.3, iyz=-1.7)
I_B_Bo = me.inertia(B, 0.5, 1.3, 0.8, izx=-0.1)
I_H_Ho
r_Fo_Ho = r_O_Fo - r_O_Ho
U = me.outer(H.x, H.x) + me.outer(H.y, H.y) + me.outer(H.z, H.z)
I_Ho_Fo = mH*(r_Fo_Ho.dot(r_Fo_Ho)*U -
me.outer(r_Fo_Ho, r_Fo_Ho))
I_Ho_Fo
r_Fo_Bo = r_O_Fo - r_O_Bo
I_Bo_Fo = mH*(r_Fo_Bo.dot(r_Fo_Bo)*U -
me.outer(r_Fo_Bo, r_Fo_Bo))
I_Bo_Fo
I_B_Fo = I_B_Bo + I_Bo_Fo
I_H_Fo = I_H_Ho + I_Ho_Fo
I_F_Fo = I_B_Fo + I_H_Fo
I_F_Fo
I_F_Fo.to_matrix(B)