Recursive Newton-Euler algorithm (2023)

Inverse dynamics refers to the computation of forces from motions. Given theconfiguration \(\bfq\), generalized velocity \(\qd\) and generalizedacceleration \(\qdd\), it amounts to finding the joint torques\(\bftau\) and contact forces \(\bff^\mathit{ext}\) such that theconstrained 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, forinstance for an arm with six degrees of freedom, but for mobile robots undermultiple contacts it is often under-determined. In that case, we can postponethe calculation of external forces to e.g. a contact model and compute jointtorques. The recursive Newton-Euler algorithm (RNEA) gives us an efficient wayto 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 asecond-order forward kinematics, followed by a backward pass that computesforces and joint torques.

Forward kinematic pass

The first pass of RNEA computes body velocities \(\bfv_i\) andaccelerations \(\bfa_i\). Starting from the root \(i = 0\) of thekinematic tree, the motion \(\bfv_{i}, \bfa_i\) of body \(i\) iscomputed from the motion \(\bfv_{\lambda(i)}, \bfa_{\lambda(i)}\) of itsparent body \(\lambda(i)\), plus the component caused by the motion\(\qd_i, \qdd_i\) of the joint between them. Let's start with the bodyvelocity:

\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 subspacematrix of the joint \(i\). Note that \(\qd_i \in \mathbb{R}^k\) is thevelocity 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 notthe \(i^\mathrm{th}\) component of the generalized velocity vector\(\qd\) (which would not make sense since \(i\) is the index of a jointwhereas 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 timederivative of the motion subspace matrix is zero. Nevermind this statementunless you are dealing with a different joint ;-) Then, the body accelerationcomputed 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 body 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 force vectors during the backward pass. Note that, since they are force vectors, our notation implies that \(\bff_i^\mathit{ext}\) is a body force vector as well. If the external force is expressed in the inertial frame as \({}^0 \bff_i^\mathit{ext}\), it can be mapped to the body frame via \(\bff_i = {}^i\bfX_0 {}^0 \bff_i^\mathit{ext}\).

Backward dynamic pass

The second pass of RNEA computes body forces. Starting from leaf nodes of thekinematic tree, the generalized force \(\bff_{i}\) of body \(i\) isadded 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 getthe corresponding joint torque \(\bftau_i\) by projecting this 6D bodyvector 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 servoshould provide to track \((\bfq, \qd, \qdd, \bff^\mathit{ext})\). All othercomponents correspond to the five degrees of constraint of our revolute jointand 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:

(Video) lec9-1: Multibody Dynamics (Inverse Dynamics, Recursive Newton-Euler Algorithm Derivation)

  • \(\bfv_0 = \bfzero\)
  • \(\bfa_0 = -\bfa_g\), where \(\bfa_g\) is the gravitational acceleration vector
  • 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\)

The initialization of the acceleration \(\bfa_0 = -\bfa_g\) is a trick toavoid adding \(\bfI_i \bfa_g\) terms to every acceleration on the forwardpass. It will yield the correct \((\bftau, \bff^\mathit{ext})\) results,however in this version of the algorithm the intermediate \(\bfa_i\) and\(\bff_i\) won't equal their physical values (rather, they becomecomputation intermediates). See page 94 of Rigid body dynamics algorithms for details on this point.

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

def rnea(q, qd, qdd, f_ext): pass

Note that q is a list of generalized coordinates for each joint, not a flatarray, and that the same holds for the other parameters. In particular, f_ext is a list of body force vectors \(\bff_i^\mathit{ext}\). With Python typeannotations, our prototype would therefore look like this:

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

This extra structure allows more general joints, such as spherical ones (notcommon) 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 linkof our kinematic tree, and \(\bfa_0\) its spatial acceleration. Weinitialize them to respectively zero and the standard acceleration due togravity:

n = len(qd) - 1 # number of links == number of joints - 1v = [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] = -np.array([0.0, 0.0, -9.81])# to be continued below...

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

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:

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] = -np.array([0.0, 0.0, -9.81]) 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 inrigid body dynamics libraries or simulators. A mapping from such lists to aflat array structure is called an articulation, and decides e.g. how torepresent 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 inversekinematics, inverse dynamics is "easy"relative to forward dynamics. Therecursive Newton-Euler algorithm we saw here was proposed by Walker and Orin(1982). This video supplement fromModern Robotics gives a goodoverview of it.

Code-wise, this algorithm is implemented in rigid body dynamics libraries, suchas OpenRAVE and Pinocchio, as well as dynamic simulators,such as Raisim. It is summed up in concise form inTable 5.1 of Featherstone's Rigid body dynamics algorithms.

Thanks to Alex C. for correcting an error in a previous version of this post!

Discussion

Thanks to all those who have contributed to the conversation so far. Feel free to leave a reply using the form below, or subscribe to the Discussion's atom feed to stay tuned.

  • Recursive Newton-Euler algorithm (1)

    Permalink

    Subs

    Posted on

    Can you publish the code on MATLAB?

  • Recursive Newton-Euler algorithm (5)

    Permalink

    Alex C.

    Posted on

    I see you are using f_ext[] as if each component is defined in the joint's local frame. Usually, external "spatial" forces are offered in global space and you have to apply a change of reference inside the RNEA algorithm.

    Also, I think you missed a sign: a[0] = gravity should be a[0] = -gravity because it gets propagated, and when it is applied, it's basically, on the right side of the expression, hence, it should have a negative sign.

    I hope I didn't mess up something... Really nice list of articles :)

    (Video) Robotics (19 of Addis Ababa Lectures): Recursive Newton-Euler Algorithm
    • Recursive Newton-Euler algorithm (6)

      Permalink

      Stéphane

      Posted on

      Thank you for pointing this out! The acceleration was indeed incorrect, I've corrected it and added a reference to page 94 of Rigid body dynamics algorithms for reference.

      External forces \(\bff_i^\mathit{ext}\) are indeed body vectors, as are the \(\bff_i\)'s used in intermediate computations. This is now pointed out, both under the corresponding equation and when typing the Python prototype.

  • Recursive Newton-Euler algorithm (7)

    Permalink

    Alex C.

    Posted on

    I was wondering, for an open chain, is there a way to identify/compute the acceleration corresponding to some possible applied control torque? It could be quite handy for system identification, particularly for the first link where is not enough variation. (When you are doing identification for spatial inertia/CoM/mass, given that the spatial force is linear in a matrix that corresponds to a linear combination of spatial inertia/CoM/mass for each link: https://arxiv.org/pdf/1701.04395.pdf.)

    If I can find the corresponding acceleration of some applied control torque then I can factor it into a regressor (I am doing the same thing with gravity but the difference is that the gravity is a fixed acceleration vector, moreover for the first link, the \({}_i X_0\), is identity, e.g. no chance for linear combinations, whereas, the corresponding external acceleration of some torque can have different values depending on the applied torque).

    Any ideas? :)

    • Recursive Newton-Euler algorithm (8)

      Permalink

      Stéphane

      Posted on

      Computing joint accelerations from joint torques corresponds to forward dynamics. You can check out the articulated body algorithm (ABA) or composite rigid body algorithm (CRBA). The latter relies on RNEA internally.

      I'm not sure how inverse or forward dynamics can help in system identification, as both functions assume the inertias of the robot are known. One way to go around that is to turn to the equations of motion: assuming a dataset of recorded trajectories \(\{(\bfq_i, \qd_i, \qdd_i, \bftau_i, \bff^\mathit{ext}_i)\}_i\), set up an optimization problem with inertial parameters as optimization variables and the equations of motion as constraints to connect them to the data:

      \begin{equation*}\bfY_i \bfphi = \bfM(\bfq_i) \qdd_i + \qd_i^T \bfC(\bfq_i) \qd_i - \bftau_g(\bfq_i) =\bfS^\top \bftau_i + \bfJ_c(\bfq_i)^T \bff^{ext}_i\end{equation*}

      Where \(\bfY_i\) is a block line of the regressor matrix \(\bfY\), which only depends on \(\{(\bfq_i, \qd_i, \qdd_i)\}_i\), and \(\bfphi\) is the vector of inertial parameters to be optimized. This approach is detailed for instance below equations (1) and (2) of Humanoid and Human Inertia Parameter Identification Using Hierarchical Optimization, and corresponds to the one described in section III of the paper your mentioned.

      Applying forward dynamics to generate a dataset \(\{(\bfq_i, \qd_i, \qdd_i, \bftau_i, \bff^\mathit{ext}_i)\}_i\) from \(\{(\bfq_i, \qd_i, \bftau_i, \bff^\mathit{ext}_i)\}_i\) can still be useful as a way to validate a system identification algorithm.

      (Video) 4 2 Newton Euler Equations University of Pennsylvania Coursera
  • Recursive Newton-Euler algorithm (9)

    Permalink

    Jonas

    Posted on

    Thank you for such a great post! Do you happen to have a reference python implementation of RNEA that can handle closed-loop kinematic chains?

    • Recursive Newton-Euler algorithm (10)

      Permalink

      Stéphane

      Posted on

      Unfortunately I don't know of any Python implementation for RNEA with closed loops.

      (Video) Recursive Robot Dynamics (1a/4 of IIT Delhi Lectures): For Serial Robots

      For an implementation that can be used in Python (via bindings), closed loop chains are handled in the pinocchio3-preview branch of Pinocchio, as detailed here and here. The feature hasn't hit release yet (meaning the doc is in examples), but when it does it will be installable by pip install pin.

Post a comment

FAQs

What is Newton Euler method? ›

In classical mechanics, the Newton–Euler equations describe the combined translational and rotational dynamics of a rigid body. Traditionally the Newton–Euler equations is the grouping together of Euler's two laws of motion for a rigid body into a single equation with 6 components, using column vectors and matrices.

What is the advantage of having recursive equation in robotics? ›

The main advantages of this technique are the facility of implementation by numerical or symbolical programming and providing models with reduced number of operations.

What is inverse dynamics biomechanics? ›

A commonly used biomechanics analysis tool is inverse dynamics, a method which uses kinematic and kinetic data from a motion-capture system to calculate net torques at anatomical joints. These net torques can be used to infer (though not directly calculate) the muscle forces acting during the motion being studied.

What is forward dynamics in robotics? ›

Description. The Forward Dynamics block computes joint accelerations for a robot model given a robot state that is made up of joint torques, joint states, and external forces. To get the joint accelerations, specify the robot configuration (joint positions), joint velocities, applied torques, and external forces.

Why Euler formula is used? ›

Euler's formula relates the complex exponential to the cosine and sine functions. This formula is the most important tool in AC analysis. It is why electrical engineers need to understand complex numbers. Created by Willy McAllister.

What is Euler theorem formula? ›

It is written F + V = E + 2, where F is the number of faces, V the number of vertices, and E the number of edges. A cube, for example, has 6 faces, 8 vertices, and 12 edges and satisfies this formula.

What is the purpose of recursive algorithm? ›

Here is the basic idea behind recursive algorithms: To solve a problem, solve a subproblem that is a smaller instance of the same problem, and then use the solution to that smaller instance to solve the original problem.

What are the disadvantages of recursive function? ›

Disadvantages of recursion
  • Recursive functions are generally slower than non-recursive function.
  • It may require a lot of memory space to hold intermediate results on the system stacks.
  • Hard to analyze or understand the code.
  • It is not more efficient in terms of space and time complexity.

Are recursive algorithms more efficient? ›

A recursive method is a method that calls itself. An iterative method is a method that uses a loop to repeat an action. Anything that can be done iteratively can be done recursively, and vice versa. Iterative algorithms and methods are generally more efficient than recursive algorithms.

What is Newton method used for? ›

The Newton-Raphson method (also known as Newton's method) is a way to quickly find a good approximation for the root of a real-valued function f ( x ) = 0 f(x) = 0 f(x)=0. It uses the idea that a continuous and differentiable function can be approximated by a straight line tangent to it.

What is Euler's method used for in real life? ›

Euler's method is commonly used in projectile motion including drag, especially to compute the drag force (and thus the drag coefficient) as a function of velocity from experimental data.

What is Euler theorem in physics? ›

In geometry, Euler's rotation theorem states that, in three-dimensional space, any displacement of a rigid body such that a point on the rigid body remains fixed, is equivalent to a single rotation about some axis that runs through the fixed point.

Videos

1. S16 - Robot Dynamics: Newton Euler Formulation I
(ASHISH SINGLA)
2. 1 - 6 - Newton-Euler Mechanics
(Dmitry Savransky)
3. lec30 Recursive Formulations of Dynamics of Manipulators
(NPTEL - Indian Institute of Science, Bengaluru)
4. Modern Robotics, Chapter 8.5: Forward Dynamics of Open Chains
(Northwestern Robotics)
5. Dynamics of an Industrial Serial Robot using Newton-Euler (NE) Approach
(Arun Dayal Udai)
6. 3 Link Robot Arm - Newton-Euler Algorithm [Demo 02]
(Ermano Arruda)
Top Articles
Latest Posts
Article information

Author: Mr. See Jast

Last Updated: 03/04/2023

Views: 6045

Rating: 4.4 / 5 (55 voted)

Reviews: 86% of readers found this page helpful

Author information

Name: Mr. See Jast

Birthday: 1999-07-30

Address: 8409 Megan Mountain, New Mathew, MT 44997-8193

Phone: +5023589614038

Job: Chief Executive

Hobby: Leather crafting, Flag Football, Candle making, Flying, Poi, Gunsmithing, Swimming

Introduction: My name is Mr. See Jast, I am a open, jolly, gorgeous, courageous, inexpensive, friendly, homely person who loves writing and wants to share my knowledge and understanding with you.