def _calculate_target_tf(self, effector, delta_tf, frame): """Calculate the target transform. Calculate the target transform from the current position of the effector and the relative position change. Parameters ---------- effector : str String of effector name. delta_tf : ndarray[float] The relative position change. frame : {FRAME_TORSO, FRAME_WORLD, FRAME_ROBOT} The task frame . """ init_tf = almath.Transform(self._motion_proxy.getTransform(effector, frame, self._use_sensor_values)) delta_tf = listify(delta_tf) d = delta_tf.pop(0) target_tf = init_tf * almath.Transform().fromPosition(*d) path = list(target_tf.toVector()) if delta_tf: path = [path] for d in delta_tf: target_tf *= almath.Transform().fromPosition(*d) path.append(list(target_tf.toVector())) return path
def _calculate_target_pos(self, effector, delta_pos, frame): """Calculate the target position. Calculate the target position from the current position of the effector and the relative position change. Parameters ---------- effector : str String of effector name. delta_pos : ndarray[float] The relative position change. frame : {FRAME_TORSO, FRAME_WORLD, FRAME_ROBOT} The task frame . """ init_pos = self._motion_proxy.getPosition(effector, frame, self._use_sensor_values) delta_pos = listify(delta_pos) d = delta_pos.pop(0) target_pos = almath.Position6D(init_pos) target_pos += almath.Position6D(d) path = list(target_pos.toVector()) if delta_pos: path = [path] for d in delta_pos: target_pos += almath.Position6D(d) path.append(list(target_pos.toVector())) return path
def interpolate_positions(self, effectors, delta_pos, times, frames=None, axis_masks=None): """Interpolate end-effectors to target positions and orientations. Moves end-effectors to the given position and orientation over time. This encapsulates the function `positionInterpolation <http://doc.aldebaran.com/2-1/naoqi/motion/control-cartesian-api.html#almotionproxy-positioninterpolations1>`_ from the ALMotion module. Parameters ---------- effectors : str or list[str] List of effector names. Effector name could be "Torso" or chain name. delta_pos : ndarray[float] or list[ndarray[float]] or list[list[ndarray[float]]] The relative position changes for each effector. times : list[float] or list[list[float]] Vector of times in seconds corresponding to the path points. frames : list[int], optional List of task frames {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2 }. Default is FRAME_ROBOT. axis_masks : list[int] List of axis masks. True for axes that should be controlled. Default is AXIS_MASK_ALL. Notes ----- This is a non-blocking call. """ effectors = listify(effectors) n = len(effectors) frames = listify(frames) if frames is not None else [NaoMotionController.FRAME_ROBOT] * n assert (len(frames) == n), "effectors and frames must have the same number of elements" axis_masks = listify(axis_masks) if axis_masks is not None else [NaoMotionController.AXIS_MASK_ALL] * n assert (len(axis_masks) == n), "effectors and axis_masks must have the same number of elements" try: path = [] for e, d, f in zip(effectors, listify(delta_pos), frames): path.append(self._calculate_target_pos(e, d, f)) self._task_id = self._motion_proxy.post.positionInterpolations(effectors, frames, path, axis_masks, times) except: exc_type, exc_value, exc_traceback = sys.exc_info() traceback.print_exception(exc_type, exc_value, exc_traceback) sys.exit(1)