This section contains examples showing cartesian commands on NAO’s body.
Arm trajectories examples.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Arm trajectory example'''
''' This example is only compatible with NAO '''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
''' Example showing a path of two positions
Warning: Needs a PoseInit before executing
'''
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)
effector = "LArm"
frame = motion.FRAME_TORSO
axisMask = almath.AXIS_MASK_VEL # just control position
useSensorValues = False
path = []
currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
targetTf = almath.Transform(currentTf)
targetTf.r1_c4 += 0.03 # x
targetTf.r2_c4 += 0.03 # y
path.append(list(targetTf.toVector()))
path.append(currentTf)
# Go to the target and back again
times = [2.0, 4.0] # seconds
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Go to rest position
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
# -*- encoding: UTF-8 -*-
'''Cartesian control: Arm trajectory example'''
''' This example is only compatible with NAO '''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
''' Example showing a hand ellipsoid
Warning: Needs a PoseInit before executing
'''
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)
effector = "LArm"
frame = motion.FRAME_TORSO
axisMask = almath.AXIS_MASK_VEL # just control position
useSensorValues = False
path = []
currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
# point 1
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 -= 0.05 # y
path.append(list(targetTf.toVector()))
# point 2
targetTf = almath.Transform(currentTf)
targetTf.r3_c4 += 0.04 # z
path.append(list(targetTf.toVector()))
# point 3
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 += 0.04 # y
path.append(list(targetTf.toVector()))
# point 4
targetTf = almath.Transform(currentTf)
targetTf.r3_c4 -= 0.02 # z
path.append(list(targetTf.toVector()))
# point 5
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 -= 0.05 # y
path.append(list(targetTf.toVector()))
# point 6
path.append(currentTf)
times = [0.5, 1.0, 2.0, 3.0, 4.0, 4.5] # seconds
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Go to rest position
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
Control NAO’s left foot.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Torso and Foot trajectories'''
''' This example is only compatible with NAO '''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
''' Example of a cartesian foot trajectory
Warning: Needs a PoseInit before executing
'''
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)
frame = motion.FRAME_WORLD
axisMask = almath.AXIS_MASK_ALL # full control
useSensorValues = False
# Lower the Torso and move to the side
effector = "Torso"
initTf = almath.Transform(
motionProxy.getTransform(effector, frame, useSensorValues))
deltaTf = almath.Transform(0.0, -0.06, -0.03) # x, y, z
targetTf = initTf*deltaTf
path = list(targetTf.toVector())
times = 2.0 # seconds
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# LLeg motion
effector = "LLeg"
initTf = almath.Transform()
try:
initTf = almath.Transform(motionProxy.getTransform(effector, frame, useSensorValues))
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
# rotation Z
deltaTf = almath.Transform(0.0, 0.04, 0.0)*almath.Transform().fromRotZ(45.0*almath.TO_RAD)
targetTf = initTf*deltaTf
path = list(targetTf.toVector())
times = 2.0 # seconds
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Go to rest position
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
Make NAO’s torso take different positions.
# -*- encoding: UTF-8 -*-
'''Cartesian control: Torso trajectory'''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
''' Example showing a path of five positions
Needs a PoseInit before execution
'''
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)
effector = "Torso"
frame = motion.FRAME_WORLD
axisMask = almath.AXIS_MASK_ALL # full control
useSensorValues = False
# Define the changes relative to the current position
dx = 0.045 # translation axis X (meter)
dy = 0.050 # translation axis Y (meter)
path = []
currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
# point 1
targetTf = almath.Transform(currentTf)
targetTf.r1_c4 += dx
path.append(list(targetTf.toVector()))
# point 2
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 -= dy
path.append(list(targetTf.toVector()))
# point 3
targetTf = almath.Transform(currentTf)
targetTf.r1_c4 -= dx
path.append(list(targetTf.toVector()))
# point 4
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 += dy
path.append(list(targetTf.toVector()))
# point 5
targetTf = almath.Transform(currentTf)
targetTf.r1_c4 += dx
path.append(list(targetTf.toVector()))
# point 6
path.append(currentTf)
times = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # seconds
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Go to rest position
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
# -*- encoding: UTF-8 -*-
'''Motion: Hula Hoop'''
''' This example is only compatible with NAO '''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
'''
Example showing a Hula Hoop Motion
with the NAO cartesian control of torso
'''
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# end initialize proxy, begin go to Stand Init
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
# end go to Stand Init, begin define control point
effector = "Torso"
frame = motion.FRAME_ROBOT
axisMask = almath.AXIS_MASK_ALL
isAbsolute = True
useSensorValues = False
currentTf = almath.Transform(motionProxy.getTransform(effector, frame, useSensorValues))
# end define control point, begin define target
# Define the changes relative to the current position
dx = 0.03 # translation axis X (meter)
dy = 0.03 # translation axis Y (meter)
dwx = 8.0*almath.TO_RAD # rotation axis X (rad)
dwy = 8.0*almath.TO_RAD # rotation axis Y (rad)
# point 01 : forward / bend backward
target1Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
target1Tf *= almath.Transform(dx, 0.0, 0.0)
target1Tf *= almath.Transform().fromRotY(-dwy)
# point 02 : right / bend left
target2Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
target2Tf *= almath.Transform(0.0, -dy, 0.0)
target2Tf *= almath.Transform().fromRotX(-dwx)
# point 03 : backward / bend forward
target3Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
target3Tf *= almath.Transform(-dx, 0.0, 0.0)
target3Tf *= almath.Transform().fromRotY(dwy)
# point 04 : left / bend right
target4Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
target4Tf *= almath.Transform(0.0, dy, 0.0)
target4Tf *= almath.Transform().fromRotX(dwx)
path = []
path.append(list(target1Tf.toVector()))
path.append(list(target2Tf.toVector()))
path.append(list(target3Tf.toVector()))
path.append(list(target4Tf.toVector()))
path.append(list(target1Tf.toVector()))
path.append(list(target2Tf.toVector()))
path.append(list(target3Tf.toVector()))
path.append(list(target4Tf.toVector()))
path.append(list(target1Tf.toVector()))
path.append(list(currentTf.toVector()))
timeOneMove = 0.5 #seconds
times = []
for i in range(len(path)):
times.append((i+1)*timeOneMove)
# end define target, begin call motion api
# call the cartesian control API
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Go to rest position
motionProxy.rest()
# end script
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
Control multiple effectors with cartesian commands.
almotion_cartesianTorsoArm1.py
# -*- encoding: UTF-8 -*-
'''Cartesian control: Multiple Effector Trajectories'''
''' This example is only compatible with NAO '''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
''' Simultaneously control three effectors:
the Torso, the Left Arm and the Right Arm
Warning: Needs a PoseInit before executing
'''
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)
frame = motion.FRAME_WORLD
coef = 0.5 # motion speed
times = [coef, 2.0*coef, 3.0*coef, 4.0*coef]
useSensorValues = False
# Relative movement between current and desired positions
dy = +0.03 # translation axis Y (meters)
dz = -0.03 # translation axis Z (meters)
dwx = +8.0*almath.TO_RAD # rotation axis X (radians)
# Motion of Torso with post process
effector = "Torso"
path = []
initTf = almath.Transform(motionProxy.getTransform(effector, frame, useSensorValues))
# point 1
deltaTf = almath.Transform(0.0, -dy, dz)*almath.Transform().fromRotX(-dwx)
targetTf = initTf*deltaTf
path.append(list(targetTf.toVector()))
# point 2
path.append(list(initTf.toVector()))
# point 3
deltaTf = almath.Transform(0.0, dy, dz)*almath.Transform().fromRotX(dwx)
targetTf = initTf*deltaTf
path.append(list(targetTf.toVector()))
# point 4
path.append(list(initTf.toVector()))
axisMask = almath.AXIS_MASK_ALL # control all the effector axes
motionProxy.post.transformInterpolations(effector, frame, path,
axisMask, times)
# Motion of Arms with block process
frame = motion.FRAME_TORSO
axisMask = almath.AXIS_MASK_VEL # 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 = []
currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 -= 0.04 # y
path.append(list(targetTf.toVector()))
path.append(currentTf)
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Motion of Left Arm during the last half of the Torso motion
effector = "LArm"
path = []
currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 += 0.04 # y
path.append(list(targetTf.toVector()))
path.append(currentTf)
motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
# Go to rest position
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
almotion_cartesianTorsoArm2.py
# -*- encoding: UTF-8 -*-
'''Cartesian control: Multiple Effector Trajectories'''
''' This example is only compatible with NAO '''
import argparse
import motion
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
''' Move the torso and keep arms fixed in nao space
Warning: Needs a PoseInit before executing
'''
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)
frame = motion.FRAME_ROBOT
useSensorValues = 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
# LArm path
pathLArm = []
targetTf = almath.Transform(motionProxy.getTransform("LArm", frame, useSensorValues))
targetTf.r2_c4 -= 0.04 # y
pathLArm.append(list(targetTf.toVector()))
# RArm path
pathRArm = []
targetTf = almath.Transform(motionProxy.getTransform("RArm", frame, useSensorValues))
targetTf.r2_c4 += 0.04 # y
pathRArm.append(list(targetTf.toVector()))
pathList = []
pathList.append(pathLArm)
pathList.append(pathRArm)
motionProxy.transformInterpolations(effectorList, frame, pathList,
axisMaskList, timesList)
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
# LArm path
pathLArm = []
pathLArm.append(motionProxy.getTransform("LArm", frame, useSensorValues))
# RArm path
pathRArm = []
pathRArm.append(motionProxy.getTransform("RArm", frame, useSensorValues))
# Torso path
pathTorso = []
currentTf = motionProxy.getTransform("Torso", frame, useSensorValues)
# 1
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 += 0.04 # y
pathTorso.append(list(targetTf.toVector()))
# 2
targetTf = almath.Transform(currentTf)
targetTf.r1_c4 -= 0.03 # x
pathTorso.append(list(targetTf.toVector()))
# 3
targetTf = almath.Transform(currentTf)
targetTf.r2_c4 -= 0.04 # y
pathTorso.append(list(targetTf.toVector()))
# 4
pathTorso.append(currentTf)
pathList = []
pathList.append(pathLArm)
pathList.append(pathRArm)
pathList.append(pathTorso)
motionProxy.transformInterpolations(effectorList, frame, pathList,
axisMaskList, timesList)
# Go to rest position
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)