/posts/algorithm-recursive-newton-euler

overview

The recursive newton euler algorithm solves the inverse dynamics problem for a kinematic tree of $n$ bodies in $O(n)$ time. This document is intended to act as a concise reference to the already informed reader. For an introduction to the topic or a deeper discussion, refer to [1].

history

The recursive newton euler algorithm was first described in [2]. Classical accerations were replaced with spatial accerations in [3], with the addition of spatial accelerations. The version presented in this document is the subsequent reformulation from [1], which introduces the use of generalized coorderinates and spatial vector algebra.

formulation

A rigid body system in any configuration is described as $(\textit{model},\textit{state})$ .

For a kinematic tree, the $\textit{model}$ is a set of parent body indices $\lambda$ , joint types $\text{jtype}$ , parent transforms $X_T$ , and inertias $I$ .

The $\textit{state}$ is a set of generalized positions $\bm{q}$ , velocities $\bm{\dot{q}}$ , and accelerations $\bm{\ddot{q}}$ .

Inverse dynamics is the problem of finding the forces of a rigid body system given its state.

\bm{\tau} = \text{ID}(\textit{model},\textit{state})

The recursive newton euler algorithm solves the inverse dynamic problem by:

Compute the velocity and acceleration of each body in the tree.
Compute the forces required to produce these accelerations.
Compute the forces transmitted across the joints from the forces acting on the bodies.

\normalsize \textbf{Recursive Newton Euler Algorithm} \\ \normalsize \hspace{1.5em} \\ \begin{aligned} &v_0 = 0 \\ &a_0 = -a_g \\ &\text{for } i = 1 \text{ to } N_B \text{ do} \\ &\hspace{1.5em} [X_J,S_i,v_j,c_j] = \text{jcalc}(\text{jtype}(i),q_i,\dot{q_i}) \\ &\hspace{1.5em} {}^i\!X_{\lambda(i)} = X_J X_T(i) \\ &\hspace{1.5em} \text{if }\lambda(i) \ne 0 \text{ then} \\ &\hspace{1.5em}\hspace{1.5em} {}^i\!X_0 = {}^i\!X_{\lambda(i)} {}^{\lambda(i)}\!X_0 \\ &\hspace{1.5em} \text{end} \\ &\hspace{1.5em} v_i = {}^i\!X_{\lambda(i)} v_{\lambda(i)} + v_J \\ &\hspace{1.5em} a_i = {}^i\!X_{\lambda(i)} a_{\lambda(i)} + S_i\ddot{q_i} + c_J + v_i \times v_J \\ &\hspace{1.5em} f_i = I_ia_i + v_i \times^{*} I_iv_i - {}^i\!X_0^*f_i^x \\ &\text{end} \\ &\text{for } i = N_B \text{ to } 1 \text{ do} \\ &\hspace{1.5em} \tau_i = S_i^Tf_i \\ &\hspace{1.5em} \text{if }\lambda(i) \ne 0 \text{ then} \\ &\hspace{1.5em}\hspace{1.5em} f_{\lambda(i)} = f_{\lambda(i)} + {}^{\lambda(i)}\!X_i^*f_i \\ &\hspace{1.5em} \text{end} \\ &\text{end} \\ \end{aligned}

\begin{array}{l} \textbf{Joint Model Formulae (jcalc)} \\ \end{array} \\ \begin{array}{lllll} \\ \textbf{Joint Type} & \textbf{Joint Transform} ({}^s\!X_p) & & \textbf{Motion Subspace} & \textbf{Constraint Force Subspace} \\ \textbf{} & \bm{E} & \bm{r} & \bm{S} & \bm{T} \\ \\ \hline \\ \text{revolute} & rz(q_1) & \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} & \begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \\ 0 \\ 0 \end{bmatrix} & \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \\ \\ \text{prismatic} & \bm{1}_{3 \times 3} & \begin{bmatrix} 0 \\ 0 \\ q_1 \end{bmatrix} & \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 1 \end{bmatrix} & \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 \end{bmatrix} \end{array}

references

[1] R. Featherstone. Rigid Body Dynamics Algorithms. Springer, 2008.

[2] J. Luh, et al. Resolved-Acceleration Control of Mechanical Manipulators. IEEE Trans. On Auto. Control, Vol. AC-25, No. 3, pp. 468– 474, June 1980.

[3] M. Brady, et al. Robot Motion: Planning and Control. The MIT Press, 1982.