def test_dcm(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = A.orientnew('B', 'Axis', [q2, A.x]) C = B.orientnew('C', 'Axis', [q3, B.y]) D = N.orientnew('D', 'Axis', [q4, N.y]) E = N.orientnew('E', 'Space', [q1, q2, q3], '123') assert N.dcm(C) == Matrix([ [- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) * cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], [sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)], [- sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)]]) # This is a little touchy. Is it ok to use simplify in assert? assert D.dcm(C) == Matrix( [[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (- sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4))], [sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)], [sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4))]]) assert E.dcm(N) == Matrix( [[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)], [sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2)], [sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), - sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2)]])
def test_dcm(): q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4") N = ReferenceFrame("N") A = N.orientnew("A", "Axis", [q1, N.z]) B = A.orientnew("B", "Axis", [q2, A.x]) C = B.orientnew("C", "Axis", [q3, B.y]) D = N.orientnew("D", "Axis", [q4, N.y]) E = N.orientnew("E", "Space", [q1, q2, q3], "123") assert N.dcm(C) == Matrix( [ [ -sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), -sin(q1) * cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1), ], [ sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3), ], [-sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)], ] ) # This is a little touchy. Is it ok to use simplify in assert? test_mat = D.dcm(C) - Matrix( [ [ cos(q1) * cos(q3) * cos(q4) - sin(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)), -sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)), ], [ sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3), ], [ sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), ], ] ) assert test_mat.expand() == zeros(3, 3) assert E.dcm(N) == Matrix( [ [cos(q2) * cos(q3), sin(q3) * cos(q2), -sin(q2)], [ sin(q1) * sin(q2) * cos(q3) - sin(q3) * cos(q1), sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), sin(q1) * cos(q2), ], [ sin(q1) * sin(q3) + sin(q2) * cos(q1) * cos(q3), -sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), ], ] )
I_1s_O = inertia_of_point_mass(m, pP1s.pos_from(pO), N) I_2s_O = inertia_of_point_mass(m, pP2s.pos_from(pO), N) I_3s_O = inertia_of_point_mass(m, pP3s.pos_from(pO), N) print("\nI_1s_rel_O = {0}".format(I_1s_O)) print("\nI_2s_rel_O = {0}".format(I_2s_O)) print("\nI_3s_rel_O = {0}".format(I_3s_O)) I_1_1s = inertia(N, m*b**2/12, m*b**2/12, 2*m*b**2/12) I_2_2s = inertia(N, 2*m*b**2/12, m*b**2/12, m*b**2/12) I_3_3s_N2 = Matrix([[2, 0, 0], [0, 1, 0], [0, 0, 1]]) I_3_3s_N = m*b**2/12 * simplify(N.dcm(N2) * I_3_3s_N2 * N2.dcm(N)) I_3_3s = inertia(N, I_3_3s_N[0, 0], I_3_3s_N[1, 1], I_3_3s_N[2, 2], I_3_3s_N[0, 1], I_3_3s_N[1, 2], I_3_3s_N[2, 0]) I_1_O = I_1_1s + I_1s_O I_2_O = I_2_2s + I_2s_O I_3_O = I_3_3s + I_3s_O print("\nI_1_rel_O = {0}".format(I_1_O)) print("\nI_2_rel_O = {0}".format(I_2_O)) print("\nI_3_rel_O = {0}".format(I_3_O)) # assembly inertia tensor is the sum of the inertia tensor of each component I_B_O = I_1_O + I_2_O + I_3_O print("\nI_B_rel_O = {0}".format(I_B_O)) # n_a is parallel to line L
zero_omega = dict(zip(speeds, zeros(3))) torques = Matrix(specified) g_terms = forcing.subs(zero_omega) - torques g_terms_a = g_terms[0] g_terms_b = g_terms[1] g_terms_c = g_terms[2] ang_vel = Matrix(speeds) coriolis = simplify(forcing - g_terms - torques) r_ai = a_frame.dcm(inertial_frame).as_mutable() t_ai = simplify(B.pos_from(A).express(inertial_frame).to_matrix(inertial_frame)) r_bi = simplify(b_frame.dcm(inertial_frame)).as_mutable() t_bi = simplify(C.pos_from(A).express(inertial_frame).to_matrix(inertial_frame)) t_ba = simplify(C.pos_from(B).express(inertial_frame).to_matrix(inertial_frame)) t_ba_in_a = simplify(C.pos_from(B).express(a_frame).to_matrix(a_frame)) tf_bi = r_bi.row_join(t_bi) r_ci = simplify(c_frame.dcm(inertial_frame)).as_mutable() t_ci = simplify(D.pos_from(A).express(inertial_frame).to_matrix(inertial_frame)) t_ca = simplify(D.pos_from(B).express(inertial_frame).to_matrix(inertial_frame)) t_ca_in_a = simplify(D.pos_from(B).express(a_frame).to_matrix(a_frame)) thetadot_omega_dict = dict(zip([theta_a.diff(time), theta_b.diff(time), theta_c.diff(time)], speeds))
pP2s = pO.locatenew('P2*', b / 2 * (2 * N.x + N.y + N.z)) pP3s = pO.locatenew( 'P3*', b / 2 * ((2 + sin(theta)) * N.x + (2 - cos(theta)) * N.y + N.z)) I_1s_O = inertia_of_point_mass(m, pP1s.pos_from(pO), N) I_2s_O = inertia_of_point_mass(m, pP2s.pos_from(pO), N) I_3s_O = inertia_of_point_mass(m, pP3s.pos_from(pO), N) print("\nI_1s_rel_O = {0}".format(I_1s_O)) print("\nI_2s_rel_O = {0}".format(I_2s_O)) print("\nI_3s_rel_O = {0}".format(I_3s_O)) I_1_1s = inertia(N, m * b**2 / 12, m * b**2 / 12, 2 * m * b**2 / 12) I_2_2s = inertia(N, 2 * m * b**2 / 12, m * b**2 / 12, m * b**2 / 12) I_3_3s_N2 = Matrix([[2, 0, 0], [0, 1, 0], [0, 0, 1]]) I_3_3s_N = m * b**2 / 12 * simplify(N.dcm(N2) * I_3_3s_N2 * N2.dcm(N)) I_3_3s = inertia(N, I_3_3s_N[0, 0], I_3_3s_N[1, 1], I_3_3s_N[2, 2], I_3_3s_N[0, 1], I_3_3s_N[1, 2], I_3_3s_N[2, 0]) I_1_O = I_1_1s + I_1s_O I_2_O = I_2_2s + I_2s_O I_3_O = I_3_3s + I_3s_O print("\nI_1_rel_O = {0}".format(I_1_O)) print("\nI_2_rel_O = {0}".format(I_2_O)) print("\nI_3_rel_O = {0}".format(I_3_O)) # assembly inertia tensor is the sum of the inertia tensor of each component I_B_O = I_1_O + I_2_O + I_3_O print("\nI_B_rel_O = {0}".format(I_B_O)) # n_a is parallel to line L
mass_matrix = simplify(kane.mass_matrix) mass_matrix_full = kane.mass_matrix_full torques = Matrix(specified) zero_omega = dict(zip(speeds, zeros(2))) g_terms = simplify(forcing.subs(zero_omega) - torques) g_terms_1 = g_terms[0] g_terms_2 = g_terms[1] coriolis = simplify(forcing - g_terms - torques) r_1i = simplify(one_frame.dcm(inertial_frame)) t_1i = simplify(two.pos_from(one).express(inertial_frame).to_matrix(inertial_frame)) t_2i = simplify(three.pos_from(one).express(inertial_frame).to_matrix(inertial_frame)) time = symbols('t') alpha1, alpha2 = dynamicsymbols('a_1, a_2') accelerations = [alpha1, alpha2] thetadot_omega_dict = dict(zip([theta1.diff(time), theta2.diff(time)], [omega1, omega2])) omegadot_alpha_dict = dict(zip([omega1.diff(time), omega2.diff(time)], [alpha1, alpha2])) com = (t_1i*one_mass + t_2i*two_mass)/(one_mass+two_mass) com_dot = com.diff(time).subs(thetadot_omega_dict)
kane = KanesMethod(inertial_frame, coordinates, speeds, kinematic_differential_equations) loads = [t_grav_force, s_link_torque] bodies = [tP] fr, frstar = kane.kanes_equations(loads, bodies) mass_matrix = kane.mass_matrix forcing = kane.forcing[0] # Constants # --------- constants = [s_length, s_mass, g] time = symbols('t') omega_s = dynamicsymbols('omega_s') thetadot_omega_dict = dict(zip([theta_s.diff(time)], [omega_s])) alpha_s = dynamicsymbols('alpha_s') omegadot_alpha_dict = dict(zip([omega_s.diff(time)], [alpha_s])) forces = forcing - s_torque ri = s_frame.dcm(inertial_frame) com = T.pos_from(S).express(inertial_frame).to_matrix(inertial_frame) com_dot = com.diff(time).subs(thetadot_omega_dict) com_ddot = com_dot.diff(time).subs(thetadot_omega_dict).subs(omegadot_alpha_dict)
# define all reference frames of all individually moving links of the robot print("Defining reference frames") inertial_frame = ReferenceFrame('I') lower_leg_left_frame = ReferenceFrame('R_1') upper_leg_left_frame = ReferenceFrame('R_2') hip_frame = ReferenceFrame('R_3') upper_leg_right_frame = ReferenceFrame('R_4') lower_leg_right_frame = ReferenceFrame('R_5') #%% Angles # everything is symbolic, so create all angles of your robot # NOTE: the angle phi is the angle between your robot and the inertial frame theta0, theta1, theta2, theta3, phi = dynamicsymbols( 'theta0, theta1, theta2, theta3, phi') lower_leg_left_frame.orient(inertial_frame, 'Axis', (phi, inertial_frame.z)) simplify(lower_leg_left_frame.dcm(inertial_frame)) upper_leg_left_frame.orient(lower_leg_left_frame, 'Axis', (theta0, -lower_leg_left_frame.z)) simplify(upper_leg_left_frame.dcm(inertial_frame)) hip_frame.orient(upper_leg_left_frame, 'Axis', (theta1, -upper_leg_left_frame.z)) hip_frame.dcm(inertial_frame) upper_leg_right_frame.orient(hip_frame, 'Axis', (theta2, -hip_frame.z)) simplify(upper_leg_right_frame.dcm(inertial_frame)) lower_leg_right_frame.orient(upper_leg_right_frame, 'Axis', (theta3, -upper_leg_right_frame.z)) simplify(lower_leg_right_frame.dcm(inertial_frame))
#%% # define all reference frames of all individually moving links of the robot print("Defining reference frames") inertial_frame = ReferenceFrame('I') lower_leg_left_frame = ReferenceFrame('R_1') upper_leg_left_frame = ReferenceFrame('R_2') hip_frame = ReferenceFrame('R_3') upper_leg_right_frame = ReferenceFrame('R_4') lower_leg_right_frame = ReferenceFrame('R_5') #%% Angles # everything is symbolic, so create all angles of your robot # NOTE: the angle phi is the angle between your robot and the inertial frame theta0, theta1, theta2, theta3, phi = dynamicsymbols('theta0, theta1, theta2, theta3, phi') lower_leg_left_frame.orient(inertial_frame, 'Axis', (phi, inertial_frame.z)) simplify(lower_leg_left_frame.dcm(inertial_frame)) upper_leg_left_frame.orient(lower_leg_left_frame, 'Axis', (theta0, -lower_leg_left_frame.z)) simplify(upper_leg_left_frame.dcm(inertial_frame)) hip_frame.orient(upper_leg_left_frame, 'Axis', (theta1, -upper_leg_left_frame.z)) hip_frame.dcm(inertial_frame) upper_leg_right_frame.orient(hip_frame, 'Axis', (theta2, -hip_frame.z)) simplify(upper_leg_right_frame.dcm(inertial_frame)) lower_leg_right_frame.orient(upper_leg_right_frame, 'Axis', (theta3, -upper_leg_right_frame.z)) simplify(lower_leg_right_frame.dcm(inertial_frame)) #%% Points and Locations # define the kinematical chain of your robot print("Defining kinematical chain")
from __future__ import print_function, division from sympy import symbols, simplify from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point from sympy.physics.vector import init_vprinting init_vprinting(use_latex='mathjax', pretty_print=False) inertial_frame = ReferenceFrame('I') lower_leg_frame = ReferenceFrame('L') theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3') lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z)) lower_leg_frame.dcm(inertial_frame) omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3') lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) print(lower_leg_frame.ang_vel_in(inertial_frame))
joint_pos_3 = P3.pos_from(P0).express(N).subs(dummy_dict).to_matrix(N) joint_pos_4 = P4.pos_from(P0).express(N).subs(dummy_dict).to_matrix(N) joint_pos_5 = P5.pos_from(P0).express(N).subs(dummy_dict).to_matrix(N) joint_pos_6 = P6.pos_from(P0).express(N).subs(dummy_dict).to_matrix(N) joint_pos_7 = P7.pos_from(P0).express(N).subs(dummy_dict).to_matrix(N) joint_pos = Matrix([[joint_pos_1[0], joint_pos_1[1], joint_pos_1[2]], [joint_pos_2[0], joint_pos_2[1], joint_pos_2[2]], [joint_pos_3[0], joint_pos_3[1], joint_pos_3[2]], [joint_pos_4[0], joint_pos_4[1], joint_pos_4[2]], [joint_pos_5[0], joint_pos_5[1], joint_pos_5[2]], [joint_pos_6[0], joint_pos_6[1], joint_pos_6[2]], [joint_pos_7[0], joint_pos_7[1], joint_pos_7[2]]]) # End-effector orientation R = N.dcm(H).subs(dummy_dict) # Write Jacobian to file # Translational part of the Jacobian Jt = Matrix([P9.pos_from(P0).dot(N.x), P9.pos_from(P0).dot(N.y), P9.pos_from(P0).dot(N.z)])\ .jacobian(Matrix([q1, q2, q3, q4, q5, q6, q7])).subs(dummy_dict) if gripper \ else Matrix([P8.pos_from(P0).dot(N.x), P8.pos_from(P0).dot(N.y), P8.pos_from(P0).dot(N.z)])\ .jacobian(Matrix([q1, q2, q3, q4, q5, q6, q7])).subs(dummy_dict) # Rotational part of the Jacobian Jr = Matrix([ [
# In[26]: # In[70]: from sympy.physics.mechanics import ReferenceFrame, Vector q0, q1, q2, q3, q4 = symbols('q0 q1 q2 q3 q4') N = ReferenceFrame('N') # In[75]: N.dcm(N) # In[26]: # In[103]: from sympy.physics.mechanics import ReferenceFrame, Vector N = ReferenceFrame('N') x, y , q1 = symbols('x, y, q1') B = ReferenceFrame('B') B.orient(N, 'Axis', [q1, N.x]) B.dcm(N)