.. _control-joint-tuto: Joint control Tutorial: The Pose Init ===================================== :ref:`Overview` | :ref:`API ` | Tutorial .. seealso:: - :ref:`naoqi-motion` ------------ 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 Pose Init (left picture) is: - kneeAngle = 40.0 deg - torsoAngle = wideAngle = 0.0 deg .. image:: /medias/dev/modules/motion/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: :download:`almotion_poseInit.py ` Please refer to the section: :ref:`python-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 <../../ref/libalmath/index.html>`_. - ALProxy: use to create a proxy to **ALMotion** module. .. code-block:: python 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. (See section: :ref:`nao-joints-40`). .. code-block:: python # 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. .. code-block:: python # 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 :cpp:func:`ALMotionProxy::getRobotConfig` to know the :ref:`hardware-product-range::nao-body-type`. | Then, it's easy to define the joint value for each chain. .. code-block:: python # 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 ++++++++++++++++++++++++++++++++++++++++++ .. code-block:: python #------------------------------ 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)