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) S16 - Robot Dynamics: Newton Euler Formulation I

      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? ›

The Newton–Euler method of dynamic formulation is based on the direct application of Newton's law and Euler's equation in the form(1) ∑ F =m a and(2) ∑ M + ∑ r × F=r cg ×m a + Iα + ω × Iω to individual bodies. This yields six scalar equations for each body.

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.

What is Euler's formula in geometry? ›

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 Euler's formula in complex numbers? ›

Euler's law states that 'For any real number x, e^ix = cos x + i sin x. This complex exponential function is sometimes denoted cis x ("cosine plus i sine"). The formula is still valid if x is a complex number.

What is inverse dynamics Robotics? ›

Manipulator inverse dynamics, or simply inverse dynamics, is the calculation of the forces and/or torques required at a robot's joints in order to produce a given motion trajectory consisting of a set of joint positions, velocities and accelerations.

What is the difference between Euler method and modified Euler method? ›

Generally the modified Euler method is more accurate than Euler method. In this work which concern with the accuracy of numerical solutions for first order differential equations. Euler and modified Euler methods have been applied in order to investigate the objective of the study.

How do you use Newton's method? ›

The trick of Newton's method is to draw a tangent line to the graph y=f(x) at the point (x1,y1). See below. This tangent line is a good linear approximation to f(x) near x1, so our next guess x2 is the point where the tangent line intersects the x-axis, as shown above. We then proceed using the same method.

Why is Runge Kutta better than Euler? ›

Explicit Runge–Kutta methods perform several evaluation of function around the point and then they compute using a weighted average of those values. Compared with Euler's, this method performs an extra evaluation of in order to compute .

How accurate is Euler's method? ›

Euler's Method will only be accurate over small increments and as long as our function does not change too rapidly. Consequently, we need to ensure that our step-size isn't too large or our numerical solution will be inaccurate.

Videos

1. Recursive Robot Dynamics (1a/4 of IIT Delhi Lectures): For Serial Robots
(Subir Kumar Saha)
2. 1 - 6 - Newton-Euler Mechanics
(Dmitry Savransky)
3. Dynamics of an Industrial Serial Robot using Newton-Euler (NE) Approach
(Arun Dayal Udai)
4. LECT.18 | Langrange Euler & Newton Euler Differences | Robotics Complete Course
(Blazengg Academy)
5. lec30 Recursive Formulations of Dynamics of Manipulators
(NPTEL - Indian Institute of Science, Bengaluru)
6. 3 Link Robot Arm - Newton-Euler Algorithm [Demo 02]
(Ermano Arruda)
Top Articles
Latest Posts
Article information

Author: Ouida Strosin DO

Last Updated: 02/18/2023

Views: 6047

Rating: 4.6 / 5 (56 voted)

Reviews: 87% of readers found this page helpful

Author information

Name: Ouida Strosin DO

Birthday: 1995-04-27

Address: Suite 927 930 Kilback Radial, Candidaville, TN 87795

Phone: +8561498978366

Job: Legacy Manufacturing Specialist

Hobby: Singing, Mountain biking, Water sports, Water sports, Taxidermy, Polo, Pet

Introduction: My name is Ouida Strosin DO, I am a precious, combative, spotless, modern, spotless, beautiful, precious person who loves writing and wants to share my knowledge and understanding with you.