{ "cells": [ { "cell_type": "markdown", "id": "bb99415c", "metadata": {}, "source": [ "### The role of thumb\n", "\n", "The lower priority task is satisfied only in the null space of the higher priority task, the concept being extensible to multiple tasks.\n", "\n", "$J(I-J^{\\dagger}J) = 0$\n", "\n", "$N = I - J^{\\dagger}J$\n", "\n", "\n", "For equation:\n", "\n", "$$\\dot{q} = J^\\dagger\\dot{r} + (I- J^\\dagger J)q_0$$\n", "\n", "Properties of $N = I - J^\\dagger J$ \n", "* symmetric aka Hermitian $N = N^T$\n", "* idempotent: $N^2 = N$\n", "* $N^\\dagger = N$ \n", "* $J^\\dagger\\dot{r}$ is orthogonal to $(I- J^\\dagger J)q_0$\n", "\n", "$\\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 \n", "\n", "**Here i and i-1 refer to i-th task ans i-1 th task**\n", "\n", "\n", "\n", "\n", "\n", "\n", "\n", "\n", "\n", "\n", "# Multi-tasks Architecture - Null space projection\n", "\n", "\n", "## What\n", "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\".\n", "\n", "## How\n", " \n", "Kim [2] Section IV-A is used to show the details of null space technique and how secondary tasks DO NOT influence prior tasks.\n", "#### Definition\n", "Before start, we need define null space projection. \n", "The null space projection of matrix $J$: \n", "$$N = I- J^\\dagger J$$\n", "\n", "#### Properties of null space projection \n", "* $JN= 0$\n", "* Symmetric aka Hermitian $N = N^T$\n", "* Idempotent: $N^2 = N$\n", "This could be easily proved by definition and $JJ^\\dagger J = J$ or $J^\\dagger JJ^\\dagger = J^\\dagger$\n", "* $N^\\dagger = N$\n", "* For equation:\n", "\n", "$$\\dot{q} = J^\\dagger\\dot{r} + (I- J^\\dagger J)q_0$$\n", "\n", "$J^\\dagger\\dot{r}$ is orthogonal to $(I- J^\\dagger J)q_0$\n", "\n", "#### Equation\n", "$$\\Delta q_i = \\Delta q_{i-1} + J_{i | pre}^\\dagger(e - J_i\\Delta q_{i-1})$$\n", "$$N_0 = I - J_0^\\dagger J_0$$\n", "\n", "\n", "Notice $J_0 = J_c$ here\n", "\n", "\n", "With assumption $J_i \\Delta q_i = \\Delta x_i$\n", "\n", "When $i = 0$\n", "$\\Delta q_0 = J_0^{\\dagger} \\Delta x_0$\n", "\n", "\n", "**When $i = 1$**\n", "$$\n", "\\begin{equation}\n", "\\begin{split} \\Delta q_1\n", "&=\\Delta q_0 + J_{1 | pre}^\\dagger \\Delta x_1\\\\\n", "&=\\Delta q_0 + (J_1N_0)^\\dagger \\Delta x_1\\\\\n", "&=\\Delta q_0 + N_0(J_1N_0)^\\dagger \\Delta x_1\\\\\n", "\\end{split}\n", "\\end{equation}\n", "$$\n", "Notice we use a magic property,\n", "if $C$ is **idempotent** and **Hermitian**, then\n", "$$C(BC)^\\dagger = (BC)^\\dagger$$\n", "\n", "which is mentioned in \"Task-priority formulations for the kinematic control of highly redundant\n", "articulated structures\"\n", "and proved in \"Obstacle Avoidance for Kinematically Redundant Manipulators in\n", "Dynamically Varying Environments\"\n", "\n", "Considering $J_0(I - J_0^\\dagger J_0) = 0$\n", "\n", "$$\n", "\\begin{equation}\n", "\\begin{split} J_0\\Delta q_1 \n", "&= J_0\\Delta q_0 + J_0N_0(J_1N_0)^\\dagger \\Delta x_1\\\\\n", "&= J_0\\Delta q_0\\\\\n", "&=\\Delta x_0\n", "\\end{split}\n", "\\end{equation}\n", "$$\n", "\n", "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.\n", "\n", "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\n", "\n", "**When $i = 2$**\n", "$$\n", "\\begin{equation}\n", "\\begin{split} \\Delta q_2\n", "&=\\Delta q_1 + J_{2 | pre}^\\dagger \\Delta x_2\\\\\n", "&=\\Delta q_1 + (J_2N_1)^\\dagger \\Delta x_2\\\\\n", "&=\\Delta q_1 + N_1(J_2N_1)^\\dagger \\Delta x_2\\\\\n", "\\end{split}\n", "\\end{equation}\n", "$$\n", "J1 equation still hold\n", "\n", "since $N_1 = N_0 -(J_1N_0)^\\dagger(J_1N_0)$\n", "again\n", "since $N_1 = N_0 - N_0(J_1N_0)^\\dagger(J_1N_0)$\n", "or \n", "$N_1 = N_0 [I - (J_1N_0)^\\dagger(J_1N_0)]$\n", "So $J_0N_1 = 0$\n", "\n", "The equation become\n", "$$\n", "\\begin{equation}\n", "\\begin{split} J_0\\Delta q_2\n", "&= J_0\\Delta q_1 + J_0N_1(J_2N_1)^\\dagger \\Delta x_2\\\\\n", "&=\\Delta x_0 + 0\\\\\n", "&=\\Delta x_0\n", "\\end{split}\n", "\\end{equation}\n", "$$\n", "\n", "**When $i$**\n", "\n", "\n", "How to understand i and i-1\n", "\n", "\n", "i is for i-th articulated task result, it's the effect of all tasks from 0, 1....i, not only i-th task.\n", "\n", "Reconsidering equation\n", "\n", "$$\\Delta q_i - \\Delta q_{i-1} = J_{i | pre}^\\dagger(e - J_i\\Delta q_{i-1})$$\n", "\n", "The actual q change achieved by i-th task is $\\Delta q_i - \\Delta q_{i-1}$\n", "\n", "In right side \n", "\n", "$e$ is all error we have\n", "$J_i\\Delta q_{i-1}$ are errors has been eliminated by first i-1th tasks.\n", "\n", "$e - J_i\\Delta q_{i-1}$ is actual $\\ x_i$ for i-th task\n", "\n", "[1] A. Liégeois, “Automatic supervisory control of the coDeltanfigura-\n", "tion and behavior of multibo\n", "dy mechanisms,” IEEE Trans. Sys.\n", "Man Cybernet., vol. 7, pp. 868–871, 1977.\n", "\n", "[3] A. A. Maciejewski and C. A. Klein, “Obstacle avoidance for\n", "kinematically redundant manipulators in dynamically varying\n", "environments,” Int. J. Robot. Res., vol. 4, no. 3, pp. 109–117,\n", "1985.\n", "\n", "[2] Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control" ] }, { "cell_type": "code", "execution_count": null, "id": "e3255272", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.11.0" } }, "nbformat": 4, "nbformat_minor": 5 }