The role of thumb

The lower priority task is satisfied only in the null space of the higher priority task, the concept being extensible to multiple tasks.

\(J(I-J^{\dagger}J) = 0\)

\(N = I - J^{\dagger}J\)

For equation:

\[\dot{q} = J^\dagger\dot{r} + (I- J^\dagger J)q_0\]

Properties of \(N = I - J^\dagger J\)

  • symmetric aka Hermitian \(N = N^T\)

  • idempotent: \(N^2 = N\)

  • \(N^\dagger = N\)

  • \(J^\dagger\dot{r}\) is orthogonal to \((I- J^\dagger J)q_0\)

\(\Delta q_i\) is eventually returned variable, it means the \(\Delta q\) iterative result after achieve \(i\)-th task. It comes from \(\Delta q_{i-1}\) with

Here i and i-1 refer to i-th task ans i-1 th task

Multi-tasks Architecture - Null space projection

What

When we deal with robot control problem, we may have multi-tasks. For example, we need end-effector at certain location and if possible in a special orientation or vice versa. Here we said that location goal has higher priority than orientation task, orientation task is so-called secondary tasks. The whole idea come from “The secondary task movement should not influence prioritized task goal”.

How

Kim [2] Section IV-A is used to show the details of null space technique and how secondary tasks DO NOT influence prior tasks. #### Definition Before start, we need define null space projection. The null space projection of matrix \(J\):

\[N = I- J^\dagger J\]

Properties of null space projection

  • \(JN= 0\)

  • Symmetric aka Hermitian \(N = N^T\)

  • Idempotent: \(N^2 = N\) This could be easily proved by definition and \(JJ^\dagger J = J\) or \(J^\dagger JJ^\dagger = J^\dagger\)

  • \(N^\dagger = N\)

  • For equation:

\[\dot{q} = J^\dagger\dot{r} + (I- J^\dagger J)q_0\]

\(J^\dagger\dot{r}\) is orthogonal to \((I- J^\dagger J)q_0\)

Equation

\[\Delta q_i = \Delta q_{i-1} + J_{i | pre}^\dagger(e - J_i\Delta q_{i-1})\]
\[N_0 = I - J_0^\dagger J_0\]

Notice \(J_0 = J_c\) here

With assumption \(J_i \Delta q_i = \Delta x_i\)

When \(i = 0\) \(\Delta q_0 = J_0^{\dagger} \Delta x_0\)

When :math:`i = 1`

\[\begin{split}\begin{equation} \begin{split} \Delta q_1 &=\Delta q_0 + J_{1 | pre}^\dagger \Delta x_1\\ &=\Delta q_0 + (J_1N_0)^\dagger \Delta x_1\\ &=\Delta q_0 + N_0(J_1N_0)^\dagger \Delta x_1\\ \end{split} \end{equation}\end{split}\]

Notice we use a magic property, if \(C\) is idempotent and Hermitian, then

\[C(BC)^\dagger = (BC)^\dagger\]

which is mentioned in “Task-priority formulations for the kinematic control of highly redundant articulated structures” and proved in “Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying Environments”

Considering \(J_0(I - J_0^\dagger J_0) = 0\)

\[\begin{split}\begin{equation} \begin{split} J_0\Delta q_1 &= J_0\Delta q_0 + J_0N_0(J_1N_0)^\dagger \Delta x_1\\ &= J_0\Delta q_0\\ &=\Delta x_0 \end{split} \end{equation}\end{split}\]

Here we proof that our \(\Delta q_1\) DO NOT influence achievement of \(\Delta x_0\). In other words, task 1’s q result doesn’t influence task 0.

Before start iterating, it’s necessary to emphasis that delta q1 we get here isn’t for task 1 but a really nice q after first iteration that could achieve all previous tasks(task0 + task1). From here we could think the tasks before i = 2 as one big task

When :math:`i = 2`

\[\begin{split}\begin{equation} \begin{split} \Delta q_2 &=\Delta q_1 + J_{2 | pre}^\dagger \Delta x_2\\ &=\Delta q_1 + (J_2N_1)^\dagger \Delta x_2\\ &=\Delta q_1 + N_1(J_2N_1)^\dagger \Delta x_2\\ \end{split} \end{equation}\end{split}\]

J1 equation still hold

since \(N_1 = N_0 -(J_1N_0)^\dagger(J_1N_0)\) again since \(N_1 = N_0 - N_0(J_1N_0)^\dagger(J_1N_0)\) or \(N_1 = N_0 [I - (J_1N_0)^\dagger(J_1N_0)]\) So \(J_0N_1 = 0\)

The equation become

\[\begin{split}\begin{equation} \begin{split} J_0\Delta q_2 &= J_0\Delta q_1 + J_0N_1(J_2N_1)^\dagger \Delta x_2\\ &=\Delta x_0 + 0\\ &=\Delta x_0 \end{split} \end{equation}\end{split}\]

When :math:`i`

How to understand i and i-1

i is for i-th articulated task result, it’s the effect of all tasks from 0, 1….i, not only i-th task.

Reconsidering equation

\[\Delta q_i - \Delta q_{i-1} = J_{i | pre}^\dagger(e - J_i\Delta q_{i-1})\]

The actual q change achieved by i-th task is \(\Delta q_i - \Delta q_{i-1}\)

In right side

\(e\) is all error we have \(J_i\Delta q_{i-1}\) are errors has been eliminated by first i-1th tasks.

\(e - J_i\Delta q_{i-1}\) is actual \(\ x_i\) for i-th task

[1] A. Liégeois, “Automatic supervisory control of the coDeltanfigura- tion and behavior of multibo dy mechanisms,” IEEE Trans. Sys. Man Cybernet., vol. 7, pp. 868–871, 1977.

[3] A. A. Maciejewski and C. A. Klein, “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments,” Int. J. Robot. Res., vol. 4, no. 3, pp. 109–117,

[2] Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control

[ ]: