.. _control-wholebody: Whole Body control ================== .. toctree:: :hidden: :maxdepth: 1 control-wholebody-api control-wholebody-tuto Overview | :ref:`API ` | :ref:`Tutorial ` .. seealso:: - :ref:`naoqi-motion` ------------ 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 :ref:`joint` (and choregraphe) or :ref:`cartesian`: * :cpp:func:`ALMotionProxy::wbFootState` * :cpp:func:`ALMotionProxy::wbEnableBalanceConstraint` * :cpp:func:`ALMotionProxy::wbGoToBalance` * safe cartesian control methods: * :cpp:func:`ALMotionProxy::wbEnableEffectorControl` * :cpp:func:`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\ :sup:`1`. The classical form of a quadratic program is: .. math:: \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. - :math:`Y`: Unknown vector; - :math:`Y^{\text{des}}`: Desired but not necessarily feasible solution; - :math:`Q`: Quadratic norm; - :math:`A`, :math:`b`, :math:`C` and :math:`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 :term:`support polygon`. We can activate/disactivate this constraint and specify the :term:`support polygon`. Y\ :sup:`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. \ :sup:`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. 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 :ref:`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. .. code-block:: python # 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 (:cpp:func:`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. .. code-block:: python # 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. :download:`motion_wbFootState.py ` To be executed, this example require a config file: :download:`config.py `. Modify the config file to enter your robot IP, and place it in the same folder as the example. .. literalinclude:: /examples/python/motion/motion_wbFootState.py :language: py 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. .. code-block:: python # 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 :ref:`alrobotpose`) :download:`motion_wbEnableBalanceConstraint.py ` To be executed, this example require a config file: :download:`config.py `. Modify the config file to enter your robot IP, and place it in the same folder as the example. .. literalinclude:: /examples/python/motion/motion_wbEnableBalanceConstraint.py :language: py 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. .. code-block:: python # 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. .. code-block:: python # 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. .. code-block:: python # 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: :download:`motion_wbEffectorControlHead.py ` Arm Position: :download:`motion_wbEffectorControlArm.py `