def roll_pitch_yaw_to_rotation_matrix(roll, pitch, yaw):
"""
"""
R = rotzyx(yaw, pitch, roll)[0:3,0:3]
return R
Matrices and vectors are representeed with numpy arrays.
Here, we present some functions and operations to do mechanics.
"""
import numpy as np
np.set_printoptions(suppress=True)
##### About homogeneous matrix #################################################
import arboris.homogeneousmatrix as Hg
H_o_a = Hg.transl(1,2,3) # get a translation homogeneous matrix
H_a_b = Hg.rotzyx(.1,.2,.3) # get a rotation R = Rz * Ry * Rx
# also Hg.rotzy R = Rz * Ry
# Hg.rotzx R = Rz * Rx
# Hg.rotyx R = Ry * Rx
# Hg.rotz R = Rz
# Hg.roty R = Ry
# Hg.rotx R = Rx
H_o_b = np.dot(H_o_a, H_a_b) # H from {o} to {b} is H{o}->{a} * H{a}->{b}
H_b_o = Hg.inv(H_o_b) # obtain inverse, H from {b} to {o}
print("H_o_b * H_b_o\n", np.dot(H_o_b, H_b_o))
def pose(self):
return Hg.rotzyx(self.gpos[0],self.gpos[1],self.gpos[2])
def pose(self):
return Hg.rotzyx(self.gpos[0], self.gpos[1], self.gpos[2])
def get_meshes_positions():
return {
"head_mesh" : dot(transl(0,0,-0.055), rotzyx(pi/2., 0, 0)),
"torso_mesh" : dot(transl(0,0.024,-0.144), rotzyx(-pi/2., pi, 0)),
"pelvis_mesh" : dot(transl(0.031,0.04,0), rotzyx(pi/2.,pi,pi/2.)),
"l_upperleg_mesh": dot(transl(0,0,-.05), rotzyx( pi/2., 0, 0)),
"r_upperleg_mesh": dot(transl(0,0, .05), rotzyx(-pi/2., pi, 0)),
"l_lowerleg_mesh": rotzyx(pi,0, pi/2.),
"r_lowerleg_mesh": rotzyx(pi,0, pi/2.),
"l_foot_mesh" : dot(transl(-0.041,0,-0.03), rotzyx(0, pi/2.,0)),
"r_foot_mesh" : dot(transl(-0.041,0, 0.03), rotzyx(pi,-pi/2.,0)),
"l_upperarm_mesh": dot(transl(0,0.01, 0.14), rotzyx(-pi/2.,0,-pi/2.)),
"r_upperarm_mesh": dot(transl(0,0.01,-0.14), rotzyx(-pi/2.,0,-pi/2.)),
"l_forearm_mesh" : dot(transl(0,0,-0.004), rotzyx(pi,0,-pi/2.)),
"r_forearm_mesh" : dot(transl(0,0, 0.004), rotzyx(0,0,-pi/2.)),
## simple meshes
"waist_mesh" : transl(.006, 0,-.1),
"chest_cylinder_mesh": transl(0, 0, -.0447),
"neck_1_mesh" : eye(4),
"neck_2_mesh" : eye(4),
"head_sphere_mesh": transl(0, 0, .0825),
"l_shoulder_1_mesh": transl(0, 0, .07224),
"l_shoulder_2_mesh": eye(4),
"l_arm_cyl_mesh": transl(0, 0, .078),
"l_elbow_1_mesh": eye(4),
"l_forearm_cyl_mesh": transl(0, 0, .07),
"l_wrist_1_mesh": eye(4),
"l_hand_mesh": transl(.0345, 0, 0),
"r_shoulder_1_mesh": transl(0, 0, -.07224),
"r_shoulder_2_mesh": eye(4),
"r_arm_cyl_mesh": transl(0, 0, -.078),
"r_elbow_1_mesh": eye(4),
"r_forearm_cyl_mesh":transl(0, 0, -.07),
"r_wrist_1_mesh": eye(4),
"r_hand_mesh": transl(-.0345, 0, 0),
"l_hip_1_mesh": transl(0, 0, .0375),
"l_hip_2_mesh": eye(4),
"l_thigh_cyl1_mesh": transl(0, 0, -.112),
"l_thigh_cyl2_mesh": dot(transl(0, 0, -.224), roty(pi/2)),
"l_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)),
"l_ankle_1_mesh": eye(4),
"l_foot_cyl_mesh": transl(0,0,.0125),
"r_hip_1_mesh": transl(0, 0, -.0375),
"r_hip_2_mesh": eye(4),
"r_thigh_cyl1_mesh": transl(0, 0, .112),
"r_thigh_cyl2_mesh": dot(transl(0, 0, .224), roty(pi/2)),
"r_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)),
"r_ankle_1_mesh": eye(4),
"r_foot_cyl_mesh": transl(0,0,-.0125),
}
