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)
