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), }