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:

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

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:

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

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:

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

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:

\begin{equation*} \bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i \end{equation*}

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

\begin{equation*} \bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext} \end{equation*}

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

\begin{equation*} \bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i \end{equation*}

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:

\begin{equation*} \bftau_i = \bfS_i^\top \bff_i \end{equation*}

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.

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