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