def moveToRedBall(IP, PORT):
targetName = "RedBall"
diameter = 0.08
distanceX = 0.08
distanceY = 0.0
angleWz = 0.0
thresholdX = 0.1
thresholdY = 0.1
thresholdWz = 1.0
effector = "None"
mode = "Move"
tracker = config.loadProxy("ALTracker")
memory = config.loadProxy("ALMemory")
tracker.setEffector(effector)
tracker.registerTarget(targetName, diameter)
tracker.setRelativePosition(
[-distanceX, distanceY, angleWz, thresholdX, thresholdY, thresholdWz])
tracker.setMode(mode)
import time
t_end = time.time() + 20
while True:
if time.time() < t_end:
tracker.track(targetName)
else:
tracker.stopTracker()
tracker.unregisterTarget(targetName)
break
def kick_right():
posture = config.loadProxy('ALRobotPosture')
motion = config.loadProxy('ALMotion')
isEnabled = True
motion.wbEnable(isEnabled)
# Legs are constrained fixed
stateName = "Fixed"
supportLeg = "Legs"
motion.wbFootState(stateName, supportLeg)
# Constraint Balance Motion
isEnable = True
supportLeg = "Legs"
motion.wbEnableBalanceConstraint(isEnable, supportLeg)
# Com go to LLeg
supportLeg = "LLeg"
duration = 1.0
motion.wbGoToBalance(supportLeg, duration)
# RLeg is free
stateName = "Free"
supportLeg = "RLeg"
motion.wbFootState(stateName, supportLeg)
# RLeg is optimized
effectorName = "RLeg"
axisMask = 63
space = mot.FRAME_TORSO
# Motion of the RLeg
dx = 0.025 # translation axis X (meters)
dz = 0.02 # translation axis Z (meters)
dwy = 5.0 * math.pi / 180.0 # rotation axis Y (radian)
times = [1.0, 1.4, 2.1]
isAbsolute = False
targetList = [[-0.7 * dx, 0.0, 1.1 * dz, 0.0, +dwy, 0.0],
[+2.2 * dx, +dx, dz, 0.0, -dwy, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]
motion.positionInterpolation(effectorName, space, targetList, axisMask,
times, isAbsolute)
# Example showing how to Enable Effector Control as an Optimization
isActive = False
motion.wbEnableEffectorOptimization(effectorName, isActive)
time.sleep(1.0)
# Deactivate Head tracking
isEnabled = False
motion.wbEnable(isEnabled)
# send robot to Pose Init
posture.goToPosture("StandInit", 0.5)
def showNaoImage(IP, PORT, camera_id): #
camProxy = config.loadProxy(
"ALVideoDevice"
) #Service which give capability to use videodevice of Nao
camProxy.kCameraSelectID = 18
print camProxy.getParam(camProxy.kCameraSelectID)
camProxy.setParam(camProxy.kCameraSelectID, camera_id)
resolution = 1 # VGA
colorSpace = 11 # RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace,
5)
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = []
naoImage = camProxy.getImageRemote(videoClient)
camProxy.unsubscribe(videoClient)
print(naoImage)
# Now we work with the image returned and save it as a PNG using ImageDraw
# package.
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
im = Image.frombytes("RGB", (imageWidth, imageHeight), array)
# Save the image.
im.save("camImage.png", "PNG")
im.show()
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 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 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 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():
''' 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(effector, file):
motionProxy = config.loadProxy("ALMotion")
output(str(motionProxy.getAngles(effector, True)), file)
#motionProxy.setStiffnesses(effector, 0.0)
while raw_input('<Enter> to record; <q> to quit. ') != 'q':
output(str(motionProxy.getAngles(effector, True)), file)
def startMotionProxy(self):
# Create a proxy to Motion
try:
motionProxy = config.loadProxy("ALMotion")
except Exception, e:
print "Error when creating motion proxy:"
print str(e)
exit(1)
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 locate_goal():
headMov = config.loadProxy("ALMotion")
i = 0
a = [-1.5, 0, 1.5]
b = [2.0, 4.0, 6.0]
isAbsolute = True
headMov.stiffnessInterpolation("HeadYaw", 1.0, 1.0)
targetName = "LandMark"
diameter = 0.13
distanceX = 0.08
distanceY = 0.0
angleWz = 0.0
thresholdX = 0.1
thresholdY = 0.1
thresholdWz = 1.0
subscribeDone = False
sizeMark = 0.1
markIds = [
68, 85, 84, 204, 145, 76, 115, 153, 112, 11, 135, 127, 170, 123, 114,
121
]
effector = "None"
mode = "WholeBody"
tracker = config.loadProxy("ALTracker")
tracker.setEffector(effector)
tracker.registerTarget(targetName, diameter)
tracker.setRelativePosition(
[-distanceX, distanceY, angleWz, thresholdX, thresholdY, thresholdWz])
tracker.setMode(mode)
t_end = time.time() + 30
tracker.track(targetName)
for i in range(3):
headMov.angleInterpolation("HeadYaw", a[i], b[i], isAbsolute)
i += 1
if tracker.isTargetLost():
continue
else:
break
headMov.angleInterpolation("HeadYaw", 0, 8.0, isAbsolute)
if time.time() == t_end:
tracker.stopTracker()
tracker.unregisterTarget(targetName)
headMov.stiffnessInterpolation("HeadYaw", 0.0, 1.0)
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 kick_left():
names = list()
times = list()
keys = list()
names.append("LHipYawPitch")
times.append([ 2.60000, 5.20000])
keys.append([ [ -0.00456, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ 0.01538, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("LHipRoll")
times.append([ 2.60000, 5.20000])
keys.append([ [ 0.13810, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ 0.13964, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("LHipPitch")
times.append([ 2.60000, 5.00000, 5.20000])
keys.append([ [ -0.28528, [ 3, -0.86667, 0.00000], [ 3, 0.80000, 0.00000]], [ -0.56549, [ 3, -0.80000, 0.10950], [ 3, 0.06667, -0.00913]], [ -0.64117, [ 3, -0.06667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("LKneePitch")
times.append([ 2.60000, 5.00000, 5.20000])
keys.append([ [ 1.02007, [ 3, -0.86667, 0.00000], [ 3, 0.80000, 0.00000]], [ 1.91812, [ 3, -0.80000, 0.00000], [ 3, 0.06667, 0.00000]], [ 0.97558, [ 3, -0.06667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("LAnklePitch")
times.append([ 2.60000, 5.00000, 5.20000])
keys.append([ [ -0.69955, [ 3, -0.86667, 0.00000], [ 3, 0.80000, 0.00000]], [ -0.46251, [ 3, -0.80000, -0.17606], [ 3, 0.06667, 0.01467]], [ -0.12736, [ 3, -0.06667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("LAnkleRoll")
times.append([ 2.60000, 5.20000])
keys.append([ [ 0.00311, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ 0.00311, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("RHipRoll")
times.append([ 2.60000, 5.20000])
keys.append([ [ 0.11202, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ 0.11202, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("RHipPitch")
times.append([ 2.60000, 5.20000])
keys.append([ [ -0.20253, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ -0.20406, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("RKneePitch")
times.append([ 2.60000, 5.20000])
keys.append([ [ 0.83761, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ 0.84528, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("RAnklePitch")
times.append([ 2.60000, 5.20000])
keys.append([ [ -0.45862, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ -0.46016, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
names.append("RAnkleRoll")
times.append([ 2.60000, 5.20000])
keys.append([ [ -0.23466, [ 3, -0.86667, 0.00000], [ 3, 0.86667, 0.00000]], [ -0.23466, [ 3, -0.86667, 0.00000], [ 3, 0.00000, 0.00000]]])
try:
motion = config.loadProxy("ALMotion")
motion.angleInterpolationBezier(names, times, keys);
except BaseException, err:
print err
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 locate_ball():
headMov = config.loadProxy("ALMotion")
i = 0
a = [-1.5, 0, 1.5]
b = [2.0, 4.0, 6.0]
isAbsolute = True
headMov.stiffnessInterpolation("HeadYaw", 1.0, 1.0)
targetName = "RedBall"
diameter = 0.08
distanceX = 0.08
distanceY = 0.0
angleWz = 0.0
thresholdX = 0.1
thresholdY = 0.1
thresholdWz = 1.0
effector = "None"
mode = "Move"
tracker = config.loadProxy("ALTracker")
tracker.setEffector(effector)
tracker.registerTarget(targetName, diameter)
tracker.setRelativePosition(
[-distanceX, distanceY, angleWz, thresholdX, thresholdY, thresholdWz])
tracker.setMode(mode)
t_end = time.time() + 30
tracker.track(targetName)
for i in range(3):
headMov.angleInterpolation("HeadYaw", a[i], b[i], isAbsolute)
i += 1
if tracker.isTargetLost():
continue
else:
break
headMov.angleInterpolation("HeadYaw", 0, 8.0, isAbsolute)
if time.time() == t_end:
tracker.stopTracker()
tracker.unregisterTarget(targetName)
headMov.stiffnessInterpolation("HeadYaw", 0.0, 1.0)
def showNaoImage(IP, PORT, camera_id):
camProxy = config.loadProxy("ALVideoDevice")
camProxy.kCameraSelectID = 18
print camProxy.getParam(camProxy.kCameraSelectID)
camProxy.setParam(camProxy.kCameraSelectID, camera_id)
resolution = 1 # VGA
colorSpace = 11 # RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace,
5)
naoImage = []
naoImage = camProxy.getImageRemote(videoClient)
camProxy.unsubscribe(videoClient)
print(naoImage)
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
im = Image.frombytes("RGB", (imageWidth, imageHeight), array)
im.save("camImage.png", "PNG")
im.show()
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 getImage():
''' First get an image from Nao, then show it on the screen with PIL '''
camProxy = config.loadProxy("ALVideoDevice")
resolution = 1 # 320x240
colorSpace = 11 # RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
t0 = time.time()
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = camProxy.getImageRemote(videoClient)
t1 = time.time()
# Time the image transfer.
print "acquisition delay ", t1 - t0
camProxy.unsubscribe(videoClient)
# Now we work with the image returned and save it as a PNG using ImageDraw
# package.
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
origImage = Image.fromstring("RGB", (imageWidth, imageHeight), array)
# Save the image.
# im.save( str(colorSpace) + ".png", "PNG")
return origImage
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)
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 posinit2(HeadYawAngle, HeadPitchAngle):
#该方法中可以设置头部俯仰角度
proxy = config.loadProxy("ALMotion")
# Define The Initial Position
ShoulderPitchAngle = +80.0
ShoulderRollAngle = +20.0
ElbowYawAngle = -80.0
ElbowRollAngle = +0.0
WristYawAngle = + 0.0
HandAngle = + 0.0
HipYawPitchAngle = + 0.0
HipRollAngle = + 0.0
HipPitchAngle = -25.0
KneePitchAngle = +40.0
AnklePitchAngle = -20.0
AnkleRollAngle = + 0.0
# Get the Robot Configuration
robotConfig = proxy.getRobotConfig()
robotName = ""
for i in range(len(robotConfig[0])):
if (robotConfig[0][i] == "Model Type"):
robotName = robotConfig[1][i]
if (robotName == "naoH25") or\
(robotName == "naoAcademics"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle, WristYawAngle, HandAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle, WristYawAngle, HandAngle]
LeftLeg = [HipYawPitchAngle, +HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, +AnkleRollAngle]
RightLeg = [HipYawPitchAngle, -HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, -AnkleRollAngle]
elif (robotName == "naoH21") or\
(robotName == "naoRobocup"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle]
LeftLeg = [HipYawPitchAngle, +HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, +AnkleRollAngle]
RightLeg = [HipYawPitchAngle, -HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, -AnkleRollAngle]
elif (robotName == "naoT14"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle, WristYawAngle, HandAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle, WristYawAngle, HandAngle]
LeftLeg = []
RightLeg = []
elif (robotName == "naoT2"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = []
RightArm = []
LeftLeg = []
RightLeg = []
else:
print "ERROR : Your robot is unknown"
print "This test is not available for your Robot"
print "---------------------"
exit(1)
# Gather the joints together
pTargetAngles = Head+LeftArm + LeftLeg + RightLeg + RightArm
# Convert to radians
pTargetAngles = [ x * motion.TO_RAD for x in pTargetAngles]
#------------------------------ send the commands -----------------------------
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
# We set the fraction of max speed
pMaxSpeedFraction = 0.2
# Ask motion to do this with a blocking call
proxy.angleInterpolationWithSpeed(pNames, pTargetAngles, pMaxSpeedFraction)
if (sys.argv[0] != ""):
IP = sys.argv[1]
####
# Create python broker
#try:
# broker = ALBroker("pythonBroker","127.0.0.1",9999,IP, PORT)
#except RuntimeError,e:
# print("cannot connect")
# exit(1)
####
# Create motion proxy
print "Creating motion proxy"
#try:
motionProxy = config.loadProxy("ALMotion")
robotConfig = motionProxy.getRobotConfig()
print robotConfig
#except Exception,e:
# print "Error when creating motion proxy:"
# print str(e)
# exit(1)
####
# Create memory proxy
print "Creating ALMemory proxy"
#try:
memoryProxy = config.loadProxy("ALMemory")
#except Exception,e:
# print "Error when creating ALMemory proxy:"
# print str(e)
import config
videodeviceProxy = config.loadProxy("ALVideoDevice")
def topCamera(): # Activate top camera.
kCameraSelectID = 18
videodeviceProxy.setParam(kCameraSelectID, 0)
def bottomCamera(): # Activate bottom camera.
kCameraSelectID = 18
videodeviceProxy.setParam(kCameraSelectID, 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)
# Included in Python
import threading
import math
# Need to get from online
import numpy
from matplotlib import pyplot
# local classes
import track
import config
motionProxy = config.loadProxy("ALMotion")
redballtracker = config.loadProxy("ALRedBallTracker")
texttospeechProxy = config.loadProxy("ALTextToSpeech")
kalmanstop=threading.Event() #creates events so that we can stop it later
trackstop=threading.Event()
def stop():
trackstop.set()#sets a flag that stops this if its looking for the ball
kalmanstop.set()#sets the flag to stop the kalman filter
class kalman(threading.Thread):
def run(self):
kalmanstop.clear() #clears the flag that seting the event does.
trackstop.clear()
#initializing the vectors that store the variables.
global xkalman, ykalman, xvelocity, yvelocity #these variables are made global, so we can call on them
#Whenever we want.
xvelocity = [0]
yvelocity = [0]
T = [0] #Time elapsed
xsensed=[1]
ysensed=[0]
# IP = sys.argv[1]
####
# Create python broker
#try:
# broker = ALBroker("pythonBroker","127.0.0.1",9999,IP, PORT)
#except RuntimeError,e:
# print("cannot connect")
# exit(1)
####
# Create motion proxy
print "Creating motion proxy"
#try:
motionProxy = config.loadProxy("ALMotion")
#except Exception,e:
# print "Error when creating motion proxy:"
# print str(e)
# exit(1)
####
# Create memory proxy
print "Creating ALMemory proxy"
#try:
memoryProxy = config.loadProxy("ALMemory")
#except Exception,e:
# print "Error when creating ALMemory proxy:"
# print str(e)
# exit(1)
import track
import walk
import config
import dive
import math
motionProxy = config.loadProxy("ALMotion")
pNames = "Body"
timeProxy = config.loadProxy("DCM")
redballtracker = config.loadProxy("ALRedBallTracker")
def goaliePose(): #This is the pose the goalie will be using.
config.stiffnessOn()
motionProxy.angleInterpolationWithSpeed(pNames, dive.goaliePosition(), .5)
def goalie():
goaliePose()
while True:
track.findRedBall()
redballtracker.startTracker()
if counter == 0: #This is for the first time to define redballposition since it will be updated after redballposition2
redballposition = redballtracker.getPosition()
counter == 1
redballposition2 = redballposition #redballposition2 stores the old position of the ball and we get the difference of them
print "redballposition2 :", redballposition2
while count < 100:
redballtracker.setWholeBodyOn(True)
redballposition2 = redballposition
redballposition = redballtracker.getPosition() #Updates the position of the ball
print "redballposition:", redballposition
distance = math.sqrt(math.pow(redballposition[0],2) + math.pow(redballposition[1],2) + math.pow(redballposition[2],2))
print "distance:" , distance
velocity = math.sqrt(math.pow((redballposition[0] - redballposition2[0]), 2))/ .1
print "velocity:" , velocity, "count" , count
def getHead(self):
memoryProxy = config.loadProxy("ALMemory")
HeadYawAngle = memoryProxy.getData("Device/SubDeviceList/HeadYaw/Position/Actuator/Value")
HeadPitchAngle = memoryProxy.getData("Device/SubDeviceList/HeadPitch/Position/Actuator/Value")
return HeadYawAngle, HeadPitchAngle
import config
import motion
from naoqi import ALProxy
proxy = config.loadProxy("ALMotion")
tts = ALProxy("ALTextToSpeech", config.IP, 9559)
pNames = "Body"
def poseint():
Head = [+0, +0]
LeftArm = [+40, +0, +40, +0, +0, +0]
RightArm = [+40, +0, -40, +0, +0, +0]
LeftLeg = [+0, +00, +0, +0, +0, +0]
RightLeg = [+0, -00, +0, +0, +0, +0]
pTargetAngles = (Head + LeftArm + LeftLeg + RightLeg + RightArm)
pTargetAngles = [x * motion.TO_RAD for x in pTargetAngles]
return pTargetAngles
def stand():
Head = [+0, +0]
LeftArm = [+110, +40, +20, -90, +0, +0]
RightArm = [+110, -40, -20, +90, +0, +0]
LeftLeg = [+0, +0, -90, +00, +0, +0]
RightLeg = [+0, -0, -90, +00, +0, +0]
pTargetAngles = (Head + LeftArm + LeftLeg + RightLeg + RightArm)
pTargetAngles = [x * motion.TO_RAD for x in pTargetAngles]
return pTargetAngles
walk.moveToRedBall(config.IP, config.PORT)
vg.showNaoImage(config.IP, config.PORT, 0)
take_position.take_position()
names = list()
times = list()
keys = list()
names.append("HeadYaw")
times.append([2.40000])
keys.append([-0.00158])
names.append("HeadPitch")
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()
def setHeadMotion(HeadYawAngle, HeadPitchAngle):
proxy = config.loadProxy("ALMotion")
# Define The Initial Position
#HeadYawAngle = + 0.0
#HeadPitchAngle = + 0.0
ShoulderPitchAngle = +80.0
ShoulderRollAngle = +20.0
#HeadYawAngle = memoryProxy.getData("Device/SubDeviceList/HeadYaw/Position/Actuator/Value")
ElbowYawAngle = -80.0
ElbowRollAngle = -60.0
WristYawAngle = + 0.0
HandAngle = + 0.0
HipYawPitchAngle = + 0.0
HipRollAngle = + 0.0
HipPitchAngle = -25.0
KneePitchAngle = +40.0
AnklePitchAngle = -20.0
AnkleRollAngle = + 0.0
# Get the Robot Configuration
robotConfig = proxy.getRobotConfig()
robotName = ""
for i in range(len(robotConfig[0])):
if (robotConfig[0][i] == "Model Type"):
robotName = robotConfig[1][i]
if (robotName == "naoH25") or\
(robotName == "naoAcademics"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle, WristYawAngle, HandAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle, WristYawAngle, HandAngle]
LeftLeg = [HipYawPitchAngle, +HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, +AnkleRollAngle]
RightLeg = [HipYawPitchAngle, -HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, -AnkleRollAngle]
elif (robotName == "naoH21") or\
(robotName == "naoRobocup"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle]
LeftLeg = [HipYawPitchAngle, +HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, +AnkleRollAngle]
RightLeg = [HipYawPitchAngle, -HipRollAngle, HipPitchAngle, KneePitchAngle, AnklePitchAngle, -AnkleRollAngle]
elif (robotName == "naoT14"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle, WristYawAngle, HandAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle, WristYawAngle, HandAngle]
LeftLeg = []
RightLeg = []
elif (robotName == "naoT2"):
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = []
RightArm = []
LeftLeg = []
RightLeg = []
else:
print "ERROR : Your robot is unknown"
print "This test is not available for your Robot"
print "---------------------"
exit(1)
# Gather the joints together
pTargetAngles = Head + LeftArm + LeftLeg + RightLeg + RightArm
# Convert to radians
pTargetAngles = [ x * motion.TO_RAD for x in pTargetAngles]
#------------------------------ send the commands -----------------------------
# We use the "Body" name to signify the collection of all joints
pNames = "Body"
# We set the fraction of max speed
pMaxSpeedFraction = 0.2
# Ask motion to do this with a blocking call
proxy.angleInterpolationWithSpeed(pNames, pTargetAngles, pMaxSpeedFraction)
#returns alpha (left/right), beta (up/down)
return HeadYawAngle, HeadPitchAngle
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)
Koen Keune - 10003527
Jouke van der Maas - 10186883
**kickmotions.py**
Defines the different moves the Nao needs to perform the kick.
positionLeftLeg: initial step, positions the left leg of the robot so the right leg can
move freely.
positionRightLeg: positions the right leg to make the kick.
liftArms: lifts the robot's arms so he can hold the ball.
kick: drops the ball and kicks it.
'''
import config
motion = config.loadProxy('ALMotion')
def factor(list, f):
return [f*x for x in list]
def positionLeftLeg():
f = 0.5
names = list()
angles = list()
times = list()
names.append('LHipPitch')
angles.append(
[-0.400331974029541,
-0.5690720081329346,
'''
Nao Goal Kick
Wessel Klijnsma - 10172432
Marysia Winkels - 10163727
Koen Keune - 10003527
Jouke van der Maas - 10186883
**dokick.py**
Performs all movements needed to do a kick. Run right after standup.py.
You will be prompted to give the robot the ball.
'''
import kickmotions
import config
tts = config.loadProxy("ALTextToSpeech")
kickmotions.liftArms()
tts.say("Ball please.")
raw_input('Druk op enter als de bal vast zit.')
kickmotions.positionLeftLeg()
kickmotions.positionRightLeg()
kickmotions.kick()
if( sys.argv[0] != "" ):
IP = sys.argv[1]
####
# Create python broker
#try:
# broker = ALBroker("pythonBroker","127.0.0.1",9999,IP, PORT)
#except RuntimeError,e:
# print("cannot connect")
# exit(1)
####
# Create motion proxy
print "Creating motion proxy"
#try:
motionProxy = config.loadProxy("ALMotion")
robotConfig = motionProxy.getRobotConfig()
print robotConfig
#except Exception,e:
# print "Error when creating motion proxy:"
# print str(e)
# exit(1)
####
# Create memory proxy
print "Creating ALMemory proxy"
#try:
memoryProxy = config.loadProxy("ALMemory")
#except Exception,e:
# print "Error when creating ALMemory proxy:"
import kalmanthreaded
import config
import kick
import stand
import track
import walk
import dive
import goalie
import mapping
import kalmanfilter
import sudoarchitecture
import objectrecognition
motionProxy = config.loadProxy("ALMotion")
texttospeechProxy = config.loadProxy("ALTextToSpeech")
def main():
def menu():
#print what options you have
print "\n*** OPTIONS ***"
print "0) Quit 5) Right Kick 10) Walk Left 15) Stand Up from Sit 20) R to S 25)Goalie"
print "1) Stiffness ON 6) Walk Foward 11) Walk Right 16) Find Red Ball L to R 21) R to L 26) Map"
print "2) Stiffness OFF 7) Walk Backwards 12) stand up on back 17) Find Red Ball L to S 22) Stop Tracker 27) Kalman Filter"
print "3) Goalie Pose 8) Turn Left 13) stand up on front 18) Find Red Ball L to L 23) Dive Left 28) Goalie intercepts clockwise"
print "4) Left Kick 9) Turn Right 14) Sit Down 19) Find Red Ball R to L 24) Dive Right 29) Goalie intercepts counterclockwise"
print "type what you want him to say in quotations"
return input ("\nChoose your option(s) as list: ")
# NOW THE PROGRAM REALLY STARTS, AS CODE IS RUN
loop = 1
choice = 0
while loop == 1:
choice = menu()
import config
import time
# ====================
# Create proxy to ALMemory
memoryProxy = config.loadProxy("ALMemory")
# Get the Gyrometers Values
GyrX = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/GyrX/Sensor/Value")
GyrY = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/GyrY/Sensor/Value")
print("Gyrometers value X: %.3f, Y: %.3f" % (GyrX, GyrY))
# Get the Accelerometers Values
AccX = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/AccX/Sensor/Value")
AccY = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/AccY/Sensor/Value")
AccZ = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/AccZ/Sensor/Value")
print("Accelerometers value X: %.3f, Y: %.3f, Z: %.3f" % (AccX, AccY, AccZ))
# Get the Compute Torso Angle in radian
TorsoAngleX = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
TorsoAngleY = memoryProxy.getData(
"Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value")
print("Torso Angles [radian] X: %.3f, Y: %.3f" % (TorsoAngleX, TorsoAngleY))
def main(effector, stiffness):
motionProxy = config.loadProxy("ALMotion")
motionProxy.setStiffnesses(effector, stiffness)