This section contains several python example on walk, both for standard walk and for customized walk.
These examples show how to use Aldebaran normal walk.
Make NAO walk backwards, forwards, and turn.
# -*- encoding: UTF-8 -*-
'''Walk: Small example to make Nao walk'''
''' This example is only compatible with NAO '''
import argparse
import motion
import time
import almath
from naoqi import ALProxy
def userArmsCartesian(motionProxy):
effector = ["LArm", "RArm"]
frame = motion.FRAME_TORSO
useSensorValues = False
# just control position
axisMask = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
# LArm path
pathLArm = []
initTf = almath.Transform(motionProxy.getTransform("LArm", frame, useSensorValues))
targetTf = almath.Transform(motionProxy.getTransform("LArm", frame, useSensorValues))
targetTf.r1_c4 += 0.04 # x
targetTf.r2_c4 -= 0.10 # y
targetTf.r3_c4 += 0.10 # z
pathLArm.append(list(targetTf.toVector()))
pathLArm.append(list(initTf.toVector()))
pathLArm.append(list(targetTf.toVector()))
pathLArm.append(list(initTf.toVector()))
# RArm path
pathRArm = []
initTf = almath.Transform(motionProxy.getTransform("RArm", frame, useSensorValues))
targetTf = almath.Transform(motionProxy.getTransform("RArm", frame, useSensorValues))
targetTf.r1_c4 += 0.04 # x
targetTf.r2_c4 += 0.10 # y
targetTf.r3_c4 += 0.10 # z
pathRArm.append(list(targetTf.toVector()))
pathRArm.append(list(initTf.toVector()))
pathRArm.append(list(targetTf.toVector()))
pathRArm.append(list(initTf.toVector()))
pathList = []
pathList.append(pathLArm)
pathList.append(pathRArm)
# Go to the target and back again
timesList = [[1.0, 2.0, 3.0, 4.0],
[1.0, 2.0, 3.0, 4.0]] # seconds
motionProxy.transformInterpolations(effector, frame, pathList,
axisMask, timesList)
def userArmArticular(motionProxy):
# Arms motion from user have always the priority than walk arms motion
JointNames = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll"]
Arm1 = [-40, 25, 0, -40]
Arm1 = [ x * motion.TO_RAD for x in Arm1]
Arm2 = [-40, 50, 0, -80]
Arm2 = [ x * motion.TO_RAD for x in Arm2]
pFractionMaxSpeed = 0.6
motionProxy.angleInterpolationWithSpeed(JointNames, Arm1, pFractionMaxSpeed)
motionProxy.angleInterpolationWithSpeed(JointNames, Arm2, pFractionMaxSpeed)
motionProxy.angleInterpolationWithSpeed(JointNames, Arm1, pFractionMaxSpeed)
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand
postureProxy.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by Motion algorithm
#####################
motionProxy.setMoveArmsEnabled(True, True)
# motionProxy.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#TARGET VELOCITY
X = -0.5 # backward
Y = 0.0
Theta = 0.0
Frequency =0.0 # low speed
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
userArmsCartesian(motionProxy)
#TARGET VELOCITY
X = 0.8
Y = 0.0
Theta = 0.0
Frequency =1.0 # max speed
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
time.sleep(4.0)
#TARGET VELOCITY
X = 0.2
Y = -0.5
Theta = 0.2
Frequency = 1.0
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
time.sleep(2.0)
userArmArticular(motionProxy)
time.sleep(2.0)
#####################
## End Walk
#####################
#TARGET VELOCITY
X = 0.0
Y = 0.0
Theta = 0.0
motionProxy.moveToward(X, Y, Theta)
motionProxy.waitUntilMoveIsFinished()
# 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 walk to an objective.
# -*- encoding: UTF-8 -*-
'''Move To: Small example to make Nao Move To an Objective'''
import argparse
import math
import almath as m # python's wrapping of almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
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)
#####################
## Enable arms control by move algorithm
#####################
motionProxy.setMoveArmsEnabled(True, True)
#~ motionProxy.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = m.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# 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 walk with greater step length.
# -*- encoding: UTF-8 -*-
''' Walk: Small example to make Nao walk '''
''' Faster (Step of 6cm) '''
''' This example is only compatible with NAO '''
import argparse
import time
from naoqi import ALProxy
def main(robotIP, PORT=9559):
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)
# Initialize the walk process.
# Check the robot pose and take a right posture.
# This is blocking called.
motionProxy.moveInit()
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# Default walk (MaxStepX = 0.04 m)
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
time.sleep(3.0)
print "walk Speed X :",motionProxy.getRobotVelocity()[0]," m/s"
# Speed walk (MaxStepX = 0.06 m)
# Could be faster: see walk documentation
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency],
["MaxStepX", 0.06]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
time.sleep(4.0)
print "walk Speed X :",motionProxy.getRobotVelocity()[0]," m/s"
# stop walk on the next double support
motionProxy.stopMove()
# 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 do a custom walk: NAO is limping and then walking correctly, just like Keyser Sose in The Usual Suspects.
# -*- encoding: UTF-8 -*-
''' Walk: Small example to make Nao walk '''
''' with gait customization '''
''' NAO is Keyser Soze '''
''' This example is only compatible with NAO '''
import argparse
import time
import almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
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)
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# Defined a limp walk
try:
motionProxy.moveToward(X, Y, Theta,[["Frequency", Frequency],
# LEFT FOOT
["LeftStepHeight", 0.02],
["LeftTorsoWy", 5.0*almath.TO_RAD],
# RIGHT FOOT
["RightStepHeight", 0.005],
["RightMaxStepX", 0.001],
["RightMaxStepFrequency", 0.0],
["RightTorsoWx", -7.0*almath.TO_RAD],
["RightTorsoWy", 5.0*almath.TO_RAD]] )
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
time.sleep(4.0)
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
time.sleep(4.0)
# stop walk in the next double support
motionProxy.stopMove()
# 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 walk to an objective with custom feet gait config.
almotion_moveToCustomization.py
# -*- encoding: UTF-8 -*-
'''Move To: Small example to make Nao Move To an Objective '''
''' With customization '''
''' This example is only compatible with NAO '''
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
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)
x = 0.2
y = 0.0
theta = 0.0
# This example show customization for the both foot
# with all the possible gait parameters
try:
motionProxy.moveTo(x, y, theta,
[ ["MaxStepX", 0.02], # step of 2 cm in front
["MaxStepY", 0.16], # default value
["MaxStepTheta", 0.4], # default value
["MaxStepFrequency", 0.0], # low frequency
["StepHeight", 0.01], # step height of 1 cm
["TorsoWx", 0.0], # default value
["TorsoWy", 0.1] ]) # torso bend 0.1 rad in front
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
# This example show customization for the both foot
# with just one gait parameter, in this case, the other
# parameters are set to the default value
try:
motionProxy.moveTo(x, y, theta, [ ["StepHeight", 0.04] ]) # step height of 4 cm
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
# 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 walk along a dubins curve.
# -*- encoding: UTF-8 -*-
'''Walk To: Small example to make Nao Walk follow'''
''' a Dubins Curve '''
import argparse
import almath as m #python's wrapping of almath
from naoqi import ALProxy
def main(robotIP, PORT=9559):
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)
# first we defined the goal
goal = m.Pose2D(0.0, -0.4, 0.0)
# We get the dubins solution (control points) by
# calling an almath function
circleRadius = 0.04
# Warning : the circle use by dubins curve
# have to be 4*CircleRadius < norm(goal)
dubinsSolutionAbsolute = m.getDubinsSolutions(goal, circleRadius)
# moveTo With control Points use relative commands but
# getDubinsSolution return absolute position
# So, we compute dubinsSolution in relative way
dubinsSolutionRelative = []
dubinsSolutionRelative.append(dubinsSolutionAbsolute[0])
for i in range(len(dubinsSolutionAbsolute)-1):
dubinsSolutionRelative.append(
dubinsSolutionAbsolute[i].inverse() *
dubinsSolutionAbsolute[i+1])
# create a vector of moveTo with dubins Control Points
moveToTargets = []
for i in range(len(dubinsSolutionRelative)):
moveToTargets.append(
[dubinsSolutionRelative[i].x,
dubinsSolutionRelative[i].y,
dubinsSolutionRelative[i].theta] )
# Initialized the Move process and be sure the robot is ready to move
# without this call, the first getRobotPosition() will not refer to the position
# of the robot before the move process
motionProxy.moveInit()
# get robot position before move
robotPositionBeforeCommand = m.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.moveTo( moveToTargets )
# With MoveTo control Points, it's also possible to customize the gait parameters
# motionProxy.moveTo(moveToTargets,
# [["StepHeight", 0.001],
# ["MaxStepX", 0.06],
# ["MaxStepFrequency", 1.0]])
# get robot position after move
robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = m.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
print "The Robot Move Command: ", robotMoveCommand
# 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 -*-
''' Walk: Small example to make Nao walk '''
''' with jerky head '''
''' This example is only compatible with NAO '''
import argparse
import time
import random
from naoqi import ALProxy
def main(robotIP, PORT=9559):
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)
# Initialize the move process.
# Check the robot pose and take a right posture.
# This is blocking called.
motionProxy.moveInit()
testTime = 10.0 # seconds
t = 0.0
dt = 0.2
while t<testTime:
# WALK
X = random.uniform(0.4, 1.0)
Y = random.uniform(-0.4, 0.4)
Theta = random.uniform(-0.4, 0.4)
Frequency = random.uniform(0.5, 1.0)
try:
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
# JERKY HEAD
motionProxy.setAngles("HeadYaw", random.uniform(-1.0, 1.0), 0.6)
motionProxy.setAngles("HeadPitch", random.uniform(-0.5, 0.5), 0.6)
t = t + dt
time.sleep(dt)
# stop move on the next double support
motionProxy.stopMove()
# 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)