{ "cells": [ { "cell_type": "markdown", "id": "3bff74e2", "metadata": {}, "source": [ "# Dynamically Consistent Pseudo Inverse\n", "\n", "This document introduces the generalized null space formulation of both kinematics and dynamics. Clearly, they share same underlying mathematical foundation. Moreover, In a redundant system, for inverse of $J$ compliance with operational acceleration constrain, we could deduce the formulation of inverse, $\\bar J$, by involving mass matrix. $\\bar J$ is named dynamically consistent pseudo-inverse.\n", "\n", "\n", "## 1. Mapping\n", "\n", "### 1. 1 Kinematics\n", "In kinematics, we have\n", "$$\n", "\\begin{equation}\n", "J\\dot q = \\dot x\n", "\\end{equation}\n", "$$\n", "\n", "in which \n", "$J$ is a Jacobian projection\n", "$\\dot q$ is in configuration space\n", "$\\dot x$ is in task space\n", "\n", "### 1.2 Dynamics\n", "In dynamics, we have $\\tau = J^Tf$ in consistent with general formula, we rewrite it as\n", "\n", "$$\n", "\\begin{equation}\n", "J^{T\\sharp}\\tau = f\n", "\\end{equation}\n", "$$\n", "\n", "in which\n", "$J^{T\\sharp}$ is projection\n", "$\\tau$ is joint torque in configuration space\n", "$f$ is operational force in task space\n", "\n", "Notice\n", "* $M^\\sharp$ is pseudo-inverse of $M$. Here we use $\\sharp$ refer pseudo-inverse while some paper using $\\dagger$.\n", "* $(A^{-1})^T = (A^T)^{-1}$ or $(A^\\sharp)^T = (A^T)^\\sharp$ and proof is shown in appendix.\n", "\n", "\n", "### 1.3 Generalize\n", "Both kinematics and dynamics formulate a mapping from configuration space to task space. The controllable variable is the item in configuration space. In general, the formulation is\n", "\n", "$$\n", "\\begin{equation}\n", "Pc = t\n", "\\end{equation}\n", "$$\n", "\n", "Here\n", "$P$ is a projection. $P = J$ in kinematics while $P = J^{T\\sharp}$ in dynamics\n", "$c$ is in configuration space\n", "$t$ is in task space\n", "\n", "## 2. Null space of the Mapping\n", "In a redundant system, null space method could be used to achieve manipulation in configuration space without affecting the resulting status in task space. For a matrix transformation like $Pc = t$, the null space of $P$ is \n", "\n", "$$\n", "\\begin{equation}\n", "N = I - P^{\\sharp }P\n", "\\end{equation}\n", "$$\n", "\n", "For redundant mechanism, the control of configuration space\n", "$$\n", "\\begin{equation}\n", "c = P^\\sharp t + (I - P^\\sharp P)V_0\n", "\\end{equation}\n", "$$\n", "\n", "where $V_0$ is a arbitrary value\n", "\n", "* For kinematics\n", "\n", "$$\n", "\\begin{equation}\n", "\\dot q = J^\\sharp \\dot x + (I - J^\\sharp J)q_0\n", "\\end{equation}\n", "$$\n", "\n", "* For dynamics,\n", "\n", "$$\n", "\\begin{equation}\n", "\\tau = J^Tf + (I - J^TJ^{T\\sharp})\\tau_0\n", "\\end{equation}\n", "$$\n", "\n", "## 3. Where are we\n", "The whole story start from goal of dynamically control which means solve the torque from a desired trajectory. \n", "\n", "* At the very beginning, we have a desired trajectory $x(t)$. \n", "* Then we may close velocity loop of hardware controller by differentiate forward kinematics $\\dot x = J(q) \\dot q$ and input $\\dot q = J^{-1}\\dot x$ to motor controller.\n", "* Finally, we may differentiate Jacobin mapping one more time to get the relationship between $\\ddot x$ and $\\ddot q$\n", "\n", "$$\n", "\\begin{equation}\n", "\\ddot x - \\dot J \\dot q= J \\ddot q\n", "\\end{equation}\n", "$$\n", "* We could calculate $\\ddot q$ by\n", "\n", "$$\n", "\\begin{equation}\n", "\\ddot q = J^\\sharp(\\ddot x - \\dot J \\dot q)\n", "\\end{equation}\n", "$$\n", "\n", "In a redundant system, in theory, there is an infinity of configuration status that could be applied without affecting the resulting status in task space. There are an infinite number of joint torques that could be applied to a redundant manipulator without affecting the resulting forces at the end effector. So there are infinity pseudo-inverse to finish this mission. \n", "**PROBLEM 1**: We need to find a pseudo-inverse from all infinity\n", "\n", "\n", "Well, my hardware colleague warns me there isn't such magic in the world to allow me control acceleration directly. He said if I recall $F = ma$ in junior school, you may understand you have to go through force to control acceleration. In motor case, through $\\tau$ to control $\\ddot q$.\n", "**PROBLEM 2**: To finish control, there is a relationship between $\\ddot q$ and $\\tau$ need to be figured.\n", "\n", "\n", "## 4. Extra dynamic constrain\n", "\n", "To emphasis above-mentioned problems, considering the derivative of $\\dot x = J\\dot q$,\n", "$$\n", "\\begin{equation}\n", "\\ddot x - \\dot J \\dot q= J \\ddot q\n", "\\end{equation}\n", "$$\n", "and the dynamic model equation\n", "$$\n", "\\begin{equation}\n", "A\\ddot q + b + g = \\tau\n", "\\end{equation}\n", "$$ \n", "\n", "bring $\\ddot q$ of second equation to first equation, we get a equation between controllable $\\tau$ and acceleration of task space\n", "\n", "$$\n", "\\begin{equation}\n", "\\begin{split} \\ddot x - \\dot J \\dot q + JA^{-1}(b + g) \n", "&= JA^{-1}\\tau\\\\\n", "&= JA^{-1}[J^Tf + (I - J^TJ^{T\\sharp})\\tau_0]\\\\\n", "&= JA^{-1}J^Tf + JA^{-1}(I - J^TJ^{T\\sharp})\\tau_0\n", "\\end{split}\n", "\\end{equation}\n", "$$\n", "\n", "This equation shows that the acceleration in task space is influenced by $\\tau_0$ unless the term involving $\\tau_0$ is 0. For joint torque not to producing any operational acceleration, we must have\n", "$$\n", "\\begin{equation}\n", "JA^{-1}[I-J^TJ^{\\sharp T}]\\tau_0 = 0\n", "\\end{equation}\n", "$$\n", "which result in a unique pseudo-inverse\n", "$$\n", "\\begin{equation}\n", "J^\\sharp = \\bar J = A^{-1}J^T\\Lambda\n", "\\end{equation}\n", "$$\n", "where $\\Lambda = (JA^{-1}J^T)^{-1}$. This is called **dynamically consistent pseudo-inverse** and usually refer $\\bar J$. The proof is shown in appendix.\n", "\n", "Now, by using extra dynamical constrain, we get this particular pseudo inverse $\\bar J$ which should be use in control equation $\\ddot q = \\bar J (\\ddot x - \\dot J \\dot q)$ and eventually input torque $\\tau$ to motor by $\\tau = A\\ddot q + b + g - J^Tf$.\n", "\n", "\n", "## 5. Conclusion\n", "In a redundant system, since there are more controllable joints in configuration space than needed. We could distribute operational space task to configuration space in a smarter way, so-called *task oriented* control by Oussama Khatib. That means we could use different pseudo inverse achieve different optimized goals.\n", "In kinematics, pseudo inverse, $J^{-1}$, by SVD minimizes the least square of error.\n", "In contrast, dynamic constrain pseudo inverse, $\\bar J$, by bring mass matrix in account to minimizes instantaneous kinetic energy.\n", "\n", "Later Roy Featherstone showed that this $\\bar J$ is independent of load in Roy (1997). This might be a torch of future research.\n", "\n", "## APPENDIX\n", "\n", "### APPENDIX.A\n", "$$\n", "A^T(A^{-1})^T = (A^{-1}A)^{T} = I^T = I \\\\\n", "(A^{-1})^TA^T = (AA^{-1})^{T} = I^T = I\n", "$$\n", "This proves that the inverse of $A^T$ is $(A^{-1})^T$\n", "### APPENDIX.B\n", "THEOREM: DYNAMIC CONSISTENCY, A generalized inverse that is consistent with the dynamic constraint of equation\n", "$$[JA^{-1} - (JA^{-1}J^T)J^{\\sharp T}]\\tau_0 = 0$$\n", "is unique and is given by\n", "$$J^\\sharp = \\bar J = A^{-1}J^T\\Lambda$$\n", "where \n", "$$\\Lambda = (JA^{-1}J^T)^{-1}$$\n", "\n", "PROOF\n", "To achieve\n", "$$[JA^{-1} - (JA^{-1}J^T)J^{\\sharp T}]\\tau_0 = 0$$\n", "by definition\n", "$$\\Lambda^{-1} = JA^{-1}J^T$$\n", "the constrain equation premultiplies of $\\tau_0$ has be 0, that is\n", "$$JA^{-1} - \\Lambda^{-1}J^{\\sharp T} = 0$$\n", "So, we get\n", "$$J^{\\sharp T} = \\Lambda JA^{-1}$$\n", "Considering inertial matrix $A$ is a symmetric positive define matrix(aka $A^T = A$), finally we have\n", "$$\n", "\\begin{split} J^\\sharp = (J^{\\sharp T})^T\n", "&=(\\Lambda JA^{-1})^T\\\\\n", "&=A^{-T}J^T\\Lambda^T\\\\\n", "&=A^{-1}J^T((JA^{-1}J^T)^{-1})^T\\\\\n", "&=A^{-1}J^T((JA^{-1}J^T)^T)^{-1}\\\\\n", "&=A^{-1}J^T(JA^{-T}J^T)^{-1}\\\\\n", "&=A^{-1}J^T(JA^{-1}J^T)^{-1}\\\\\n", "&=A^{-1}J^T\\Lambda\n", "\\end{split}\n", "$$\n", "\n", "\n", "Reference\n", "[1] Khatib, O. 1995. Inertial properties in robotic manipulation: an object-level framework. Int. J. Robot.\n", "[2] Featherstone, R., and Khatib, O. 1997. Load Independence of the Dynamically Consistent Inverse of the Jacobian Matrix. Int. J. Robot.\n", "\n", "Weita\n", "2021/11/20" ] } ], "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 }