def retarget(self,source_joint_angles): '''returns the retargeted target joint angles given the source joint angles. ''' angles_matrix = np.matrix(source_joint_angles) self.source = mlab.setJointAngles(self.source, angles_matrix) self.target = mlab.easy_retarget(self.source, self.target) return self.target.joints.angles
def build_chain(chain_file): ''' builds and returns a matlab chain object according to the specifications in the file.''' f = open(chain_file, 'ro') lengths = eval(f.readline()) lengths = np.matrix(lengths) chain = mlab.easy_chain_builder(lengths) z = lengths*0 #zero matrix of same dimension as lengths chain = mlab.setJointAngles(chain,z) return chain
# human24(lengths) returns a 2-link, 4-dof chain similar to a human arm. the # lengths vector specifies link lengths. # Degrees of freedom: # [shoulder_roll, shoulder_yaw, shoulder_pitch, elbow_pitch] # note: don't use the roll dof, it doesn't work (sorry!) lengths = np.matrix([L/2, L/2]) # Note that we must use numpy data structures. human = mlab.human24(lengths) # ----------------------------------------------------------- # # Manipulating and Visualizing Chains: # ----------------------------------------------------------- # # Manipulate chain pose with setJointAngles(chain, angles) pr2_initial_angles = np.matrix([0,0,0,0]) pr2 = mlab.setJointAngles(pr2, pr2_initial_angles) human_initial_angles = np.matrix([0,-110,40,-60]) human = mlab.setJointAngles(human, human_initial_angles) # Visualize chains with the draw() function. mlab.figure() mlab.grid('on') mlab.headSphere() #creates a dummy head that provides context. mlab.draw(pr2,'k') mlab.draw(human) # ----------------------------------------------------------- # # Retargeting Chains: