import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting()

N, A, B = sm.symbols("N, A, B", cls=me.ReferenceFrame)

alpha, beta = me.dynamicsymbols('alpha, beta')
d, w, c, l = sm.symbols('d, w, c, l')

A.orient_axis(N, alpha, N.z)
B.orient_axis(A, beta, A.x)

N_v_S = -d*alpha.diff()*A.y
N_v_S

A.ang_vel_in(N) + B.ang_vel_in(A)

B.ang_vel_in(N)

r_S_Q = -w*B.x - (c + l/2)*B.z
r_S_Q

N_v_Q = N_v_S + B.ang_vel_in(N).cross(r_S_Q)
N_v_Q

N_v_Q.express(B)

S, Q = sm.symbols('S, Q', cls=me.Point)

Q.set_pos(S, r_S_Q)

Q.pos_from(S)

S.set_vel(N, N_v_S)

S.vel(N)

Q.v2pt_theory(S, N, B)

s = me.dynamicsymbols('s')

r_S_R = (s-w)*B.x + (l/2-c)*B.z
r_S_R

B_v_R = s.diff()*B.x
B_v_R

B.ang_vel_in(A)

A_v_R = B_v_R + me.cross(B.ang_vel_in(A), r_S_R)
A_v_R

me.cross(B.ang_acc_in(A), r_S_R)

me.cross(B.ang_vel_in(A), me.cross(B.ang_vel_in(A), r_S_R))

2*me.cross(B.ang_vel_in(A), B_v_R)

B_v_R

t = me.dynamicsymbols._t

B_v_R.diff(t, B)

R = me.Point('R')

R.set_pos(S, r_S_R)

R.set_vel(B, B_v_R)

S.set_vel(A, 0)

R.a1pt_theory(S, A, B)

R.a1pt_theory(S, N, B)