kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
K = mB*R**2/4
J = mB*R**2/2
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print('inertia_coefficient_matrix = {0}'.format(msprint(M)))
M_expected = Matrix([[IA + mA*a**2 + mB*(R**2/2 + 3*b**2), 0],
[0, mA + 3*mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
kde = [u1 - dot(A.ang_vel_in(F), A.x), u2 - dot(pD.vel(F), A.y), u3 - q3d, u4 - q4d, u5 - q5d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
K = mB * R ** 2 / 4
J = mB * R ** 2 / 2
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody("rbA", pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody("rbB", pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody("rbC", pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print("inertia_coefficient_matrix = {0}".format(msprint(M)))
M_expected = Matrix([[IA + mA * a ** 2 + mB * (R ** 2 / 2 + 3 * b ** 2), 0], [0, mA + 3 * mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
def find_coupled_speeds(kde_map):
partials = partial_velocities([rb.frame, rb.masscenter], [u1, u2, u3],
A, kde_map)
M = trigsimp(inertia_coefficient_matrix([rb], partials))
coupled_speeds(M, partials)
return M
