Analytical Whole Body Control, Prioritized Recursive Nullspace

Oct 2, 2023 | Tech Robotics

Motivation

A robot can have more degrees-of-freedom (DOF) than the DOF needed to accomplish a certain task. In that case, one may want a controller that (1) guarantees that robot utilizes its DOF to achieve the primary task, (2) without conflicting the primary task, employs the remaining DOF to perform some other tasks as much as possible.

Take a humanoid robot for example, which may have more than 20 DOF. It could have one of its hands holding an object. Without changing the object's position and orientation in space, the robot could use its remaining DOF to change its body posture, or avoid collision with another coming object. The so-called Whole Body Controller was proposed specifically to accomplish such goals.

This post summarizes and physically interprets some results presented in 2005@Sentis and 2007@Sentis , which laid the very foundation of the analytical Whole Body Control.

Demonstration

Below demonstrates: (1) an 8-DOF robot employs whole-body control with 2 tasks to fix its end-effector in space while moving its posture; and (2) a 15-DOF robot employs whole-body control with 3 tasks to fix its left and right hands in space, while moving its posture. Notice that the prioritized whole body control allows the robots to move their posture without affecting their primary tasks.
‎Check network for missing images.
‎Check network for missing images.

Nomenclature

In addition to nomenclature used in Fundamentals of Dynamics and Control of Flexible Joint Robots and Operational Space Framework, Hybrid Motion-Force Control:

  • $\Gamma_{wbc}$ - analytical whole body control joint torque
  • $\Gamma$ - generalized joint torque
  • $F$ - generalized task force
  • $N$ - nullspace projector
  • $N_{pre(i)}$ - nullspace projector that is dynamically consistent with all preceding tasks $1$ to $i-1$
  • $\mathcal{X}_{i}$ - quantity $\mathcal{X}$ of the $i$-th task
  • $\mathcal{X}_{i|pre(i)}$ - quantity $\mathcal{X}$ of the $i$-th task that is dynamically consistent with all preceding tasks $1$ to $i-1$, typically $\mathcal{X}_{i|pre(i)} = \mathcal{X}_{i} N_{pre(i)}$

Note that $\mathcal{X}$ is a wildcard symbol. It could represent $J$, $F$, $\Gamma$, $\Lambda$, $\mu$, $\rho$, etc.

Controlling a Cartesian Task while Regulating Robot Posture

As discussed in Operational Space Framework, Hybrid Motion-Force Control, the dynamics of a fixed-base robot is given by $$ M\ddot{q} + c + g = \Gamma $$ To achieve a single task, such as an operational space Cartesian motion control task, a controller in the form of $$ \Gamma = J^T F $$ would do. With redundant DOF, in additional to achieving a Cartesian control task, one typically want the robot to maintain a certain posture as much as possible, without affecting the primary Cartesian control task. In this case, a controller can be designed to have the form of $$ \Gamma = \Gamma_{1} + N^T_{1} \Gamma_{2} $$ where

  • $ \Gamma_1 = J_1^T F_1 $ denotes the Cartesian task control torque, with $J_1$ mapping joint velocity to Cartesian task velocity $\dot{x} = J_1 \dot{q}$, and
  • $ \Gamma_2 = J_2^T F_2 $ denotes the posture task control torque, with $J_2 = I$ mapping joint velocity to posture task velocity $\dot{q}= J_2 \dot{q} = I \dot{q}$, since posture task is in the joint space.

As for the nullspace projector $N_1$, if one employs $$ N_1 = I - \bar{J}_{1} J_{1} $$ by selecting $\bar{J}_{1}$ to be the dynamically consistent generalized inverse of the Jacobian of the Cartesian task, it is guaranteed that the posture task torque/acceleration will not affect the Cartesian task, as shown in the reference. And such a nullspace project is said to be dynamically consistent with the primary task.

Physically, it is analogous to solving a system with less equations than unknowns. There are infinitely many solutions to choose from. Among them, one can choose the ones that meet certain criteria.

Generalization to Controlling Multiple Tasks with Strict Priorities

The key insight from the previous section is that, if the nullspace projector is dynamically consistent with higher priority tasks, the task torque will try to achieve its goal (as long as there are still enough DOF to do so) without affecting those higher priority tasks.

A natural way to generalize such an idea is to project a lower priority task torque by the nullspace projector that is dynamically consistent with its immediately higher priority tasks, recursively. Such a controller can be written as $$ \Gamma_{wbc} = \Gamma_1 + N^T_1 \Bigl( \Gamma_2 + N_2^T \Bigl( \Gamma_3 + \cdots \Bigl) \Bigl) \label{eq:multi-task-control} $$ One can see very clearly from \eqref{eq:multi-task-control} that

  • each low priority task torque is first projected by the nullspace of its immediately higher priority task, then
  • is added with the torque of its immediately higher priority task, and
  • the pattern repeats in a recursive fashion.

Closed-Form Formula of the Generalized Multitask Controller

It is shown in 2007@Sentis that the recursive equation in \eqref{eq:multi-task-control} has a very elegant closed-form formula in the form of $$ \Gamma_{wbc} = \Gamma_{1|pre(1)} + \Gamma_{2|pre(2)} + \Gamma_{3|pre(3)} + \cdots + \Gamma_{N|pre(N)} = \sum_{k=1}^{N}\Gamma_{k|pre(k)} \label{eq:total-torque} $$ where $$ \Gamma_{k|pre(k)} \triangleq J^T_{k|pre(k)} F_{k|pre(k)} $$ and $$ F_{k|pre(k)} = \Lambda_{k|pre(k)}F_k + \mu_{k|pre(k)} + p_{k|pre(k)} $$ and \begin{align} \Lambda_{k|pre(k)} &= (J_{k|pre(k)} M^{-1} J^T_{k|pre(k)})^{-1} \label{eq:task-inertia} \\ u_{k|pre(k)} &= \bar{J}_{k|pre(k)} c - \Lambda_{k|pre(k)} \dot{J}_{k|pre(k)} \dot{q} \\ p_{k|pre(k)} &= \bar{J}_{k|pre(k)} g \end{align} where $$ \begin{align} J^T_{k|pre(k)} &\triangleq J_k N_{pre(k)} \\ \bar{J}_{k|pre(k)} &= M^{-1} J_{k|pre(k)}\Lambda_{k|pre(k)} \end{align} $$ and $$ N_{pre(1)} \triangleq I \label{eq:initial-nullspace} $$ $$ N_{pre(k)} = I - \sum_{i=1}^{k-1} \bar{J}_{i|pre(i)} J_{i|pre(i)} $$ which can also be written as $$ N_{pre(k)} = N_{pre(k-1)} - \bar{J}_{k-1|pre(k-1)} J_{k-1|pre(k-1)} \label{eq:recursive-nullspace} $$ with detailed derivation given in Sec. 3 in 2007@Sentis .

Physically, equations \eqref{eq:total-torque} to \eqref{eq:recursive-nullspace} can be interpreted as:

  • the total joint torque is a summation of all task torques each of which being dynamically consistent with its preceding tasks,
  • to achieve the prioritized dynamic consistency, all task quantities have to be computed using the recursive nullspace,
  • Eq. \eqref{eq:initial-nullspace} indicates that the recursive nullspace starts from an identity matrix (fully available),
  • Eq. \eqref{eq:recursive-nullspace} indicates that for each succeeding task, the nullspace equals to the remaining nullspace subtracting some amount (getting smaller) that is decided by the multiplication of the dynamically consistent generalized inverse of the task Jacobian and itself, in a recursive fashion.

Practical Simplification

In practice, the analytical whole body control law can be simplified by subtracting the Coriolis and gravity forces from the joint space first, that is $$ M\ddot{q} + c + g = \Gamma_{wbc} + c + g $$ With the Coriolis and gravity force already compensated for in the joint space, the whole body control law $$ \Gamma_{wbc} = \sum_{k=1}^{N}\Gamma_{k|pre(k)} $$ simplifies to $$ \Gamma_{k|pre(k)} = J^T_{k|pre(k)} \Lambda_{k|pre(k)} F_k - \Lambda_{k|pre(k)} \dot{J}_{k|pre(k)} \dot{q} $$ and because the recursive computation of dynamically consistent task Jacobian derivative is computationally much heavier than other terms, people often replace it with the task Jacobian derivative $\dot{J}_k$ in $\Lambda_{k|pre(k)} \dot{J}_{k|pre(k)} \dot{q}$, or completely ignore that term all together. Whereas the rest of the terms remain the same, that is \begin{align} J^T_{k|pre(k)} &\triangleq J_k N_{pre(k)} \\ \bar{J}_{k|pre(k)} &= M^{-1} J_{k|pre(k)}\Lambda_{k|pre(k)} \end{align} and $$ N_{pre(1)} \triangleq I $$ $$ N_{pre(k)} = N_{pre(k-1)} - \bar{J}_{k-1|pre(k-1)} J_{k-1|pre(k-1)} $$

Limitations

Through the analytical whole body control is elegant and computationally efficient, there are limitations to this approach, such as:

  • it assumes that the inverse exists when computing the dynamically consistent task inertia in \eqref{eq:task-inertia},
  • it is difficult to integrate constraints into the computation due to its analytical nature,
  • some constraints can be formulated as individual tasks but each of them would take a unique form and hard to generalize.

More on Whole Body Control

For simplicity, this post only presents the analytical whole body control with the fixed-base robot model as an example. The method also can be generalized to the floating-base case, which was how this method was originally proposed for, such as shown in 2006@Park .

Furthermore, they are other methods that solves the whole body control problem that can be more general than the analytical whole body control approach. Such as the hierarchical optimization based whole body control method proposed in 2017@EthRsl and 2016@Bellicoso .

These will be discussed in separate posts.