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?
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)
