NAOqi Motion - Overview | API | Tutorial
NAO only - Do not use on Pepper
This tutorial explains how to use whole body Cartesian control API in order to make the robot dance with foot constraints, torso and arms Cartesian commands.
Note
The tutorial is written in Python.
You can download the whole Body multiple effectors control example here: almotion_wbMultipleEffectors.py
For any troubleshooting linked to python, see Python SDK Install Guide section.
In this section we describe each important piece of code of the example.
Then, the proxy to ALMotion module is created. This proxy is useful to called ALMotion API.
''' Whole Body Motion: Multiple Effectors control '''
import argparse
import motion
import almath
import time
from naoqi import ALProxy
def main(robotIP, PORT=9559):
'''
Example of a whole body multiple effectors control "LArm", "RArm" and "Torso"
Warning: Needs a PoseInit before executing
Whole body balancer must be inactivated at the end of the script
'''
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
Here, we specify the whole body constraints:
# Enable Whole Body Balancer
isEnabled = True
motionProxy.wbEnable(isEnabled)
# Legs are constrained fixed
stateName = "Fixed"
supportLeg = "Legs"
motionProxy.wbFootState(stateName, supportLeg)
# Constraint Balance Motion
isEnable = True
supportLeg = "Legs"
motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)
useSensorValues = False
# Arms motion
effectorList = ["LArm", "RArm"]
frame = motion.FRAME_ROBOT
# pathLArm
pathLArm = []
currentTf = motionProxy.getTransform("LArm", frame, useSensorValues)
# 1
target1Tf = almath.Transform(currentTf)
target1Tf.r2_c4 += 0.08 # y
target1Tf.r3_c4 += 0.14 # z
# 2
target2Tf = almath.Transform(currentTf)
target2Tf.r2_c4 -= 0.05 # y
target2Tf.r3_c4 -= 0.07 # z
pathLArm.append(list(target1Tf.toVector()))
pathLArm.append(list(target2Tf.toVector()))
pathLArm.append(list(target1Tf.toVector()))
pathLArm.append(list(target2Tf.toVector()))
pathLArm.append(list(target1Tf.toVector()))
# pathRArm
pathRArm = []
currentTf = motionProxy.getTransform("RArm", frame, useSensorValues)
# 1
target1Tf = almath.Transform(currentTf)
target1Tf.r2_c4 += 0.05 # y
target1Tf.r3_c4 -= 0.07 # z
# 2
target2Tf = almath.Transform(currentTf)
target2Tf.r2_c4 -= 0.08 # y
target2Tf.r3_c4 += 0.14 # z
pathRArm.append(list(target1Tf.toVector()))
pathRArm.append(list(target2Tf.toVector()))
pathRArm.append(list(target1Tf.toVector()))
pathRArm.append(list(target2Tf.toVector()))
pathRArm.append(list(target1Tf.toVector()))
pathRArm.append(list(target2Tf.toVector()))
pathList = [pathLArm, pathRArm]
axisMaskList = [almath.AXIS_MASK_VEL, # for "LArm"
almath.AXIS_MASK_VEL] # for "RArm"
coef = 1.5
timesList = [ [coef*(i+1) for i in range(5)], # for "LArm" in seconds
[coef*(i+1) for i in range(6)] ] # for "RArm" in seconds
# called cartesian interpolation
motionProxy.transformInterpolations(effectorList, frame, pathList, axisMaskList, timesList)
# Torso Motion
effectorList = ["Torso", "LArm", "RArm"]
dy = 0.06
dz = 0.06
# pathTorso
currentTf = motionProxy.getTransform("Torso", frame, useSensorValues)
# 1
target1Tf = almath.Transform(currentTf)
target1Tf.r2_c4 += dy
target1Tf.r3_c4 -= dz
# 2
target2Tf = almath.Transform(currentTf)
target2Tf.r2_c4 -= dy
target2Tf.r3_c4 -= dz
pathTorso = []
for i in range(3):
pathTorso.append(list(target1Tf.toVector()))
pathTorso.append(currentTf)
pathTorso.append(list(target2Tf.toVector()))
pathTorso.append(currentTf)
pathLArm = [motionProxy.getTransform("LArm", frame, useSensorValues)]
pathRArm = [motionProxy.getTransform("RArm", frame, useSensorValues)]
pathList = [pathTorso, pathLArm, pathRArm]
axisMaskList = [almath.AXIS_MASK_ALL, # for "Torso"
almath.AXIS_MASK_VEL, # for "LArm"
almath.AXIS_MASK_VEL] # for "RArm"
coef = 0.5
timesList = [
[coef*(i+1) for i in range(12)], # for "Torso" in seconds
[coef*12], # for "LArm" in seconds
[coef*12] # for "RArm" in seconds
]
motionProxy.transformInterpolations(
effectorList, frame, pathList, axisMaskList, timesList)
This part is essential and stops the whole body balancer.
# Deactivate whole body
isEnabled = False
motionProxy.wbEnable(isEnabled)
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.3)
# Go to rest position
motionProxy.rest()