# Recursive Newton-Euler algorithm

Inverse dynamics refers to the computation of forces from motions. Given the configuration \(\bfq\), generalized velocity \(\qd\) and generalized acceleration \(\qdd\), it amounts to finding the joint torques \(\bftau\) and contact forces \(\bff^\mathit{ext}\) such that the constrained equations of motion are satisfied:

\begin{align} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff^\mathit{ext} \\ \bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = \boldsymbol{0} \end{align}Mathematically, we could formulate this operation as a function:

This function is well-defined when our linear system is fully determined, for
instance for an arm with six degrees of freedom, but for mobile robots under
multiple contacts it is often under-determined. In that case, we can postpone
the calculation of external forces to *e.g.* a contact model and compute joint
torques. The recursive Newton-Euler algorithm (RNEA) gives us an efficient way
to do this:

The algorithm works in two passes: a forward pass which is mostly a second-order forward kinematics, followed by a backward pass that computes forces and joint torques.

## Forward kinematic pass

The first pass of RNEA computes body velocities \(\bfv_i\) and accelerations \(\bfa_i\). Starting from the root \(i = 0\) of the kinematic tree, the motion \(\bfv_{i}, \bfa_i\) of body \(i\) is computed from the motion \(\bfv_{\lambda(i)}, \bfa_{\lambda(i)}\) of its parent body \(\lambda(i)\), plus the component caused by the motion \(\qd_i, \qdd_i\) of the joint between them. Let's start with the body velocity:

In this equation, \({}^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\). Note that \(\qd_i \in \mathbb{R}^k\) is the
velocity of the joint, for instance \(k = 6\) for the floating base
(*a.k.a.* free flyer) joint, \(k = 2\) for a spherical joint, and \(k
= 1\) for a revolute or a prismatic joint. At any rate, \(\qd_i\) is *not*
the \(i^\mathrm{th}\) component of the generalized velocity vector
\(\qd\) (which would not make sense since \(i\) is the index of a joint
whereas the vector \(\qd\) is indexed by degrees of freedom). Consequently,
the motion subspace matrix has dimension \(6 \times k\).

In what follows, let us assume a "common" joint (revolute, prismatic, helical, cylindrical, planar, spherical, free flyer) so that the apparent time derivative of the motion subspace matrix is zero. Nevermind this statement unless you are dealing with a different joint ;-) Then, the body acceleration computed from the parent during the forward pass is:

So far this forward pass is a second-order forward kinematics. The last thing we want to compute along the way are the inertial forces produced by the motion \(\bfv_i, \bfa_i\) of body \(i\):

We will update these body force vectors during the backward pass.

## Backward dynamic pass

The second pass of RNEA computes body forces. Starting from leaf nodes of the kinematic tree, the generalized force \(\bff_{i}\) of body \(i\) is added to the force \(\bff_{\lambda(i)}\) computed so far for its parent \(\lambda(i)\):

Once the generalized force \(\bff_i\) on body \(i\) is computed, we get the corresponding joint torque \(\bftau_i\) by projecting this 6D body vector along the joint axis:

For a revolute joint, \(\bfS_i\) is a \(6 \times 1\) column vector, so that we end with a single number \(\tau_i = \bfS_i^\top \bff_i\): the actuation torque that the joint servo should provide to track \((\bfq, \qd, \qdd, \bff^\mathit{ext})\). All other components correspond to the five degrees of constraint of our revolute joint and will be provided passively by the joint's mechanics.

## Pseudocode

Summing up the math, our function \(\mathrm{RNEA}(\bfq, \qd, \qdd, \bff^\mathit{ext})\) does the following:

- \(\bfv_0 = 0\), \(\bfa_0 = \mathrm{gravity}\)
**for**link \(i = 1\) to \(n\):- \({}^i \bfX_{\lambda(i)}, \bfS_i, \bfI_i = \mathrm{compute\_joint}(\mathrm{joint\_type}_i, \bfq_i, \qd_i)\)
- \(\bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i\)
- \(\bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i\)
- \(\bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}\)

**for**link \(i = n\) to \(1\):- \(\bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i\)
- \(\bftau_i = \bfS_i^\top \bff_i\)

Let us explicit a few more things by doing the same in pseudo-Python. Our function prototype is:

def rnea(q, qd, qdd, f_ext): # ...

Note that `q` is a list of generalized coordinates for each joint, not a flat
array, and that the same holds for the other parameters. With Python type
annotations, our prototype would therefore look like this:

from typing import List import numpy as np def rnea( q: List[np.ndarray], qd: List[np.ndarray], qdd: List[np.ndarray], f_ext: List[np.ndarray], ) -> List[np.ndarray]: # ...

This extra structure allows more general joints, such as spherical ones (not common) or a free-flyer joint for the floating base of a mobile robot (common). If all joints were revolute, then all types would coalesce to flat arrays.

Let us denote by \(\bfv_0 = \bfzero\) the spatial velocity of the root link of our kinematic tree, and \(\bfa_0\) its spatial acceleration. We initialize them to respectively zero and the standard acceleration due to gravity:

def rnea(q, qd, qdd, f_ext): n = len(qd) - 1 # number of links == number of joints - 1 v = [np.empty((6,)) for i in range(n + 1)] a = [np.empty((6,)) for i in range(n + 1)] f = [np.empty((6,)) for i in range(n + 1)] tau = [np.empty(qd[i].shape) for i in range(n + 1)] v[0] = np.zeros((6,)) a[0] = gravity

We continue with the forward pass, which ranges from link \(i = 1\) to the last link \(i = n\) of the tree:

# def rnea(q, qd, qdd, f_ext): for i in range(1, n + 1): p = lambda_[i] # p for "parent" X_p_to_i[i], S[i], I[i] = compute_joint(joint_type[i], q[i], qd[i]) v[i] = X_p_to_i[i] * v[p] + S[i] * qd[i] a[i] = X_p_to_i[i] * a[p] + S[i] * qdd[i] + v[i].cross(S[i] * qd[i]) f[i] = I[i] * a[i] + v[i].cross(I[i] * v[i]) - f_ext[i]

The backward pass traverses the same range in reverse order:

# def rnea(q, qd, qdd, f_ext): for i in range(n, 0, -1): p = lambda_[i] tau[i] = S[i].T * f[i] f[p] += X_p_to_i[i].T * f[i]

Wrapping it up, we get:

def rnea(q, qd, qdd, f_ext): n = len(qd) v = [np.empty((6,)) for i in range(n + 1)] a = [np.empty((6,)) for i in range(n + 1)] f = [np.empty((6,)) for i in range(n + 1)] tau = [np.empty(qd[i].shape) for i in range(n + 1)] v[0] = np.zeros((6,)) a[0] = gravity for i in range(1, n + 1): p = lambda_[i] X_p_to_i[i], S[i], I[i] = compute_joint(joint_type[i], q[i], qd[i]) v[i] = X_p_to_i[i] * v[p] + S[i] * qd[i] a[i] = X_p_to_i[i] * a[p] + S[i] * qdd[i] + v[i].cross(S[i] * qd[i]) f[i] = I[i] * a[i] + v[i].cross(I[i] * v[i]) - f_ext[i] for i in range(n, 0, -1): p = lambda_[i] tau[i] = S[i].T * f[i] f[p] += X_p_to_i[i].T * f[i] return tau

Lists of arrays with different lengths are typically an internal structure in
rigid body dynamics libraries or simulators. A mapping from such lists to a
flat array structure is called an *articulation*, and decides *e.g.* how to
represent orientations for spherical and free-flyer joints (unit quaternions?
full rotation matrices? or, heaven forfend, some flavor of Euler angles).

## To go further

Dually to forward kinematics being the "easy" problem relative to inverse kinematics, inverse dynamics is "easy" relative to forward dynamics. The recursive Newton-Euler algorithm was proposed by Walker and Orin (1982). This video supplement from Modern Robotics gives a good overview of it. The algorithm is implemented in rigid body dynamics libraries, such as OpenRAVE and Pinocchio, as well as dynamic simulators, such as Raisim. It is summed up in concise form in Table 5.1 of Featherstone's Rigid body dynamics algorithms.