# SoftBank Robotics documentation What's new in NAOqi 2.8?

# Whole Body control Tutorial: A dance

NAOqi Motion - Overview | API | Tutorial

NAO only

Warning

Do not use on Pepper.

## Introduction

This tutorial explains how to use whole body Cartesian control API in order to make the robot dance with foot constraints, torso and arms Cartesian commands.

Note

The tutorial is written in Python.

## Download

You can download the whole Body multiple effectors control example here: `almotion_wbMultipleEffectors.py`

For any troubleshooting linked to python, see Python SDK - Installation Guide section.

## Code review

In this section we describe each important piece of code of the example.

### NAOqi tools

First, we import some external libraries:
  - argparse: toolbox useful to define parameter
  - motion: some useful definitions such as FRAME.
  - almath: an optimized mathematic toolbox for robotics. For further details, see: libalmath API reference.
  - time: mainly for the function sleep
  - ALProxy: create proxy to motion and robotposture modules

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

    """Example: Whole Body Motion - Multiple Effectors control """

    import qi
    import argparse
    import sys
    import almath
    import motion

    def main(session):
        """
            Example of a whole body multiple effectors control "LArm", "RArm" and "Torso"
            Warning: Needs a PoseInit before executing
                     Whole body balancer must be inactivated at the end of the script.
        """
        # Get the services ALMotion & ALRobotPosture.

        motion_service = session.service("ALMotion")
        posture_service = session.service("ALRobotPosture")

        # Wake up robot
        motion_service.wakeUp()

        # Send robot to Stand Init
        posture_service.goToPosture("StandInit", 0.5)

        # end go to Stand Init, begin initialize whole body

        # Enable Whole Body Balancer
        isEnabled  = True
        motion_service.wbEnable(isEnabled)

        # Legs are constrained fixed
        stateName  = "Fixed"
        supportLeg = "Legs"
        motion_service.wbFootState(stateName, supportLeg)

        # Constraint Balance Motion
        isEnable   = True
        supportLeg = "Legs"
        motion_service.wbEnableBalanceConstraint(isEnable, supportLeg)

        # end initialize whole body, define arms motions

        useSensorValues = False

        # Arms motion
        effectorList = ["LArm", "RArm"]

        frame        = motion.FRAME_ROBOT

        # pathLArm
        pathLArm = []
        currentTf = motion_service.getTransform("LArm", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += 0.08 # y
        target1Tf.r3_c4 += 0.14 # z

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= 0.05 # y
        target2Tf.r3_c4 -= 0.07 # z

        pathLArm.append(list(target1Tf.toVector()))
        pathLArm.append(list(target2Tf.toVector()))
        pathLArm.append(list(target1Tf.toVector()))
        pathLArm.append(list(target2Tf.toVector()))
        pathLArm.append(list(target1Tf.toVector()))

        # pathRArm
        pathRArm = []
        currentTf = motion_service.getTransform("RArm", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += 0.05 # y
        target1Tf.r3_c4 -= 0.07 # z

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= 0.08 # y
        target2Tf.r3_c4 += 0.14 # z

        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))
        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))
        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))

        pathList = [pathLArm, pathRArm]

        axisMaskList = [almath.AXIS_MASK_VEL, # for "LArm"
                        almath.AXIS_MASK_VEL] # for "RArm"

        coef       = 1.5
        timesList  = [ [coef*(i+1) for i in range(5)],  # for "LArm" in seconds
                       [coef*(i+1) for i in range(6)] ] # for "RArm" in seconds

        # called cartesian interpolation
        motion_service.transformInterpolations(effectorList, frame, pathList, axisMaskList, timesList)

        # end define arms motions, define torso motion

        # Torso Motion
        effectorList = ["Torso", "LArm", "RArm"]

        dy = 0.06
        dz = 0.06

        # pathTorso
        currentTf = motion_service.getTransform("Torso", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += dy
        target1Tf.r3_c4 -= dz

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= dy
        target2Tf.r3_c4 -= dz

        pathTorso = []
        for i in range(3):
            pathTorso.append(list(target1Tf.toVector()))
            pathTorso.append(currentTf)
            pathTorso.append(list(target2Tf.toVector()))
            pathTorso.append(currentTf)

        pathLArm = [motion_service.getTransform("LArm", frame, useSensorValues)]
        pathRArm = [motion_service.getTransform("RArm", frame, useSensorValues)]

        pathList = [pathTorso, pathLArm, pathRArm]

        axisMaskList = [almath.AXIS_MASK_ALL, # for "Torso"
                        almath.AXIS_MASK_VEL, # for "LArm"
                        almath.AXIS_MASK_VEL] # for "RArm"

        coef       = 0.5
        timesList  = [
                      [coef*(i+1) for i in range(12)], # for "Torso" in seconds
                      [coef*12],                       # for "LArm" in seconds
                      [coef*12]                        # for "RArm" in seconds
                     ]

        motion_service.transformInterpolations(
            effectorList, frame, pathList, axisMaskList, timesList)

        # end define torso motion, disable whole body

        # Deactivate whole body
        isEnabled    = False
        motion_service.wbEnable(isEnabled)

        # Send robot to Pose Init
        posture_service.goToPosture("StandInit", 0.3)

        # Go to rest position
        motion_service.rest()

        # end script

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session)

### Initialization of the robot

When doing Cartesian control, it’s important to be sure that the robot 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 Torso.

### Whole body initialization

Here, we specify the whole body constraints:

- enable whole body
- both legs are fixed
- balance on double support

        # Enable Whole Body Balancer
        isEnabled  = True
        motion_service.wbEnable(isEnabled)

        # Legs are constrained fixed
        stateName  = "Fixed"
        supportLeg = "Legs"
        motion_service.wbFootState(stateName, supportLeg)

        # Constraint Balance Motion
        isEnable   = True
        supportLeg = "Legs"
        motion_service.wbEnableBalanceConstraint(isEnable, supportLeg)

### Arms motion

This code is dedicated to create motion with left and right arms in parallel.
By executing only this code, you will see all the robot move, but only the arms effectors are controlled.

        useSensorValues = False

        # Arms motion
        effectorList = ["LArm", "RArm"]

        frame        = motion.FRAME_ROBOT

        # pathLArm
        pathLArm = []
        currentTf = motion_service.getTransform("LArm", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += 0.08 # y
        target1Tf.r3_c4 += 0.14 # z

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= 0.05 # y
        target2Tf.r3_c4 -= 0.07 # z

        pathLArm.append(list(target1Tf.toVector()))
        pathLArm.append(list(target2Tf.toVector()))
        pathLArm.append(list(target1Tf.toVector()))
        pathLArm.append(list(target2Tf.toVector()))
        pathLArm.append(list(target1Tf.toVector()))

        # pathRArm
        pathRArm = []
        currentTf = motion_service.getTransform("RArm", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += 0.05 # y
        target1Tf.r3_c4 -= 0.07 # z

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= 0.08 # y
        target2Tf.r3_c4 += 0.14 # z

        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))
        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))
        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))

        pathList = [pathLArm, pathRArm]

        axisMaskList = [almath.AXIS_MASK_VEL, # for "LArm"
                        almath.AXIS_MASK_VEL] # for "RArm"

        coef       = 1.5
        timesList  = [ [coef*(i+1) for i in range(5)],  # for "LArm" in seconds
                       [coef*(i+1) for i in range(6)] ] # for "RArm" in seconds

        # called cartesian interpolation
        motion_service.transformInterpolations(effectorList, frame, pathList, axisMaskList, timesList)

### Torso motion

This code is dedicated to create Cartesian motion of the robot’s torso.
During execution the arms will be fixed in FRAME_ROBOT.
This is due to the fact that a previous called (positionInterpolation) on Arms effector has automatically set effector optimization to True ( wbEnableEffectorOptimization(Arms, True) ).

        # Torso Motion
        effectorList = ["Torso", "LArm", "RArm"]

        dy = 0.06
        dz = 0.06

        # pathTorso
        currentTf = motion_service.getTransform("Torso", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += dy
        target1Tf.r3_c4 -= dz

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= dy
        target2Tf.r3_c4 -= dz

        pathTorso = []
        for i in range(3):
            pathTorso.append(list(target1Tf.toVector()))
            pathTorso.append(currentTf)
            pathTorso.append(list(target2Tf.toVector()))
            pathTorso.append(currentTf)

        pathLArm = [motion_service.getTransform("LArm", frame, useSensorValues)]
        pathRArm = [motion_service.getTransform("RArm", frame, useSensorValues)]

        pathList = [pathTorso, pathLArm, pathRArm]

        axisMaskList = [almath.AXIS_MASK_ALL, # for "Torso"
                        almath.AXIS_MASK_VEL, # for "LArm"
                        almath.AXIS_MASK_VEL] # for "RArm"

        coef       = 0.5
        timesList  = [
                      [coef*(i+1) for i in range(12)], # for "Torso" in seconds
                      [coef*12],                       # for "LArm" in seconds
                      [coef*12]                        # for "RArm" in seconds
                     ]

        motion_service.transformInterpolations(
            effectorList, frame, pathList, axisMaskList, timesList)

### Release constraint and disable whole body

This part is essential and stops the whole body balancer.

        # Deactivate whole body
        isEnabled    = False
        motion_service.wbEnable(isEnabled)

        # Send robot to Pose Init
        posture_service.goToPosture("StandInit", 0.3)

        # Go to rest position
        motion_service.rest()

Glossary Site map Index Support Contact Legal Notices