def executeMove(self, moves, interp_type = INTERPOLATION_SMOOTH, description=('executemove',), headIncluded=True):
""" Go through a list of body angles, works like northern bites code:
moves is a list, each item contains:
head (the only optional, tuple of 2), larm (tuple of 4), lleg (tuple of 6), rleg, rarm, interp_time
interp_type - 1 for SMOOTH, 0 for Linear
interp_time - time in seconds for interpolation
NOTE: currently this is SYNCHRONOUS - it takes at least
sum(interp_time) to execute.
"""
if len(moves[0]) == 6:
if headIncluded:
def getangles((head, larm, lleg, rleg, rarm, interp_time)):
return list(head) + list(larm) + [0.0, 0.0] + list(lleg) + list(rleg) + list(rarm) + [0.0, 0.0]
joints = self._joint_names
else:
def getangles((head, larm, lleg, rleg, rarm, interp_time)):
return list(larm) + [0.0, 0.0] + list(lleg) + list(rleg) + list(rarm) + [0.0, 0.0]
joints = self._joint_names[2:]
else:
def getangles((larm, lleg, rleg, rarm, interp_time)):
return list(larm) + [0.0, 0.0] + list(lleg) + list(rleg) + list(rarm) + [0.0, 0.0]
joints = self._joint_names[2:]
n_joints = len(joints)
angles_matrix = transpose([[x for x in getangles(move)] for move in moves])
durations_matrix = [list(cumsum(move[-1] for move in moves))] * n_joints
#print repr((joints, angles_matrix, durations_matrix))
self._current_motion_bd = self._movecoordinator.doMove(joints, angles_matrix, durations_matrix,
interp_type, description = description)
return self._current_motion_bd
def executeHeadMove(self, moves, interp_type = INTERPOLATION_SMOOTH, description=('headmove',)):
""" Go through a list of head angles
moves is a list, each item contains:
head (tuple of 2), interp_time
interp_type - 1 for SMOOTH, 0 for Linear
interp_time - time in seconds for interpolation
NOTE: this is ASYNCHRONOUS
"""
joints = self._joint_names[:2]
n_joints = len(joints)
angles_matrix = [[angles[i] for angles, interp_time in moves] for i in xrange(n_joints)]
durations_matrix = [list(cumsum(interp_time for angles, interp_time in moves))] * n_joints
#print repr((joints, angles_matrix, durations_matrix))
#print "executeHeadMove: duration = %s" % duration
self._current_head_bd = self._movecoordinator.doMove(
joints, angles_matrix, durations_matrix, interp_type, description=description)
return self._current_head_bd
