Inverse dynamics

Inverse dynamics (ID) refers to the computation of forces from motions. Given the configuration \(\bfq\), generalized velocity \(\qd\) and generalized acceleration \(\qdd\), inverse dynamics computes the joint torques \(\bftau\) and contact forces \(\bff\) such that the constrained equations of motion are satisfied:

\begin{equation*} \begin{array}{rcl} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = & \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \\ \bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = & \boldsymbol{0} \end{array} \end{equation*}

Mathematically, we can write inverse dynamics as a function \(\mathrm{ID}\) such that:

\begin{equation*} (\bftau, \bff) = \mathrm{ID}(\bfq, \qd, \qdd) \end{equation*}

Note that this function implicitly depends on our robot model. Different robots will have e.g. different inertias, thus different inertia matrices \(\bfM(\bfq)\), thus different \(\mathrm{ID}\) functions in a given configuration \(\bfq\).

Recursive Newton-Euler algorithm

The recursive Newton-Euler algorithm (RENA) computes the joint side of inverse dynamics, i.e. it computes joint torques assuming contact forces are known:

\begin{equation*} \bftau = \mathrm{RNEA}(\bfq, \qd, \qdd, \bff) \end{equation*}

This algorithm works in two passes.

Forward pass

This first pass computes body velocities \(\bfv_i\) and accelerations \(\bfa_i\). Starting from the root of the kinematic tree, the motion (velocity or acceleration, let's do velocities) \(\bfv_{i}\) of body \(i\) is computed from the motion \(\bfv_{\lambda(i)}\) of its parent body \(\lambda(i)\) plus the component caused by the velocity \(\dot{q}_i = \bfS_i \qd\) of the joint between them:

\begin{equation*} \bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd \end{equation*}

We won't go into the details of this equation here, but to look it up in the references below, \({}^i \bfX_{\lambda(i)}\) is the Plücker transform from \(\lambda(i)\) to \(i\), and \(\bfS_i\) is the motion subspace matrix of the joint \(i\). This forward pass is a second-order forward kinematics that also computes inertial forces (of the form \(\bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i\)) along the way.

Backward pass

This second pass compute body forces. Starting from leaf nodes of the kinematic tree, the generalized force \(\bff_i\) of body \(i\) is computed from the force \(\bff_{i+1}\) of its successor body \(i+1\) plus the inertial forces \(\bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i\) computed from motions in the forward pass. The latter correspond to the forces to apply to track the desired joint motions \(\qd, \qdd\) from the current configuration \(\bfq\).

Once the generalized force \(\bff_i\) on body \(i\) is computed, we get the corresponding joint torque \(\tau_i\) by projecting this 6D vector along the joint axis. This component corresponds to the actuation torque that the joint servo will have to provide. All other components correspond to the five degrees of constraint of our revolute joint and will be provided passively by the joint's mechanics.

To go further

RNEA was proposed by Walker and Orin (1982). There is an excellent introduction to it in this video supplement to Modern Robotics. RNEA itself has been implemented in rigid body dynamics libraries, such as OpenRAVE or Pinocchio, as well as dynamic simulators, such as Raisim. It is described in concise form in Table 5.1 of Featherstone's Rigid body dynamics algorithms.

Dually to forward kinematics being the "easy" problem relative to inverse kinematics, inverse dynamics is "easy" relative to forward dynamics.

© Stéphane Caron — All content on this website is licensed under the CC BY 4.0 license.
π