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'''
import sys
import motion
import time
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):
# 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)
#####################
## Enable arms control by Walk algorithm
#####################
motionProxy.setWalkArmsEnabled(True, True)
#~ motionProxy.setWalkArmsEnabled(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
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
time.sleep(4.0)
#TARGET VELOCITY
X = 0.8
Y = 0.0
Theta = 0.0
Frequency =1.0 # max speed
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
time.sleep(4.0)
#TARGET VELOCITY
X = 0.2
Y = -0.5
Theta = 0.2
Frequency =1.0
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
time.sleep(2.0)
#####################
## Arms User Motion
#####################
# 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)
time.sleep(2.0)
#####################
## End Walk
#####################
#TARGET VELOCITY
X = 0.0
Y = 0.0
Theta = 0.0
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_walk.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Make NAO walk to an objective.
# -*- encoding: UTF-8 -*-
'''Move To: Small example to make Nao Move To an Objective'''
import math
import almath as m # python's wrapping of almath
import sys
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):
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
# Set NAO in stiffness On
StiffnessOn(motionProxy)
#####################
## Enable arms control by move algorithm
#####################
motionProxy.setWalkArmsEnabled(True, True)
#~ motionProxy.setWalkArmsEnabled(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
print "Robot Move :", robotMove
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_moveTo.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Make NAO walk with greater step length.
# -*- encoding: UTF-8 -*-
''' Walk: Small example to make Nao walk '''
''' Faster (Step of 6cm) '''
import sys
import time
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):
# 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)
# 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)
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
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
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency, [["MaxStepX", 0.06]])
time.sleep(4.0)
print "walk Speed X :",motionProxy.getRobotVelocity()[0]," m/s"
# stop walk on the next double support
motionProxy.stopMove()
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_walkFaster.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
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 '''
import sys
import time
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 = "127.0.0.1"):
# 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)
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# Defined a limp walk
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency,
[ # LEFT FOOT
["StepHeight", 0.02],
["TorsoWy", 5.0*almath.TO_RAD] ],
[ # RIGHT FOOT
["StepHeight", 0.005],
["MaxStepX", 0.001],
["MaxStepFrequency", 0.0],
["TorsoWx", -7.0*almath.TO_RAD],
["TorsoWy", 5.0*almath.TO_RAD] ] )
time.sleep(4.0)
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
time.sleep(4.0)
# stop walk in the next double support
motionProxy.stopMove()
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_walkCustomization.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Make NAO walk to an objective with custom feet gait config.
# -*- encoding: UTF-8 -*-
'''Move To: Small example to make Nao Move To an Objective '''
''' With customization '''
import sys
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):
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
# Set NAO in stiffness On
StiffnessOn(motionProxy)
x = 0.2
y = 0.0
theta = 0.0
# This example show customization for the both foot
# with all the possible gait parameters
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
# 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
motionProxy.moveTo(x, y, theta, [ ["StepHeight", 0.04] ]) # step height of 4 cm
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_moveToCustomization.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
Make NAO walk along a dubins curve.
# -*- encoding: UTF-8 -*-
'''Walk To: Small example to make Nao Walk follow'''
''' a Dubins Curve '''
import sys
import almath as m #python's wrapping of 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):
try:
motionProxy = ALProxy("ALMotion", robotIP, 9559)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
# Set NAO in stiffness On
StiffnessOn(motionProxy)
# 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
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_moveToDubinsCurve.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
# -*- encoding: UTF-8 -*-
''' Walk: Small example to make Nao walk '''
''' with jerky head '''
import sys
import time
import random
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):
# 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", 1.0)
# Initialize the walk process.
# Check the robot pose and take a right posture.
# This is blocking called.
motionProxy.moveInit()
testTime = 10 # seconds
t = 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)
motionProxy.setWalkTargetVelocity(X, Y, Theta, Frequency)
# 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 walk on the next double support
motionProxy.stopMove()
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_walkWithJerkyHead.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)