The Recursive Newton-Euler Algorithm algorithm using spatial vectors

We recall here briefly this algorithm for a better understanding.

$inputs: {\bf q},\dot{\bf q}, \ddot{\bf q}, \textit{model}, {^0{\bf f}{^e}{_i}}$
$output: \tau, {\bf f} $
$\textit{model data}: N_B, jtype(i), p(i), {\bf X}_L(i), I_i $

  1. ${\bf v}_0 = {\bf 0}$
  2. ${\bf a}_o = -{\bf a}_g$
  3. $for \; i=1\; to \; N_b \; then$
    1. ${\bf X}_J(i) = xjcalc(jtype(i),{\bf q}_i)$
    2. ${^i{\bf X}}_{p(i)} = {\bf X}_J(i){\bf X}_L(i)$
    3. $\text{if } p(i)\neq 0 \text{ then }$
      1. ${^i{\bf X}_0} = {^i{\bf X}_{p(i)}} {^{p(i)}{\bf X}_0} $
    4. $ \text{end}$
    5. $ {\bf \Phi}_i = pcalc(jtype(i),{\bf q}_i)$
    6. $ {\mathring{\bf \Phi}}c_i = pdcalc(jtype(i),{\bf q}_i)$
    7. $ {\bf v}_i = {^i{\bf X}_{p(i)}}{\bf v}_{p(i)} + \Phi_i\dot{\bf q}_i$
    8. $ {\bf \zeta}_i = {\mathring{\bf \Phi}}c_i \dot{\bf q}_i + {\bf v}_i \times \Phi_i\dot{\bf q}_i $
    9. $ {\bf a}_i = {^i{\bf X}_{p(i)}} {\bf a}_{p(i)} + \Phi_i \ddot{\bf q}_i + {\bf \zeta}_i $
    10. $ {\bf f}_i = {\bf I}_i {\bf a}_i + {\bf v}_i \times {\bf I}_i {\bf v}_i $

  4. $ for \; i=N_b\; to \; 1 \; do $

    1. $ {\bf \tau}_i = \Phi_i^T {\bf f}_i $
    2. $ \text{if } p(i)\neq 0 \text{ then } $
      1. ${\bf f}_p(i) = {\bf f}_{p(i)} + {^i}{\bf X}_{p(i)}^T $
    3. $ \text{end} $