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
../../_images/motion_initposes.jpg

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)