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: