Joint control Tutorial: The Pose Init¶
NAOqi Motion - Overview | API | Tutorial
Introduction¶
This tutorial explains how to use joint control API in the context of the aldebaran Robotics
definition of the “NAO initial position” known as Pose Init.
By changing the parameters for kneeAngle, torsoAngle and wideAngle, you can make
a variety of stable poses.
- Please note that the official Aldebaran Robotics Pose Init (left picture) is:
- kneeAngle = 40.0 deg
- torsoAngle = wideAngle = 0.0 deg
In this tutorial, we directly use the specific kinematic of NAO (body symmetric and quite the same length for the thigh and the tibia)
Note
The tutorial is written in Python.
Download¶
You can download the PoseInit example here: almotion_poseInit.py
Please refer to the section: Python SDK Install Guide for any troubleshooting linked to python.
Code review¶
In this section we describe each important piece of code of the example.
NAOqi tools¶
- First, we import some external library:
- almath: an optimized mathematic toolbox for robotics. For further details, see: libalmath API reference.
- ALProxy: use to create a proxy to ALMotion module.
import sys
import almath
from naoqi import ALProxy
if (len(sys.argv) < 2):
print "Usage: 'python motion_setangles.py IP [PORT]'"
sys.exit(1)
IP = sys.argv[1]
PORT = 9559
if (len(sys.argv) > 2):
PORT = sys.argv[2]
try:
proxy = ALProxy("ALMotion", IP, PORT)
except Exception,e:
print "Could not create proxy to ALMotion"
print "Error was: ",e
sys.exit(1)
Joint position initialization¶
First, the upper body joints are defined. We define them in degrees because we found degrees
more human friendly than radian. (We will convert them in radian later).
We only define one value for left and right joint because NAO is symmetric. Just be aware of
sign that depends of the joint direction. For further details, see section: Joints.
# Define The Initial Position for the upper body
HeadYawAngle = + 0.0
HeadPitchAngle = + 0.0
ShoulderPitchAngle = +80.0
ShoulderRollAngle = +20.0
ElbowYawAngle = -80.0
ElbowRollAngle = -60.0
WristYawAngle = + 0.0
HandAngle = + 0.0
As NAO thigh and tibia length are quite the same, we could use the trigonometric properties
of the isosceles triangle. (i.e.: hipPitch angle = AnklePitch angle = kneePitch angle / 2.0).
By using this rule, the torso will be kept always vertical.
The torso orientation could be only controlled with a angle addition on the hipPitch joint.
To spread feet and keep them flat on the ground, we just have to send the same angle
on the rolls joints (pitch and ankle). Just be aware of the joint direction.
# Define legs position
kneeAngle = +40.0
torsoAngle = + 0.0 # bend the torso
spreadAngle = + 0.0 # spread the legs
Find your NAO Body type¶
We want to use the “Body” group of joints (all joints will arrived in the same time). So, we have
to know the number of joints to control. For this, we use the ALMotion API
ALMotionProxy::getRobotConfig() to know the NAO Body type.
Then, it’s easy to define the joint value for each chain.
# 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":
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle, WristYawAngle, HandAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle, WristYawAngle, HandAngle]
LeftLeg = [0.0, #hipYawPitch
spreadAngle, #hipRoll
-kneeAngle/2-torsoAngle, #hipPitch
kneeAngle, #kneePitch
-kneeAngle/2, #anklePitch
-spreadAngle] #ankleRoll
RightLeg = [0.0, -spreadAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, spreadAngle]
elif robotName == "naoH21":
Head = [HeadYawAngle, HeadPitchAngle]
LeftArm = [ShoulderPitchAngle, +ShoulderRollAngle, +ElbowYawAngle, +ElbowRollAngle]
RightArm = [ShoulderPitchAngle, -ShoulderRollAngle, -ElbowYawAngle, -ElbowRollAngle]
LeftLeg = [0.0, spreadAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, -spreadAngle]
RightLeg = [0.0, -spreadAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, spreadAngle]
elif robotName == "naoT14":
Head = [HeadYawAngle, HeadPitchAngle]
LeftLeg = [0.0, spreadAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, -spreadAngle]
RightLeg = [0.0, -spreadAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, spreadAngle]
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 * almath.TO_RAD for x in pTargetAngles]
Called stiffness and joint control API¶
#------------------------------ send stiffness -----------------------------
proxy.stiffnessInterpolation("Body", 1.0, 0.5)
#------------------------------ 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)