This section contains examples showing cartesian commands on NAO’s body.
Arm trajectories examples.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Arm trajectory example'''
import sys
import motion
import almath
from naoqi import ALProxy
def StiffnessOn(proxy):
#We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
''' Example showing a path of two positions
Warning: Needs a PoseInit before executing
'''
# Init proxies.
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
effector = "LArm"
space = motion.FRAME_ROBOT
axisMask = almath.AXIS_MASK_VEL # 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.03 # translation axis X (meters)
dy = 0.03 # 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
motionProxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_cartesianArm1.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
# -*- encoding: UTF-8 -*-
'''Cartesian control: Arm trajectory example'''
import sys
import motion
from naoqi import ALProxy
def StiffnessOn(proxy):
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
''' Example showing a hand ellipsoid
Warning: Needs a PoseInit before executing
'''
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
effector = "LArm"
space = motion.FRAME_ROBOT
path = [
[0.0, -0.05, +0.00, 0.0, 0.0, 0.0], # point 1
[0.0, +0.00, +0.04, 0.0, 0.0, 0.0], # point 2
[0.0, +0.04, +0.00, 0.0, 0.0, 0.0], # point 3
[0.0, +0.00, -0.02, 0.0, 0.0, 0.0], # point 4
[0.0, -0.05, +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
motionProxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_cartesianArm2.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Control NAO’s left foot.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Torso and Foot trajectories'''
import sys
import motion
import almath
from naoqi import ALProxy
def StiffnessOn(proxy):
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
''' Example of a cartesian foot trajectory
Warning: Needs a PoseInit before executing
'''
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
space = motion.FRAME_ROBOT
axisMask = almath.AXIS_MASK_ALL # full control
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
motionProxy.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
motionProxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_cartesianFoot.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Make NAO’s torso take different positions.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Torso trajectory'''
import sys
import motion
import almath
from naoqi import ALProxy
def StiffnessOn(proxy):
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
''' Example showing a path of five positions
Needs a PoseInit before execution
'''
# Init proxies.
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
effector = "Torso"
space = motion.FRAME_WORLD
# Position Only
axisMask = almath.AXIS_MASK_ALL # full control
isAbsolute = False
# Define the changes relative to the current position
dx = 0.045 # translation axis X (meter)
dy = 0.050 # 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
motionProxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_cartesianTorso.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
# -*- encoding: UTF-8 -*-
import sys
import motion
import almath
from naoqi import ALProxy
def StiffnessOn(proxy):
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
'''
Example showing a Hula Hoop Motion
with the NAO cartesian control of torso
'''
# Init proxies.
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Define the changes relative to the current position
dx = 0.07 # translation axis X (meter)
dy = 0.07 # translation axis Y (meter)
dwx = 0.15 # rotation axis X (rad)
dwy = 0.15 # rotation axis Y (rad)
# Define a path of two hula hoop loops
path = [ [+dx, 0.0, 0.0, 0.0, -dwy, 0.0], # point 01 : forward / bend backward
[0.0, -dy, 0.0, -dwx, 0.0, 0.0], # point 02 : right / bend left
[-dx, 0.0, 0.0, 0.0, dwy, 0.0], # point 03 : backward / bend forward
[0.0, +dy, 0.0, dwx, 0.0, 0.0], # point 04 : left / bend right
[+dx, 0.0, 0.0, 0.0, -dwy, 0.0], # point 01 : forward / bend backward
[0.0, -dy, 0.0, -dwx, 0.0, 0.0], # point 02 : right / bend left
[-dx, 0.0, 0.0, 0.0, dwy, 0.0], # point 03 : backward / bend forward
[0.0, +dy, 0.0, dwx, 0.0, 0.0], # point 04 : left / bend right
[+dx, 0.0, 0.0, 0.0, -dwy, 0.0], # point 05 : forward / bend backward
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ] # point 06 : Back to init pose
timeOneMove = 0.4 #seconds
times = []
for i in range(len(path)):
times.append( (i+1)*timeOneMove )
# call the cartesian control API
effector = "Torso"
space = motion.FRAME_ROBOT
axisMask = almath.AXIS_MASK_ALL
isAbsolute = False
motionProxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_hulaHoop.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Control multiple effectors with cartesian commands.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Multiple Effector Trajectories'''
import sys
import motion
import almath
from naoqi import ALProxy
def StiffnessOn(proxy):
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
''' Simultaneously control three effectors:
the Torso, the Left Arm and the Right Arm
Warning: Needs a PoseInit before executing
'''
# Init proxies.
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send Robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
space = motion.FRAME_ROBOT
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 = almath.AXIS_MASK_ALL # control all the effector axes
motionProxy.post.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
# Motion of Arms with block process
axisMask = almath.AXIS_MASK_VEL # control just the position
times = [1.0*coef, 2.0*coef] # seconds
dy = +0.03 # translation axis Y (meters)
# 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
motionProxy.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
motionProxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_cartesianTorsoArm1.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
# -*- encoding: UTF-8 -*-
'''Cartesian control: Multiple Effector Trajectories'''
import sys
import motion
import almath
from naoqi import ALProxy
def StiffnessOn(proxy):
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP):
''' Move the torso and keep arms fixed in nao space
Warning: Needs a PoseInit before executing
'''
# Init proxies.
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
space = motion.FRAME_ROBOT
isAbsolute = False
effectorList = ["LArm", "RArm"]
# Motion of Arms with block process
axisMaskList = [almath.AXIS_MASK_VEL, almath.AXIS_MASK_VEL]
timesList = [[1.0], [1.0]] # seconds
pathList = [[[0.0, -0.04, 0.0, 0.0, 0.0, 0.0]],
[[0.0, 0.04, 0.0, 0.0, 0.0, 0.0]]]
motionProxy.positionInterpolations(effectorList, space, pathList,
axisMaskList, timesList, isAbsolute)
effectorList = ["LArm", "RArm", "Torso"]
# Motion of Arms and Torso with block process
axisMaskList = [almath.AXIS_MASK_VEL,
almath.AXIS_MASK_VEL,
almath.AXIS_MASK_ALL]
timesList = [[4.0], # LArm in seconds
[4.0], # RArm in seconds
[1.0, 2.0, 3.0, 4.0]] # Torso in seconds
dx = 0.03 # translation axis X (meters)
dy = 0.04 # translation axis Y (meters)
pathList = [[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # LArm do not move
[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # RArm do not move
[[0.0, +dy, 0.0, 0.0, 0.0, 0.0], # Torso point 1
[-dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Torso point 2
[0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Torso point 3
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Torso point 4
]
motionProxy.positionInterpolations(effectorList, space, pathList,
axisMaskList, timesList, isAbsolute)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_cartesianTorsoArm2.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)