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