Cartesian control Tutorial: The Hula-Hoop motion

NAOqi Motion - Overview | API | Tutorial


Introduction

This tutorial explains how to use Cartesian control API in the context of making NAO a Hula-Hoop motion (Torso control in position and in rotation).

Note

The tutorial is written in Python.

Download

You can download the Hula Hoop example here: motion_hulaHoop.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:
  • config: the config file (see above the Download section)
  • motion: some useful definition such as SPACE.
  • almath: an optimized mathematic toolbox for robotics. For further details, see: libalmath API reference.

Then, the proxy to ALMotion module is created. This proxy is useful to call ALMotion API.

import config
import motion
import almath

def main():
    '''
        Example showing a Hula Hoop Motion
        with the NAO cartesian control of torso
    '''

    motionProxy = config.loadProxy("ALMotion")

NAO initialization

When doing Cartesian control, it is important to be sure that NAO is in a good configuration. To have the maximum range of control, the maximum stability and far away of singularity.
A PoseInit is a good posture before a Cartesian control of the NAO Torso.
# Set NAO in Stiffness On
config.StiffnessOn(motionProxy)

# Send NAO to Pose Init
config.PoseInit(motionProxy)

Hula hoop motion

We define the hula hoop motion with four checkPoints:
  • forward / bend backward
  • right / bend left
  • backward / bend forward
  • left / bend right
../../_images/motion_hulaHoop.png

We define two loops of hula hoop. You can accelerate the motion by playing with the timeOneMove variable.

# Define the changes relative to the current position
dx         = 0.07                    # translation axis X (meter)
dy         = 0.07                    # translation axis Y (meter)
dwx        = 0.15                    # rotation axis X (rad)
dwy        = 0.15                    # rotation axis Y (rad)

# define a path of two hula hoop loops
path = [ [+dx, 0.0, 0.0,  0.0, -dwy, 0.0],  # point 01 : forward  / bend backward
         [0.0, -dy, 0.0, -dwx,  0.0, 0.0],  # point 02 : right    / bend left
         [-dx, 0.0, 0.0,  0.0,  dwy, 0.0],  # point 03 : backward / bend forward
         [0.0, +dy, 0.0,  dwx,  0.0, 0.0],  # point 04 : left     / bend right

         [+dx, 0.0, 0.0,  0.0, -dwy, 0.0],  # point 01 : forward  / bend backward
         [0.0, -dy, 0.0, -dwx,  0.0, 0.0],  # point 02 : right    / bend left
         [-dx, 0.0, 0.0,  0.0,  dwy, 0.0],  # point 03 : backward / bend forward
         [0.0, +dy, 0.0,  dwx,  0.0, 0.0],  # point 04 : left     / bend right

         [+dx, 0.0, 0.0,  0.0, -dwy, 0.0],  # point 05 : forward  / bend backward
         [0.0, 0.0, 0.0,  0.0,  0.0, 0.0] ] # point 06 : Back to init pose

timeOneMove  = 0.4 #seconds
times = []
for i in range(len(path)):
    times.append( (i+1)*timeOneMove )

Call the Cartesian control API

Here, we specify that we want to control the Torso (see Effectors) in the FRAME_ROBOT (see Spaces) and that we want to control all the motion with a AXIS_MASK_ALL (see Axis Masks).
We also specify that the torso path is defined in relative with the initial robot position.
# call the cartesian control API
effector   = "Torso"
space      =  motion.FRAME_ROBOT

axisMask   = almath.AXIS_MASK_ALL
isAbsolute = False

motionProxy.positionInterpolation(effector, space, path,
                                  axisMask, times, isAbsolute)