Whole Body control¶

What it does¶

It is a powerful tool which gives a very natural and safe motion.

The main goal is to generate and stabilize consistent motions and adapt NAO’s behaviour to the situation.

This tool comes with two main functionalities:

stabilized motion methods: The goal is to stabilize motion generated by joint (and choregraphe) or cartesian:
safe cartesian control methods:
- ALMotionProxy::wbEnableEffectorControl
- ALMotionProxy::wbSetEffectorControl

How it works¶

Whole Body Balancer¶

It is a Generalized Inverse Kinematics which deals with cartesian and joint control, balance, redundancy and task priority.

This formulation takes into account all joints of the robot in a single problem. The motion obtained guaranties several specified conditions like balance, keeping a foot fixed ...

Moreover, when asking NAO to reach out his arm, he bends over because arms but also legs joints are taken into account. And he stops before stretching too much in order to keep his balance.

This behaviour is automatically managed by Whole Body Balancer. The user only asks NAO to reach out his arm.

HipYawPitch joint coupling is implicitly taken into account in the balancer.

Whole Body Balancer - detailed¶

The Generalized Inverse Kinematics problem is written as a quadratic program which is solved every 20 ms using the C++ open source library qpOases¹.

The classical form of a quadratic program is:

$\text{min} \frac{1}{2} \|Y - Y^{\text{des}} \|^{2}_{Q} \text{ such as } \left\{\begin{array}{c} A \: Y + b = 0 \\ C \: Y + d \geq 0 \\ \end{array}\right.$

$Y$ : Unknown vector;
$Y^{\text{des}}$ : Desired but not necessarily feasible solution;
$Q$ : Quadratic norm;
$A$ , $b$ , $C$ and $d$ : Matrices and vectors which express linear equality and inequality constraints.

In our case, the unknown vector is composed of:

Velocity of torso. It is an unactuated body with 6 degrees of freedom (3 translations, 3 rotations);
Velocity of all the articulated joints. LHand and RHand are not taken into account because they are not part of a kinematics chain. Moreover, LHipYawPitch and RHipYawPitch are mapped to a single variable because these articulations are controlled by only one actuator.

The equality constraints are about keeping feet fixed or in a plane.

The inequality constraints are:

Joint limits. These constraints are always taken into account;
Balance. The Center Of Mass is constrained to stay within the support polygon. We can activate/disactivate this constraint and specify the support polygon.

Y^des is composed of:

Cartesian desired trajectories.
Articular desired trajectories.

These orders are not necessarily feasible and may even contradict. The solution obtained is feasible (it fulfills all the constraints) and is a compromise between the desired motions.

¹ H.J. Ferreau, H.G. Bock and M. Diehl, ”An online active set strategy to overcome the limitation of explicit MPC,” IEEE - International Journal of Robust and Nonlinear Control, pp. 816-830, 2008.

Getting started¶

When use it¶

This balancer can be used with every joint control (angle interpolation, choregraphe timeline) and effector control.

There are two exceptions when Whole Body balancer can not be used:

during walk.
if the robot is not standing on his feet (see ALRobotPose).

How to activate/deactivate it¶

The Whole Body Motion is by default deactivated on the robot. The following example show how to activate it.

# Example showing how to active Whole Body Balancer.
isEnabled = True
proxy.wbEnable(isEnabled)

Warning

Take care to deactivate Whole Body Balancer at the end of your behaviour.

Use Cases¶

During joint control (choregraphe timeline for example), motion can be stabilized by Whole Body balancer. Consequently, initial motion is modified to the closest motion which respects balance and/or foot state.

Whole Body balancer has to be activated to use the following function (ALMotionProxy::wbEnable).

Case 1: Feet Condition¶

This api sets the foot state:

Fixed: the 6 cartesian degrees of freedom are constrained. The foot are completely fixed.
Plane: it constrains the foot in the plane. The following cartesian axis are constrained (Z, Wx, Wy). Foot can move in X, Y and Wz axis.
Free: the foot can move in all the cartesian axis.

# Example showing how to fix the feet.
stateName = "Fixed"
supportLeg = "Legs"
proxy.wbFootState(stateName, supportLeg)

Note

Use choregraphe box “Keep Balance”!

In the following example, we have specified that the feet have to keep flat on the ground.

Then, we send command on HipYawPitch joint. This joint is the only way to rotate the foot of NAO, but with only joint control it is really difficult to keep the feet flat with Whole body it’s easy and the generated motion is stable.

motion_wbFootState.py

To be executed, this example require a config file: config.py. Modify the config file to enter your robot IP, and place it in the same folder as the example.

#-*- coding: iso-8859-15 -*-

''' Whole Body Motion: Foot State '''

import config
import math

def main():
    ''' Example of a whole body FootState
    Warning: Needs a PoseInit before executing
             Whole body balancer must be inactivated at the end of the script
    '''

    proxy = config.loadProxy("ALMotion")

    # Set NAO in stiffness On
    config.StiffnessOn(proxy)

    # Send robot to Pose Init
    config.PoseInit(proxy)

    # Activate Whole Body Balancer.
    isEnabled  = True
    proxy.wbEnable(isEnabled)

    # Legs are constrained in a plane
    stateName  = "Plane"
    supportLeg = "Legs"
    proxy.wbFootState(stateName, supportLeg)

    # HipYawPitch angleInterpolation
    # Without Whole Body balancer, foot will not be keeped plane.
    names      = "LHipYawPitch"
    angleLists = [-45.0, 10.0, 0.0]
    timeLists  = [3.0, 6.0, 9.0]
    isAbsolute = True
    angleLists = [angle*math.pi/180.0 for angle in angleLists]
    proxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)

    # Disactivate Whole Body Balancer.
    isEnabled  = False
    proxy.wbEnable(isEnabled)

if __name__ == "__main__":
    main()

Case 2: Balance Constraint¶

The main gaol is to main the COM of NAO inside the support polygon (definition of static balance).

The support polygon depends of support leg: Legs (both feet), LLeg or RLeg.

# Example showing how to Constraint Balance Motion.
isEnable   = True
supportLeg = "Legs"
proxy.wbEnableBalanceConstraint(isEnable, supportLeg)

At last, Foot have to be constrained. The following combination are allowed.

wbEnableBalanceConstraint(True, “Legs”) with:

wbFootState(“Fixed”, “Legs”)
wbFootState(“Fixed”, “LLeg”) and wbFootState(“Plane”, “RLeg”)
wbFootState(“Fixed”, “RLeg”) and wbFootState(“Plane”, “LLeg”)

wbEnableBalanceConstraint(True, “LLeg”) with:

wbFootState(“Fixed”, “Legs”)
wbFootState(“Fixed”, “LLeg”) and wbFootState(“Fixed”, “RLeg”)
wbFootState(“Fixed”, “LLeg”) and wbFootState(“Plane”, “RLeg”)
wbFootState(“Fixed”, “LLeg”) and wbFootState(“Free” , “RLeg”)

wbEnableBalanceConstraint(True, “RLeg”) with:

wbFootState(“Fixed”, “Legs”)
wbFootState(“Fixed”, “RLeg”) and wbFootState(“Fixed”, “LLeg”)
wbFootState(“Fixed”, “RLeg”) and wbFootState(“Plane”, “LLeg”)
wbFootState(“Fixed”, “RLeg”) and wbFootState(“Free” , “LLeg”)

Warning

When asked, if COM is not inside the desired support polygon, this constraint is not activated.

This is equivalent to robot is stand (see ALRobotPose)

motion_wbEnableBalanceConstraint.py

To be executed, this example require a config file: config.py. Modify the config file to enter your robot IP, and place it in the same folder as the example.

#-*- coding: iso-8859-15 -*-

''' Whole Body Motion: Enable Balance Constraint '''

import config
import math

def main():
    ''' Example of a whole body Enable Balance Constraint
    Warning: Needs a PoseInit before executing
             Whole body balancer must be inactivated at the end of the script
    '''

    motionProxy = config.loadProxy("ALMotion")

    # Set NAO in Stiffness On
    config.StiffnessOn(motionProxy)

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

    # Activate Whole Body Balancer
    isEnabled  = True
    motionProxy.wbEnable(isEnabled)

    # Legs are constrained in a plane
    stateName  = "Fixed"
    supportLeg = "Legs"
    motionProxy.wbFootState(stateName, supportLeg)

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

    # KneePitch angleInterpolation
    # Without Whole Body balancer, foot will fall down
    names      = ["LKneePitch", "RKneePitch"]
    angleLists = [ [0.0, 40.0*math.pi/180.0], [0.0, 40.0*math.pi/180.0]]
    timeLists  = [ [5.0, 10.0], [5.0, 10.0]]
    isAbsolute = True
    motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)

    # Disactivate Whole Body Balancer
    isEnabled  = False
    motionProxy.wbEnable(isEnabled)

if __name__ == "__main__":
    main()

case 3: safe cartesian control¶

It is a user friendly API which enable whole body cartesian control of an effector:

Head Orientation
LArm/RArm Position.

It is useful for reactive behaviour. User can update the target (Head Orientation, Arm Position) and motion is safe and smooth using a SE3 Interpolation.

How it works¶

The effector target is reach following the next conditions:

Keep the two feet fixed and on the ground
Keep balance
Redundancy: reach the target by keeping as possible an init pose.

How to use it¶

First of all, choose the effector you want to control, with the api wbEnableEffectorControl. It implicitly activate Whole Body Balancer.

# Example showing how to active Head tracking.
effectorName = 'Head'
isEnabled = True
proxy.wbEnableEffectorControl(effectorName, isEnabled)

Update as you want the target with api wbSetEffectorControl:

Head is controlled in rotation (WX, WY, WZ) (radian).
LArm and RArm are controlled in position (X, Y, Z) (meter).

TargetCoordinate must be absolute and expressed in SPACE_NAO.

If the desired position/orientation is unfeasible, target is resize to the nearest feasible motion.

# Example showing how to set orientation target for Head tracking.
effectorName     = "Head"
targetCoordinate = [0.1, 0.0, 0.0]
proxy.wbSetEffectorControl(effectorName, targetCoordinate)

At last, think to disable effector control.

# Example showing how to disactivate Head tracking.
effectorName = 'Head'
isEnabled = False
proxy.wbEnableEffectorControl(effectorName, isEnabled)

Note

Do an init posture before using this api.

Examples¶

Head Orientation: motion_wbEffectorControlHead.py

Arm Position: motion_wbEffectorControlArm.py

NAO Software 1.12 documentation

Whole Body control¶

What it does¶

How it works¶

Whole Body Balancer¶

Whole Body Balancer - detailed¶

Getting started¶

When use it¶

How to activate/deactivate it¶

Use Cases¶

Case 1: Feet Condition¶

Case 2: Balance Constraint¶

case 3: safe cartesian control¶

How it works¶

How to use it¶

Examples¶

Table Of Contents

On this page

NAO Software 1.12 documentation

Whole Body control¶

What it does¶

How it works¶

Whole Body Balancer¶

Whole Body Balancer - detailed¶

Getting started¶

When use it¶

How to activate/deactivate it¶

Use Cases¶

Case 1: Feet Condition¶

Case 2: Balance Constraint¶

case 3: safe cartesian control¶

How it works¶

How to use it¶

Examples¶

Quick search

Table Of Contents

On this page