# Constrained equations of motion

Let us take a closer look at how Jacobian-transpose term appears in the equations of motion of legged robots in contact with their environment:

$\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}} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c(\bfq)^\top \bff$

The Jacobian-transpose term maps external contact forces to joint torques (for actuated coordinates) and to the net floating-base wrench (for unactuated coordinates). We will see how to derive it from the principle of least constraint, using the method of Lagrange multipliers.

## Constraint derivatives¶

We consider a legged robot, for example the humanoid depicted in the figure to the right, making point contacts or surface contacts with its environment. These kinematic constraints can be summarized into a holonomic constraint $\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}} \bfc(\bfq) = \bfzero$. In the case of point contacts, it would be the stack of contact point equality constraints $\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}} \bfp_C(\bfq) = \bfp_{D}$, with $\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}} \bfp_C$ the mobile point on the robot's foot 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}} \bfp_D$ the ground contact point in the inertial frame. Differentiating $\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}} \bfc(\bfq) = \bfzero$ twice with respect to time yields:

$\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}} \bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}$

where the constraint Jacobian and Hessian matrices are defined 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}} \begin{align*} \bfJ_c(\bfq) & = \frac{\partial \bfc}{\partial \bfq}(\bfq) & \bfH_c(\bfq) & = \frac{\partial^2 \bfc}{\partial \bfq^2}(\bfq) \end{align*}

The Jacobian 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}} 3 k \times n$ matrix, with $\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$ the number of contact poins and with $\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$ the degree of freedom of our robot (number of actuated joints plus six), while the Hessian is an $\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 \times 3 k \times n$ tensor. Notations are the same for surface contacts, except that the degree of constraint for each contact is six rather than three.

## Principle of least constraint¶

Remember how, in the absence of constraint, the equations of free motion of our system would be:

$\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}} \bfM(\bfq) \qdd_{\mathrm{free}} + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_{\mathit{ext}}$

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}} \bfM(\q)$ is 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}} n \times n$ joint-space inertia 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}} \bfC(\q)$ is 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}} n \times n \times n$ joint-space Coriolis tensor,
• $\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$ is 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}} n$-dimensional vector of actuated 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_g(\q)$ is 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}} n$-dimensional vector of gravity 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_{\mathit{ext}}$ is an optional vector of external forces and torques.

We know how to compute these matrices and vectors from link inertias and Jacobians. However, when the robot is in contact, its motion doesn't follow 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}} \qdd_{\mathrm{free}}$ defined by this equation because of the additional contact constraint: the actual $\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$ has to be such 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}} \bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}$

Gauss's principle of least constraint states that the robot's actual 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$ is as close as possible to the free 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_{\mathrm{free}}$, subject to this constraint:

$\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{array}{rl} \qdd = \arg\min_{\bfa} & \frac{1}{2} \| \bfa - \qdd_{\mathrm{free}}\|_{\bfM}^2 \\ \textrm{subject to} & \bfJ_c(\bfq) \bfa + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0} \end{array}$

Note how the inertia matrix appears in the objective function to define what as close as possible means here. The inertia matrix is positive definite so this objective function is strictly convex.

### Introducing contact forces¶

We can write the Lagrangian of this equality-constrained least-squares problem 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}} \calL(\bfa, \bflambda) = \frac{1}{2} (\bfa - \qdd_{\mathrm{free}})^\top \bfM (\bfa - \qdd_{\mathrm{free}}) - \bflambda^\top \left(\bfJ_c \bfa + \qd^\top \bfH_c \qd\right)$

where we omit dependencies 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$ for concision. Dual variables stacked in 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}} \bflambda$ correspond to contact forces. Why? By definition! In Lagrangian mechanics, there are two kinds of forces: constraint and non-constraint forces. Non-constraint forces are somehow easier to deal with: they correspond to the 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}} \bftau_g(\bfq)$ 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}} \bftau_{\mathit{ext}}$ in the equations of free motion, and we already know how to compute them. Contact forces are the constraint forces arising from our kinematic contact constraint.

We can think of forces as dual variables that appear in the equations of motion to ensure that the system's motion ends up satisfying the kinematic constraint. Our assumption here is that the constraint will hold, i.e. that the robot will stay in contact with its environment. In practice this is not true for any command: if we send maximum joint torques, our robot will likely shake and jump all over the place, breaking the assumption of fixed contact. To make sure that this assumption holds, we will complement our kinematic constraint with a dynamic contact stability condition, but this is another story. Let us get back to our derivation.

### Method of Lagrange multipliers¶

From the method of Lagrange multipliers, the optimum $\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^*, \bflambda^*)$ of our constrained problem is a critical point of the Lagrange 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}} \begin{array}{ccc} \displaystyle \frac{\partial \calL}{\partial \bfa}(\bfa^*, \bflambda^*) = \bfzero & \wedge & \displaystyle \frac{\partial \calL}{\partial \bflambda}(\bfa^*, \bflambda^*) = \bfzero \end{array}$

The second equation is, by construction, our contact constraint. The first one gives us:

$\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}} \bfM (\bfa^* - \qdd_{\mathrm{free}}) - \bfJ_c^T \bflambda^* = \bfzero$

In fine, replacing $\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^*$ by our notation for joint 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}} \qdd$ and similarly $\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}} \bflambda^*$ 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}} \bff$, we obtain the constrained equations of motion in joint space:

$\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}} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff$

with $\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}} \bfM(\q)$ the joint-space inertia 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}} \bfC(\q)$ the joint-space Coriolis tensor, $\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_g(\q)$ the vector of gravity 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$ the vector of actuated 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}} \bfJ_c$ the stacked Jacobian of kinematic contact constraints 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$ the corresponding stacked vector of contact forces.

## To go further¶

This page is based on Wieber's comments on the structure of the dynamics of articulated motion, as well as on a very good summary in Section IV.A of this paper by Budhiraja et al.

Forces are virtual quantities that balance out our calculations. Defining them as the dual multipliers of motion constraints is a key step in Lagrangian mechanics: constraints "pull" on the system, which aspires to be somewhere else according to the principle of least motion, and multipliers can be seen as a measure of how hard the constraints need to pull to keep the system on track. Check out Jensen's explanation on the meaning of the multiplier for some great examples to build up intuition around this.

## Discussion ¶

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