Whole Body Motion
It is a powerful tool which gives very natural and safe motion.
The main goal is to generate and stabilized consistent motions and adapt nao's behaviour to the situation.
The two main functionalities are:
- Effector Control: The goal is to control an effector in space.
- Balance: The main idea is to stabilize pre-computed motion like choregraphe joints timeline.
How it works
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 same problem. The motion obtained guaranty several specified conditions like balance, keeping foot fix.
Moreover, when asking nao to reach out his arm, he bends over because arms but also legs joints are taking into account. And he stops to reach to much in order to keep his balance.
This behaviour is automatically managed by Whole Body Balancer. User only asks nao to reach out his arm.
HipYawPitch joint coupling is implicitly taking into account in the balancer.
When using it
This balancer can be used with every joint control (angle interpolation, choregraphe timeline) and effector control.
It can not be used during walk.
| Warning: |
At present, Whole Body balancer is useful for standing but not seated posture. |
|---|
How to activate/disactivate it
The Whole Body Motion is by default disactivated 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 disactivate Whole Body Balancer at the end of your behaviour. |
|---|
Advanced
The generalized inverse kinematic problem is writing as a quadratic programming. We solve each step (every 20 ms) a quadratic programming on the robot. We used the C++ open source library qpOases1 to solve this problem.
The classical form of a quadratic programming is:
- Y: Unknown vector;
- Ydes: Desired but not necessarily accessible 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 of 6 degrees of freedom (3 translations, 3 rotations);
- Velocity of all the articulated joints. LHand and RHand are not taking into account because it is not part of kinematics chain. Moreover, LHipYawPitch and RHipYawPitch are only one variable because these articulations are controlled by only one actuator.
The quadratic minimization is composed of:
- Cartesian desired trajectories (section Effector Control);
- Articular desired trajectories (section Balance).
These orders are not necessary feasible. The solution obtained is a compromise between desired and feasible motion.
The equality constraints are about keeping feet fix or plane (section Balance).
The inequality constraints are:
- Joint limits. These constraints are always taking into account;
- Balance. The Center Of Mass is constrained in the support polygon. We can activate/disactivate this constraint and specify the support polygon (section Balance).
References
1 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.