# Recursive Newton-Euler algorithm

Inverse dynamics refers to the computation of forces from motions. Given the configuration $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfq$, generalized velocity $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qd$ and generalized acceleration $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qdd$, it amounts to finding the joint torques $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bftau$ and contact forces $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff^\mathit{ext}$ such that the constrained equations of motion are satisfied:

\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \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:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} (\bftau, \bff^\mathit{ext}) = \mathrm{ID}(\bfq, \qd, \qdd)$

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:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bftau = \mathrm{RNEA}(\bfq, \qd, \qdd, \bff^\mathit{ext})$

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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_i$ and accelerations $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_i$. Starting from the root $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i = 0$ of the kinematic tree, the motion $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_{i}, \bfa_i$ of body $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$ is computed from the motion $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_{\lambda(i)}, \bfa_{\lambda(i)}$ of its parent body $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \lambda(i)$, plus the component caused by the motion $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qd_i, \qdd_i$ of the joint between them. Let's start with the body velocity:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i$

In this equation, $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} {}^i \bfX_{\lambda(i)}$ is the Plücker transform from $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \lambda(i)$ to $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$, and $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfS_i$ is the motion subspace matrix of the joint $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$. Note that $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qd_i \in \mathbb{R}^k$ is the velocity of the joint, for instance $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} k = 6$ for the floating base (a.k.a. free flyer) joint, $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} k = 2$ for a spherical joint, and $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} k = 1$ for a revolute or a prismatic joint. At any rate, $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qd_i$ is not the $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i^\mathrm{th}$ component of the generalized velocity vector $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qd$ (which would not make sense since $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$ is the index of a joint whereas the vector $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \qd$ is indexed by degrees of freedom). Consequently, the motion subspace matrix has dimension $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} 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:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i$

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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_i, \bfa_i$ of body $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}$

We will update these force vectors during the backward pass. Note that, since they are force vectors, our notation implies that $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i^\mathit{ext}$ is a body force vector as well. If the external force is expressed in the inertial frame as $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} {}^0 \bff_i^\mathit{ext}$, it can be mapped to the body frame via $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \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 the kinematic tree, the generalized force $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_{i}$ of body $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$ is added to the force $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_{\lambda(i)}$ computed so far for its parent $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \lambda(i)$:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i$

Once the generalized force $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i$ on body $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i$ is computed, we get the corresponding joint torque $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bftau_i$ by projecting this 6D body vector along the joint axis:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bftau_i = \bfS_i^\top \bff_i$

For a revolute joint, $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfS_i$ is a $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} 6 \times 1$ column vector, so that we end with a single number $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \tau_i = \bfS_i^\top \bff_i$: the actuation torque that the joint servo should provide to track $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} (\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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \mathrm{RNEA}(\bfq, \qd, \qdd, \bff^\mathit{ext})$ does the following:

• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_0 = \bfzero$
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_0 = -\bfa_g$, where $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_g$ is the gravitational acceleration vector
• for link $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i = 1$ to $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} n$:
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} {}^i \bfX_{\lambda(i)}, \bfS_i, \bfI_i = \mathrm{compute\_joint}(\mathrm{joint\_type}_i, \bfq_i, \qd_i)$
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i$
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i$
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}$
• for link $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i = n$ to $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} 1$:
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i$
• $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bftau_i = \bfS_i^\top \bff_i$

The initialization of the acceleration $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_0 = -\bfa_g$ is a trick to avoid adding $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfI_i \bfa_g$ terms to every acceleration on the forward pass. It will yield the correct $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} (\bftau, \bff^\mathit{ext})$ results, however in this version of the algorithm the intermediate $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_i$ and $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i$ won't equal their physical values (rather, they become computation 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. Our function prototype is:

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


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. In particular, f_ext is a list of body force vectors $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i^\mathit{ext}$. 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]:
pass


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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfv_0 = \bfzero$ the spatial velocity of the root link of our kinematic tree, and $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfa_0$ its spatial acceleration. We initialize them to respectively zero and the standard acceleration due to gravity:

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] = -np.array([0.0, 0.0, -9.81])
# to be continued below...


We continue with the forward pass, which ranges from link $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} i = 1$ to the last link $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} 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 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 we saw here was proposed by Walker and Orin (1982). This video supplement from Modern Robotics gives a good overview of it.

Code-wise, this 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.

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.

• Posted on

Can you publish the code on MATLAB?

• Posted on

Take a look at the Spatial Matlab package by Roy Featherstone, which implements most of the algorithms from Rigid Body Dynamics Algorithms. The function for the recursive Newton-Euler algorithm is ID.

• Posted on

On running the code, I get the following error on Matlab "Unrecognized function or variable 'robot'". Please let me know a way how can I define in case of a RR manipulator.

• Posted on

I won't be able to help you with Matlab. If you don't get further replies here, you can try contacting the package developer himself, or asking your question to a broader audience like the Robotics Stack Exchange.

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

• 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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i^\mathit{ext}$ are indeed body vectors, as are the $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bff_i$'s used in intermediate computations. This is now pointed out, both under the corresponding equation and when typing the Python prototype.

• 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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} {}_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? :)

• 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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \{(\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:

$\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \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$

Where $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfY_i$ is a block line of the regressor matrix $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \bfY$, which only depends on $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \{(\bfq_i, \qd_i, \qdd_i)\}_i$, and $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \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 $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \{(\bfq_i, \qd_i, \qdd_i, \bftau_i, \bff^\mathit{ext}_i)\}_i$ from $\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \{(\bfq_i, \qd_i, \bftau_i, \bff^\mathit{ext}_i)\}_i$ can still be useful as a way to validate a system identification algorithm.

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

• Posted on

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

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.

You can use Markdown with $\LaTeX$ formulas in your comment.