SE3 Interpolation
SE3 Interpolation is used for all interpolations that are defined in Cartesian Space. It provides a spline-like interpolation which allows for initial speeds and points of passage to be taken into account, ensuring smooth trajectories that respect speed constraints.
| Warning: |
If the desired motion is unfeasible, the robot can loose balance and fall. All Cartesian motions should be tested in a simulator before being tried on the robot. |
|---|
Arm trajectory examples
This example show a simple path composed of two control points, the target and the current position. It uses relative coordinates, so the current position is all zeros.
# Cartesian control: Arm trajectory example # Example showing a path of two positions # Needs a PoseInit before executing # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_cartesianArm1.py effector = "LArm" space = motion.SPACE_NAO axisMask = 7 # just control position isAbsolute = False # Since we are in relative, the current position is zero currentPos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # define the changes relative to the current position dx = 0.06 # translation axis X (meters) dy = 0.06 # translation axis Y (meters) dz = 0.00 # translation axis Z (meters) dwx = 0.00 # rotation axis X (radians) dwy = 0.00 # rotation axis Y (radians) dwz = 0.00 # rotation axis Z (radians) targetPos = [dx, dy, dz, dwx, dwy, dwz] # go to the target and back again path = [targetPos, currentPos] times = [2.0, 4.0] # seconds proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
# Cartesian control: Arm trajectory example # Example showing a hand ellipsoid # Needs a PoseInit before executing # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_cartesianArm2.py effector = "LArm" space = motion.SPACE_NAO path = [ [0.0, -0.02, +0.00, 0.0, 0.0, 0.0], # Point 1 [0.0, +0.00, +0.01, 0.0, 0.0, 0.0], # Point 2 [0.0, +0.08, +0.00, 0.0, 0.0, 0.0], # Point 3 [0.0, +0.00, -0.04, 0.0, 0.0, 0.0], # Point 4 [0.0, -0.02, +0.00, 0.0, 0.0, 0.0], # Point 5 [0.0, +0.00, +0.00, 0.0, 0.0, 0.0]] # Point 6 axisMask = 7 # just control position times = [0.5, 1.0, 2.0, 3.0, 4.0, 4.5] # seconds isAbsolute = False proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
Torso and Foot trajectories
This example shows how to do simultaneous trajectories of the Torso and the legs. The non-controlled right leg, will behave as if it received a zero relative command.
- Lower the Torso and move to a point above the Right Leg
- Move and turn the Left Foot outwards, with an upright intermediate position.
# Cartesian control: Torso and Foot trajectories # Example of a cartesian foot trajectory # Needs a PoseInit before executing # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_cartesianFoot.py space = motion.SPACE_NAO axisMask = 63 # control all the effector's axes isAbsolute = False # Lower the Torso and move to the side effector = "Torso" path = [0.0, -0.07, -0.03, 0.0, 0.0, 0.0] time = 2.0 # seconds proxy.positionInterpolation(effector, space, path, axisMask, time, isAbsolute) # LLeg motion effector = "LLeg" path = [0.0, 0.06, 0.00, 0.0, 0.0, 0.8] times = 2.0 # seconds proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
Torso trajectory
This example shows a torso trajectory comprised of several target position, creating a kind of hula-hoop motion.
# Cartesian control: Torso trajectory # Example showing a path of five positions # Needs a PoseInit before execution # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_cartesianTorso.py effector = "Torso" space = motion.SPACE_NAO # Position Only axisMask = 63 # Full control isAbsolute = False # define the changes relative to the current position dx = 0.050 # translation axis X (meter) dy = 0.055 # translation axis Y (meter) path = [ [+dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 1 [0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 2 [-dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 3 [0.0, +dy, 0.0, 0.0, 0.0, 0.0], # Point 4 [+dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 5 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 6 times = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # seconds proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
Multiple Effector Trajectories
The goal of this example is to simultaneously control three effectors: the Torso, the Left Arm and the Right Arm.
- Torso motion: non-blocking method;
- Right Arm motion: blocking method during the first half of torso motion;
- Left Arm motion: blocking method during the last half of the torso motion.
# Cartesian control: Multiple Effector Trajectories # Simultaneously control three effectors: # the Torso, the Left Arm and the Right Arm # Needs a PoseInit before executing # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_cartesianTorsoArm1.py space = motion.SPACE_NAO coef = 0.5 # motion speed times = [coef, 2.0*coef, 3.0*coef, 4.0*coef] isAbsolute = False # Relative movement between current and desired positions dy = +0.06 # translation axis Y (meters) dz = -0.03 # translation axis Z (meters) dwx = +0.30 # rotation axis X (radians) # Motion of Torso with post process effector = "Torso" path = [ [0.0, -dy, dz, -dwx, 0.0, 0.0], # Point 1 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 2 [0.0, +dy, dz, +dwx, 0.0, 0.0], # Point 3 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 4 axisMask = 63 # control all the effector axes proxy.post.positionInterpolation(effector, space, path, axisMask, times, isAbsolute) # Motion of Arms with block process axisMask = 7 # control just the position times = [1.0*coef, 2.0*coef] # seconds # Motion of Right Arm during the first half of the Torso motion effector = "RArm" path = [ [0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 1 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 2 proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute) # Motion of Left Arm during the last half of the Torso motion effector = "LArm" path = [ [0.0, dy, 0.0, 0.0, 0.0, 0.0], # Point 1 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 2 proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
The goal of this example is to move the torso and keep arms fixed in world space:
# Cartesian control: Multiple Effector Trajectories # Move the torso and keep arms fixed in world space # Needs a PoseInit before executing # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_cartesianTorsoArm2.py space = motion.SPACE_NAO coef = 1.0 # Speed motion times = [coef, 2.0*coef, 3.0*coef, 4.0*coef] # seconds isAbsolute = False dx = 0.04 # translation axis X (meters) dy = 0.04 # translation axis Y (meters) # Motion of the Torso effector = "Torso" path = [ [ 0.0, +dy, 0.0, 0.0, 0.0, 0.0], # Point 1 [ -dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 2 [ 0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 3 [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 4 axisMask = 63 # control all the effector axis # Using 'post' makes this command execute in a another thread proxy.post.positionInterpolation(effector, space, path, axisMask, times, isAbsolute) # Motion of Arms axisMask = 7 # control the position time = 4.0*coef # seconds # Arms have a null relative motion during all the torso motion path = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Point 1 # Motion of Right Arm effector = "RArm" proxy.post.positionInterpolation(effector, space, path, axisMask, time, isAbsolute) # Motion of Left Arm effector = "LArm" proxy.positionInterpolation(effector, space, path, axisMask, time, isAbsolute)