Move and Walk¶
This section contains several python example on walk, both for standard walk and for customized walk.
Simple walk¶
These examples show how to use SoftBank Robotics normal walk.
Walk¶
Make NAO walk backwards, forwards, and turn.
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Walk - Small example to make Nao walk"""
import qi
import argparse
import sys
import motion
import time
import almath
def userArmsCartesian(motion_service):
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(motion_service.getTransform("LArm", frame, useSensorValues))
targetTf = almath.Transform(motion_service.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(motion_service.getTransform("RArm", frame, useSensorValues))
targetTf = almath.Transform(motion_service.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
motion_service.transformInterpolations(effector, frame, pathList,
axisMask, timesList)
def userArmArticular(motion_service):
# 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
motion_service.angleInterpolationWithSpeed(JointNames, Arm1, pFractionMaxSpeed)
motion_service.angleInterpolationWithSpeed(JointNames, Arm2, pFractionMaxSpeed)
motion_service.angleInterpolationWithSpeed(JointNames, Arm1, pFractionMaxSpeed)
def main(session):
"""
Walk - Small example to make Nao walk
This example is only compatible with NAO
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand
posture_service.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by Motion algorithm
#####################
motion_service.setMoveArmsEnabled(True, True)
# motion_service.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#TARGET VELOCITY
X = -0.5 # backward
Y = 0.0
Theta = 0.0
Frequency =0.0 # low speed
try:
motion_service.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
userArmsCartesian(motion_service)
#TARGET VELOCITY
X = 0.8
Y = 0.0
Theta = 0.0
Frequency =1.0 # max speed
try:
motion_service.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:
motion_service.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(motion_service)
time.sleep(2.0)
#####################
## End Walk
#####################
#TARGET VELOCITY
X = 0.0
Y = 0.0
Theta = 0.0
motion_service.moveToward(X, Y, Theta)
motion_service.waitUntilMoveIsFinished()
# Go to rest position
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
Move to¶
Make NAO walk to an objective.
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Move To - Small example to make Nao Move To an Objective"""
import qi
import argparse
import sys
import math
import almath
def main(session):
"""
Move To: Small example to make Nao Move To an Objective.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motion_service.setMoveArmsEnabled(True, True)
#~ motion_service.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motion_service.moveTo(X, Y, Theta, _async=True)
# wait is useful because with _async moveTo is not blocking function
motion_service.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
Walk faster¶
Make NAO walk with greater step length.
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Walk - Small example to make Nao walk"""
import qi
import argparse
import sys
import time
def main(session):
"""
Walk: Small example to make Nao walk Faster (Step of 6cm).
This example is only compatible with NAO.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
# Initialize the walk process.
# Check the robot pose and take a right posture.
# This is blocking called.
motion_service.moveInit()
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# Default walk (MaxStepX = 0.04 m)
try:
motion_service.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 :",motion_service.getRobotVelocity()[0]," m/s"
# Speed walk (MaxStepX = 0.06 m)
# Could be faster: see walk documentation
try:
motion_service.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 :",motion_service.getRobotVelocity()[0]," m/s"
# stop walk on the next double support
motion_service.stopMove()
# Go to rest position
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
Customized walk¶
Walk customization¶
Make NAO do a custom walk: NAO is limping and then walking correctly, just like Keyser Sose in The Usual Suspects.
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Walk - Small example to make Nao walk"""
import qi
import argparse
import sys
import time
import almath
def main(session):
"""
Walk: Small example to make Nao walk with gait customization.
NAO is Keyser Soze.
This example is only compatible with NAO.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# Defined a limp walk
try:
motion_service.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:
motion_service.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
motion_service.stopMove()
# Go to rest position
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
Walk to customization¶
Make NAO walk to an objective with custom feet gait config.
almotion_moveToCustomization.py
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Move To - Small example to make Nao Move To an Objective"""
import qi
import argparse
import sys
def main(session):
"""
Move To - Small example to make Nao Move To an Objective.
This example is only compatible with NAO.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.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:
motion_service.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:
motion_service.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
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
Dubins curve¶
Make NAO walk along a dubins curve.
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Walk To - Small example to make Nao Walk follow a Dubin Curve"""
import qi
import argparse
import sys
import almath
def main(session):
"""
Walk To: Small example to make Nao Walk follow a Dubins Curve.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
# first we defined the goal
goal = almath.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 = almath.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
motion_service.moveInit()
# get robot position before move
robotPositionBeforeCommand = almath.Pose2D(motion_service.getRobotPosition(False))
motion_service.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 = almath.Pose2D(motion_service.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = almath.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
print "The Robot Move Command: ", robotMoveCommand
# Go to rest position
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
Move head while walking¶
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Walk - Small example to make Nao walk"""
import qi
import argparse
import sys
import time
import random
def main(session):
"""
Walk: Small example to make Nao walk with jerky head.
This example is only compatible with NAO.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
# Initialize the move process.
# Check the robot pose and take a right posture.
# This is blocking called.
motion_service.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:
motion_service.moveToward(X, Y, Theta, [["Frequency", Frequency]])
except Exception, errorMsg:
print str(errorMsg)
print "This example is not allowed on this robot."
exit()
# JERKY HEAD
motion_service.setAngles("HeadYaw", random.uniform(-1.0, 1.0), 0.6)
motion_service.setAngles("HeadPitch", random.uniform(-0.5, 0.5), 0.6)
t = t + dt
time.sleep(dt)
# stop move on the next double support
motion_service.stopMove()
# Go to rest position
motion_service.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
session = qi.Session()
try:
session.connect("tcp://" + args.ip + ":" + str(args.port))
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
main(session)
