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():
''' 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():
''' 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():
''' 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)
times.append([2.40000])
keys.append([0.51487])
motionProxy = config.loadProxy("ALMotion")
motionProxy.angleInterpolation(names, keys, times, True)
vg.showNaoImage(config.IP, config.PORT, 1)
adjust = fb.findball()
print adjust[0]
print adjust[1]
if (adjust[1] == 1):
kick_ball.adjust_front()
print "Moving Front to adjust"
if (adjust[0] == 5):
kick_ball.adjust_right()
kick_ball.kick_right()
elif (adjust[0] == 4):
kick_ball.adjust_left()
kick_ball.kick_left()
elif (adjust[0] == 3):
kick_ball.kick_right()
elif (adjust[0] == 2):
kick_ball.kick_left()
elif (adjust[0] == 1):
kick_ball.adjust_left()
kick_ball.kick_right()
elif (adjust[0] == 0):
kick_ball.adjust_right()
kick_ball.kick_left()
time.sleep(5)
config.PoseInit(motionProxy)
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)