def adjust_front():
motionProxy = config.loadProxy("ALMotion")
config.StiffnessOn(motionProxy)
x = 0.05
y = 0.0
theta = 0.0
motionProxy.walkTo(x, y, theta, [ ["StepHeight", 0.02] ]) # step height of 4 cm
def main():
''' Example showing a hand ellipsoid
Warning: Needs a PoseInit before executing
Example available: path/to/aldebaran-sdk/modules/src/examples/
python/motion_cartesianArm2.py
'''
proxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(proxy)
# send robot to Pose Init
config.PoseInit(proxy)
effector = "LArm"
space = motion.SPACE_NAO
path = [
[0.0, -0.02, +0.00, 0.0, 0.0, 0.0], # Point 1
[0.0, +0.00, +0.01, 0.0, 0.0, 0.0], # Point 2
[0.0, +0.08, +0.00, 0.0, 0.0, 0.0], # Point 3
[0.0, +0.00, -0.04, 0.0, 0.0, 0.0], # Point 4
[0.0, -0.02, +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
proxy.positionInterpolation(effector, space, path, axisMask, times,
isAbsolute)
def move(x, y, angle):
## flag=0 left flag=1 right
motionProxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(motionProxy)
config.PoseInit(motionProxy)
#####################
## 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]])
X = x * 0.01
Y = y * 0.01
Theta = angle
motionProxy.post.walkTo(X, Y, Theta)
# wait is useful because with post walkTo is not blocking function
motionProxy.waitUntilWalkIsFinished()
def main():
''' Example of a cartesian foot trajectory
Warning: Needs a PoseInit before executing
Example available: path/to/aldebaran-sdk/modules/src/examples/
python/motion_cartesianFoot.py
'''
proxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(proxy)
# send robot to Pose Init
config.PoseInit(proxy)
space = motion.SPACE_NAO
axisMask = 63 # control all the effector's axes
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
proxy.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
proxy.positionInterpolation(effector, space, path, axisMask, times,
isAbsolute)
def main():
''' Simultaneously control three effectors :
the Torso, the Left Arm and the Right Arm
Warning: Needs a PoseInit before executing
Example available: path/to/aldebaran-sdk/modules/src/examples/
python/motion_cartesianTorsoArm1.py
'''
proxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(proxy)
# send robot to Pose Init
config.PoseInit(proxy)
space = motion.SPACE_NAO
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 = 63 # control all the effector axes
proxy.post.positionInterpolation(effector, space, path, axisMask, times,
isAbsolute)
# Motion of Arms with block process
axisMask = 7 # 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 = [
[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
proxy.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
proxy.positionInterpolation(effector, space, path, axisMask, times,
isAbsolute)
def main(pChainName = "LArm"):
''' Example showing the effect of collision detection
Nao bumps his chest with his left arm with collision detection enabled
or disabled.
'''
##################
# Initialization #
##################
motionProxy = config.loadProxy("ALMotion")
# Set NAO in stiffness On.
config.StiffnessOn(motionProxy)
# Send robot to Pose Init.
config.poseZero(motionProxy)
# Get the robot configuration.
robotConfig = motionProxy.getRobotConfig()
robotName = ""
for i in range(len(robotConfig[0])):
if (robotConfig[0][i] == "Model Type"):
robotName = robotConfig[1][i]
###############################
# Arm motion bumping on torso #
###############################
# Disable collision detection on chainName.
pEnable = True
success = motionProxy.setCollisionProtectionEnabled(pChainName, pEnable)
if (not success):
print("Failed to disable collision protection")
time.sleep(1.0)
config.poseZero(motionProxy)
moveHead.moveHead(motionProxy, "HeadYaw", 45)
moveHead.moveHead(motionProxy, "HeadPitch", 45)
moveHand.moveHand(motionProxy, "RHand", 45)
moveLegs.moveLegs(motionProxy, "RHipYawPitch", 45)
moveLegs.moveLegs(motionProxy, "LHipYawPitch", 45)
moveFoot.moveFoot(motionProxy, "RAnklePitch", 45)
time.sleep(3.0)
# Make NAO's arm move so that it bumps its torso.
#moveArm(motionProxy, robotName, pChainName, [0, 40, 0, 0])
#moveArm(motionProxy, robotName, "LArm", [0, 40, 0, 0])
#time.sleep(2.0)
i = 0
# while i < 3:
# moveArm(motionProxy, robotName, pChainName, [10, 0, -90, -50], 0.3)
# moveArm(motionProxy, robotName, "LArm", [10, 10, -90, -50], 0.3)
# time.sleep(1.0)
# moveArm(motionProxy, robotName, pChainName, [10, -10, -40, -50], 0.3)
# moveArm(motionProxy, robotName, "LArm", [10, -10, -40, -50], 0.3)
# time.sleep(1.0)
# i += 1
# Go back to pose init.
config.poseZero(motionProxy)
def take_position():
motionProxy = config.loadProxy("ALMotion")
config.StiffnessOn(motionProxy)
D = find_goal.findgoal()
theta = math.atan(D / 120.0)
x = 0.30 * (1 - math.cos(theta))
y = -0.30 * (math.sin(theta))
print x, y, theta
if D > 30.0 or D < -(30.0):
motionProxy.walkTo(x, y, theta)
def main():
''' Move the torso and keep arms fixed in nao space
Warning: Needs a PoseInit before executing
Example available: path/to/aldebaran-sdk/modules/src/examples/
python/motion_cartesianTorsoArm2.py
'''
proxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(proxy)
# send robot to Pose Init
config.PoseInit(proxy)
space = motion.SPACE_NAO
coef = 1.0 # Speed motion
times = [coef, 2.0*coef, 3.0*coef, 4.0*coef] # seconds
isAbsolute = False
dx = 0.04 # translation axis X (meters)
dy = 0.04 # translation axis Y (meters)
# Motion of the Torso
effector = "Torso"
path = [
[ 0.0, +dy, 0.0, 0.0, 0.0, 0.0], # Point 1
[ -dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 2
[ 0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 3
[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 4
axisMask = 63 # control all the effector axis
# Using 'post' makes this command execute in a another thread
proxy.post.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
# Motion of Arms
axisMask = 7 # control the position
time = 4.0*coef # seconds
# Arms have a null relative motion during all the torso motion
path = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Point 1
# Motion of Right Arm
effector = "RArm"
proxy.post.positionInterpolation(effector, space, path,
axisMask, time, isAbsolute)
# Motion of Left Arm
effector = "LArm"
proxy.positionInterpolation(effector, space, path,
axisMask, time, isAbsolute)
def headcheck(pitch,yaw):
''' Example of a whole body head orientation control
Warning: Needs a PoseInit before executing
Whole body balancer must be inactivated at the end of the script
'''
motionProxy = config.loadProxy("ALMotion")
# Set NAO in Stiffness On
config.StiffnessOn(motionProxy)
# Send NAO to Pose Init
config.PoseInit(motionProxy)
effectorName = "Head"
# Active Head tracking
isEnabled = True
motionProxy.wbEnableEffectorControl(effectorName, isEnabled)
# Example showing how to set orientation target for Head tracking
# The 3 coordinates are absolute head orientation in NAO_SPACE
# Rotation in RAD in x, y and z axis
# X Axis Head Orientation feasible movement = [-20.0, +20.0] degree
# Y Axis Head Orientation feasible movement = [-75.0, +70.0] degree
# Z Axis Head Orientation feasible movement = [-30.0, +30.0] degree
targetCoordinate=[0,pitch,yaw]
# wbSetEffectorControl is a non blocking function
# time.sleep allow head go to his target
# The minimum period advised between two successives set commands is 0.2 s
targetCoordinate = [target*math.pi/180.0 for target in targetCoordinate]
motionProxy.wbSetEffectorControl(effectorName, targetCoordinate)
time.sleep(3.0)
# Deactivate Head tracking
isEnabled = False
motionProxy.wbEnableEffectorControl(effectorName, isEnabled)
def main():
''' Example showing a path of five positions
Warning: Needs a PoseInit before execution
Example available: path/to/aldebaran-sdk/modules/src/examples/
python/motion_cartesianTorso.py
'''
proxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(proxy)
# send robot to Pose Init
config.PoseInit(proxy)
effector = "Torso"
space = motion.SPACE_NAO
# Position Only
axisMask = 63 # Full control
isAbsolute = False
# define the changes relative to the current position
dx = 0.040 # translation axis X (meter)
dy = 0.040 # 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
proxy.positionInterpolation(effector, space, path,
axisMask, times, isAbsolute)
def main():
''' Example showing a path of two positions
Warning: Needs a PoseInit before executing
Example available: path/to/aldebaran-sdk/modules/src/examples/
python/motion_cartesianArm1.py
'''
proxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(proxy)
# send robot to Pose Init
config.PoseInit(proxy)
effector = "LArm"
space = motion.SPACE_NAO
axisMask = 7 # 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.06 # translation axis X (meters)
dy = 0.06 # 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
proxy.positionInterpolation(effector, space, path, axisMask, times,
isAbsolute)
"""
Step To
Small example to make Nao one Step
"""
import time
import math
import config
motionProxy = config.loadProxy("ALMotion")
#Set NAO in stiffness On
config.StiffnessOn(motionProxy)
#~ DUBINS_INTERPOLATION = True
DUBINS_INTERPOLATION = False
#####################
## Enable arms control by Walk algorithm
#####################
motionProxy.setWalkArmsEnable(True, True)
#~ motionProxy.setWalkArmsEnable(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
##get robot position before move
import config
print "Creating motion proxy"
try:
motion = config.loadProxy("ALMotion")
config.StiffnessOn(motion)
config.PoseInit(motion)
except Exception, e:
print "Error when creating motion proxy:"
print str(e)
exit(1)
print "Creating ALVideoDevice proxy"
try:
cam = config.loadProxy("ALVideoDevice")
except Exception, e:
print "Error when creating vision proxy:"
print str(e)
exit(1)
def grab():
''' Example of a whole body Enable Balance Constraint
Warning: Needs a PoseInit before executing
Whole body balancer must be inactivated at the end of the script
'''
motionProxy = config.loadProxy("ALMotion")
# Set NAO in Stiffness On
config.StiffnessOn(motionProxy)
# Activate Whole Body Balancer
isEnabled = True
motionProxy.wbEnable(isEnabled)
names = [
"RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll",
"RWristYaw", "RHand", "LShoulderPitch", "LShoulderRoll", "LElbowYaw",
"LElbowRoll", "LWristYaw", "LHand", "RHipPitch", "RKneePitch",
"RAnklePitch", "RAnkleRoll", "RHipRoll", "RHipYawPitch", "LHipPitch",
"LKneePitch", "LAnklePitch", "LAnkleRoll", "LHipRoll", "LHipYawPitch"
]
angleLists = [[
80.0 * math.pi / 180.0, -10.0 * math.pi / 180.0,
-14.0 * math.pi / 180.0, -14.0 * math.pi / 180.0,
-14.0 * math.pi / 180.0
],
[
-80.0 * math.pi / 180.0, -80.0 * math.pi / 180.0,
0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0,
0.0 * math.pi / 180.0
],
[
80.0 * math.pi / 180.0, 50.0 * math.pi / 180.0,
0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0,
0.0 * math.pi / 180.0
],
[
20.0 * math.pi / 180.0, 40.0 * math.pi / 180.0,
50.0 * math.pi / 180.0, 50.0 * math.pi / 180.0,
50.0 * math.pi / 180.0
],
[
0.0 * math.pi / 180.0, 60.0 * math.pi / 180.0,
65.0 * math.pi / 180.0, 65.0 * math.pi / 180.0,
65.0 * math.pi / 180.0
],
[
80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0,
80.0 * math.pi / 180.0, 15.0 * math.pi / 180.0,
15.0 * math.pi / 180.0
],
[
80.0 * math.pi / 180.0, -10.0 * math.pi / 180.0,
-20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0,
-20.0 * math.pi / 180.0
],
[
80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0,
0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0,
0.0 * math.pi / 180.0
],
[
-80.0 * math.pi / 180.0, -50.0 * math.pi / 180.0,
0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0,
0.0 * math.pi / 180.0
],
[
-20.0 * math.pi / 180.0, -40.0 * math.pi / 180.0,
-50.0 * math.pi / 180.0, -50.0 * math.pi / 180.0,
-50.0 * math.pi / 180.0
],
[
0.0 * math.pi / 180.0, -60.0 * math.pi / 180.0,
-65.0 * math.pi / 180.0, -65.0 * math.pi / 180.0,
-65.0 * math.pi / 180.0
],
[
80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0,
80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0,
80.0 * math.pi / 180.0
],
[
-25.0 * math.pi / 180.0, -25.0 * math.pi / 180.0,
-72.0 * math.pi / 180.0, -72.0 * math.pi / 180.0,
-25.0 * math.pi / 180.0
],
[
40.0 * math.pi / 180.0, 40.0 * math.pi / 180.0,
60.0 * math.pi / 180.0, 60.0 * math.pi / 180.0,
40.0 * math.pi / 180.0
],
[
-18.0 * math.pi / 180.0, -18.0 * math.pi / 180.0,
-20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0,
-20.0 * math.pi / 180.0
], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0],
[
-25.0 * math.pi / 180.0, -25.0 * math.pi / 180.0,
-72.0 * math.pi / 180.0, -72.0 * math.pi / 180.0,
-25.0 * math.pi / 180.0
],
[
40.0 * math.pi / 180.0, 40.0 * math.pi / 180.0,
60.0 * math.pi / 180.0, 60.0 * math.pi / 180.0,
40.0 * math.pi / 180.0
],
[
-18.0 * math.pi / 180.0, -18.0 * math.pi / 180.0,
-20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0,
-20.0 * math.pi / 180.0
], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0]]
timeLists = [[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15],
[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15]]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)
# Activate Whole Body Balancer
isEnabled = False
motionProxy.wbEnable(isEnabled)
