This section shows how to make NAO go to poses Pose Init and Pose Zero outside Choregraphe.
Make NAO go to a good initial position.
#-*- coding: iso-8859-15 -*-
''' PoseInit: Small example to make Nao go to an initial position. '''
import config
import motion
def main():
proxy = config.loadProxy("ALMotion")
# Define The Initial Position
HeadYawAngle = + 0.0
HeadPitchAngle = + 0.0
ShoulderPitchAngle = +80.0
ShoulderRollAngle = +20.0
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)
if __name__ == "__main__":
main()
Put all NAO motors to zero.
#-*- coding: iso-8859-15 -*-
''' PoseZero: Set all the motors of the body to zero. '''
import config
def main():
motionProxy = config.loadProxy("ALMotion")
# We use the "Body" name to signify the collection of all joints and actuators
pNames = "Body"
# Get the Number of Joints
numBodies = len(motionProxy.getJointNames(pNames))
# We prepare a collection of floats
pTargetAngles = [0.0] * numBodies
# We set the fraction of max speed
pMaxSpeedFraction = 0.3
# Ask motion to do this with a blocking call
motionProxy.angleInterpolationWithSpeed(pNames, pTargetAngles, pMaxSpeedFraction)
if __name__ == "__main__":
main()