Dynamically Consistent Pseudo Inverse

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.

1. Mapping

1. 1 Kinematics

In kinematics, we have

\[\begin{equation} J\dot q = \dot x \end{equation}\]

in which \(J\) is a Jacobian projection \(\dot q\) is in configuration space \(\dot x\) is in task space

1.2 Dynamics

In dynamics, we have \(\tau = J^Tf\) in consistent with general formula, we rewrite it as

\[\begin{equation} J^{T\sharp}\tau = f \end{equation}\]

in which \(J^{T\sharp}\) is projection \(\tau\) is joint torque in configuration space \(f\) is operational force in task space

Notice

  • \(M^\sharp\) is pseudo-inverse of \(M\). Here we use \(\sharp\) refer pseudo-inverse while some paper using \(\dagger\).

  • \((A^{-1})^T = (A^T)^{-1}\) or \((A^\sharp)^T = (A^T)^\sharp\) and proof is shown in appendix.

1.3 Generalize

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

\[\begin{equation} Pc = t \end{equation}\]

Here \(P\) is a projection. \(P = J\) in kinematics while \(P = J^{T\sharp}\) in dynamics \(c\) is in configuration space \(t\) is in task space

2. Null space of the Mapping

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

\[\begin{equation} N = I - P^{\sharp }P \end{equation}\]

For redundant mechanism, the control of configuration space

\[\begin{equation} c = P^\sharp t + (I - P^\sharp P)V_0 \end{equation}\]

where \(V_0\) is a arbitrary value

  • For kinematics

\[\begin{equation} \dot q = J^\sharp \dot x + (I - J^\sharp J)q_0 \end{equation}\]

  • For dynamics,

\[\begin{equation} \tau = J^Tf + (I - J^TJ^{T\sharp})\tau_0 \end{equation}\]

3. Where are we

The whole story start from goal of dynamically control which means solve the torque from a desired trajectory.

  • At the very beginning, we have a desired trajectory \(x(t)\).

  • 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.

  • Finally, we may differentiate Jacobin mapping one more time to get the relationship between \(\ddot x\) and \(\ddot q\)

\[\begin{equation} \ddot x - \dot J \dot q= J \ddot q \end{equation}\]

  • We could calculate \(\ddot q\) by

\[\begin{equation} \ddot q = J^\sharp(\ddot x - \dot J \dot q) \end{equation}\]

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. PROBLEM 1: We need to find a pseudo-inverse from all infinity

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\). PROBLEM 2: To finish control, there is a relationship between \(\ddot q\) and \(\tau\) need to be figured.

4. Extra dynamic constrain

To emphasis above-mentioned problems, considering the derivative of \(\dot x = J\dot q\),

\[\begin{equation} \ddot x - \dot J \dot q= J \ddot q \end{equation}\]

and the dynamic model equation

\[\begin{equation} A\ddot q + b + g = \tau \end{equation}\]

bring \(\ddot q\) of second equation to first equation, we get a equation between controllable \(\tau\) and acceleration of task space

\[\begin{split}\begin{equation} \begin{split} \ddot x - \dot J \dot q + JA^{-1}(b + g) &= JA^{-1}\tau\\ &= JA^{-1}[J^Tf + (I - J^TJ^{T\sharp})\tau_0]\\ &= JA^{-1}J^Tf + JA^{-1}(I - J^TJ^{T\sharp})\tau_0 \end{split} \end{equation}\end{split}\]

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

\[\begin{equation} JA^{-1}[I-J^TJ^{\sharp T}]\tau_0 = 0 \end{equation}\]

which result in a unique pseudo-inverse

\[\begin{equation} J^\sharp = \bar J = A^{-1}J^T\Lambda \end{equation}\]

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.

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\).

5. Conclusion

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. In kinematics, pseudo inverse, \(J^{-1}\), by SVD minimizes the least square of error. In contrast, dynamic constrain pseudo inverse, \(\bar J\), by bring mass matrix in account to minimizes instantaneous kinetic energy.

Later Roy Featherstone showed that this \(\bar J\) is independent of load in Roy (1997). This might be a torch of future research.

APPENDIX

APPENDIX.A

\[\begin{split}A^T(A^{-1})^T = (A^{-1}A)^{T} = I^T = I \\ (A^{-1})^TA^T = (AA^{-1})^{T} = I^T = I\end{split}\]

This proves that the inverse of \(A^T\) is \((A^{-1})^T\) ### APPENDIX.B THEOREM: DYNAMIC CONSISTENCY, A generalized inverse that is consistent with the dynamic constraint of equation

\[[JA^{-1} - (JA^{-1}J^T)J^{\sharp T}]\tau_0 = 0\]

is unique and is given by

\[J^\sharp = \bar J = A^{-1}J^T\Lambda\]

where

\[\Lambda = (JA^{-1}J^T)^{-1}\]

PROOF To achieve

\[[JA^{-1} - (JA^{-1}J^T)J^{\sharp T}]\tau_0 = 0\]

by definition

\[\Lambda^{-1} = JA^{-1}J^T\]

the constrain equation premultiplies of \(\tau_0\) has be 0, that is

\[JA^{-1} - \Lambda^{-1}J^{\sharp T} = 0\]

So, we get

\[J^{\sharp T} = \Lambda JA^{-1}\]

Considering inertial matrix \(A\) is a symmetric positive define matrix(aka \(A^T = A\)), finally we have

\[\begin{split}\begin{split} J^\sharp = (J^{\sharp T})^T &=(\Lambda JA^{-1})^T\\ &=A^{-T}J^T\Lambda^T\\ &=A^{-1}J^T((JA^{-1}J^T)^{-1})^T\\ &=A^{-1}J^T((JA^{-1}J^T)^T)^{-1}\\ &=A^{-1}J^T(JA^{-T}J^T)^{-1}\\ &=A^{-1}J^T(JA^{-1}J^T)^{-1}\\ &=A^{-1}J^T\Lambda \end{split}\end{split}\]

Reference [1] Khatib, O. 1995. Inertial properties in robotic manipulation: an object-level framework. Int. J. Robot. [2] Featherstone, R., and Khatib, O. 1997. Load Independence of the Dynamically Consistent Inverse of the Jacobian Matrix. Int. J. Robot.

Weita 2021/11/20