Kinematics of a symmetric leg

Kinematics of a symmetric leg

In this post, we are taking a look at the kinematics of a simple leg with two links and one knee. Both links have the same length \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}} \ell, making the leg "symmetric" in the sense that the mirror of any configuration with respect to the vertical plane is also a valid configuration. The joint angles, depicted to the right, are further subject to the following limits:

q1<π2q2<π\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*} | q_1 | & < \frac{\pi}{2} & | q_2 | & < \pi \end{align*}

We will derive analytically the forward kinematics of this leg in the sagittal plane, then consider its inverse kinematics for crouching.

Forward kinematics

Consider the leg depicted in the figure above. The hip angle q1\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}} q_1 and knee angle q2\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}} q_2 fully define the position of every point on every link of the leg, including the origin of the hip G\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}} G (this could be the center of gravity of a lumped mass sitting on top of the leg and representing the robot's chassis), the knee K\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 and the end effector C\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}} C. Forward kinematics is the function that maps joint angles (q1,q2)\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}} (q_1, q_2) to the coordinates (xC,zC)=FK(q1,q2)\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}} (x_C, z_C) = \mathrm{FK}(q_1, q_2) of the end effector.

Our model is an articulated system (the most common case) where each link is connected to exactly one parent link by one (usually revolute) joint. For such systems, the forward kinematics function is naturally recursive: the coordinates of a point on a link can be decomposed into (1) a local offset from the joint between the link and its parent, and (2) the coordinates of that joint in the parent link. Applying this general idea to our case, we write:

xC=xK+(xCxK)zC=zK+(zCzK)\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*} x_C & = x_K + (x_C - x_K) \\ z_C & = z_K + (z_C - z_K) \end{align*}

The coordinates of the knee point are fully determined by the hip angle and the length of the first link:

xK=xG+sin(q1)zK=zGcos(q1)\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*} x_K & = x_G + \ell \sin(q_1) \\ z_K & = z_G - \ell \cos(q_1) \end{align*}

We see how the recursive argument applies to the knee as well as with the appearance of the coordinates (xG,zG)\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}} (x_G, z_G) of the origin of the hip, which is also the root of our kinematic tree. Recursion stops there.

Deriving the forward kinematics of a symmetric leg

Moving on to the second link, the figure to the right illustrates how we get the following sines and cosines:

xCxK=cos(π2q1q2)=sin(q1+q2)zCzK=sin(π2q1q2)=cos(q1+q2)\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*} x_C - x_K & = \ell \cos\left(\frac{\pi}{2} - q_1 - q_2\right) = \ell \sin(q_1 + q_2) \\ z_C - z_K & = -\ell \sin\left(\frac{\pi}{2} - q_1 - q_2\right) = -\ell \cos(q_1 + q_2) \end{align*}

Combining this system of equations with the previous one for (xK,zK)\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}} (x_K, z_K), we get the overall formula for the forward kinematics of our leg:

xC=xG+sin(q1)+sin(q1+q2)zC=zGcos(q1)cos(q1+q2)\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*} x_C & = x_G + \ell \sin(q_1) + \ell \sin(q_1 + q_2) \\ z_C & = z_G - \ell cos(q_1) - \ell \cos(q_1 + q_2) \end{align*}

This expression has the form (xC,zC)=FK(q1,q2)\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}} (x_C, z_C) = \mathrm{FK}(q_1, q_2), but the FK\def\LdG{\dot{L}_G} \def\Ld{\dot{L}} \def\bfA{\boldsymbol{A}} \def\bfB{\boldsymbol{B}} \def\bfC{\boldsymbol{C}} \def\bfD{\boldsymbol{D}} \def\bfE{\boldsymbol{E}} \def\bfF{\boldsymbol{F}} \def\bfG{\boldsymbol{G}} \def\bfH{\boldsymbol{H}} \def\bfI{\boldsymbol{I}} \def\bfJ{\boldsymbol{J}} \def\bfK{\boldsymbol{K}} \def\bfL{\boldsymbol{L}} \def\bfM{\boldsymbol{M}} \def\bfN{\boldsymbol{N}} \def\bfO{\boldsymbol{O}} \def\bfP{\boldsymbol{P}} \def\bfQ{\boldsymbol{Q}} \def\bfR{\boldsymbol{R}} \def\bfS{\boldsymbol{S}} \def\bfT{\boldsymbol{T}} \def\bfU{\boldsymbol{U}} \def\bfV{\boldsymbol{V}} \def\bfW{\boldsymbol{W}} \def\bfX{\boldsymbol{X}} \def\bfY{\boldsymbol{Y}} \def\bfZ{\boldsymbol{Z}} \def\bfalpha{\boldsymbol{\alpha}} \def\bfa{\boldsymbol{a}} \def\bfbeta{\boldsymbol{\beta}} \def\bfb{\boldsymbol{b}} \def\bfcd{\dot{\bfc}} \def\bfchi{\boldsymbol{\chi}} \def\bfc{\boldsymbol{c}} \def\bfd{\boldsymbol{d}} \def\bfe{\boldsymbol{e}} \def\bff{\boldsymbol{f}} \def\bfgamma{\boldsymbol{\gamma}} \def\bfg{\boldsymbol{g}} \def\bfh{\boldsymbol{h}} \def\bfi{\boldsymbol{i}} \def\bfj{\boldsymbol{j}} \def\bfk{\boldsymbol{k}} \def\bflambda{\boldsymbol{\lambda}} \def\bfl{\boldsymbol{l}} \def\bfm{\boldsymbol{m}} \def\bfn{\boldsymbol{n}} \def\bfomega{\boldsymbol{\omega}} \def\bfone{\boldsymbol{1}} \def\bfo{\boldsymbol{o}} \def\bfpdd{\ddot{\bfp}} \def\bfpd{\dot{\bfp}} \def\bfphi{\boldsymbol{\phi}} \def\bfp{\boldsymbol{p}} \def\bfq{\boldsymbol{q}} \def\bfr{\boldsymbol{r}} \def\bfsigma{\boldsymbol{\sigma}} \def\bfs{\boldsymbol{s}} \def\bftau{\boldsymbol{\tau}} \def\bft{\boldsymbol{t}} \def\bfu{\boldsymbol{u}} \def\bfv{\boldsymbol{v}} \def\bfw{\boldsymbol{w}} \def\bfxi{\boldsymbol{\xi}} \def\bfx{\boldsymbol{x}} \def\bfy{\boldsymbol{y}} \def\bfzero{\boldsymbol{0}} \def\bfz{\boldsymbol{z}} \def\calA{\mathcal{A}} \def\calB{\mathcal{B}} \def\calC{\mathcal{C}} \def\calD{\mathcal{D}} \def\calE{\mathcal{E}} \def\calF{\mathcal{F}} \def\calG{\mathcal{G}} \def\calH{\mathcal{H}} \def\calI{\mathcal{I}} \def\calJ{\mathcal{J}} \def\calK{\mathcal{K}} \def\calL{\mathcal{L}} \def\calM{\mathcal{M}} \def\calN{\mathcal{N}} \def\calO{\mathcal{O}} \def\calP{\mathcal{P}} \def\calQ{\mathcal{Q}} \def\calR{\mathcal{R}} \def\calS{\mathcal{S}} \def\calT{\mathcal{T}} \def\calU{\mathcal{U}} \def\calV{\mathcal{V}} \def\calW{\mathcal{W}} \def\calX{\mathcal{X}} \def\calY{\mathcal{Y}} \def\calZ{\mathcal{Z}} \def\d#1{{\rm d}{#1}} \def\defeq{\stackrel{\mathrm{def}}{=}} \def\dim{\rm dim} \def\p{\boldsymbol{p}} \def\qdd{\ddot{\bfq}} \def\qd{\dot{\bfq}} \def\q{\boldsymbol{q}} \def\xdd{\ddot{x}} \def\xd{\dot{x}} \def\ydd{\ddot{y}} \def\yd{\dot{y}} \def\zdd{\ddot{z}} \def\zd{\dot{z}} \mathrm{FK} function implicitly depends on the coordinates (xG,zG)\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}} (x_G, z_G) of the origin of the hip frame. This frame is known as the floating base of the mobile robot. We can equivalently consider it as a free joint between the hip and an inertial frame (often called the "world" frame in physics simulators and robotics software), in which case the coordinates of this joint become an additional argument to forward kinematics: (xC,zC)=FK(xG,zG,q1,q2)\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}} (x_C, z_C) = \mathrm{FK}(x_G, z_G, q_1, q_2). If we had a robotic arm, G\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}} G would directly be a point of an inertial frame and we could without loss of generality fix it to, for instance, xG=0\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}} x_G = 0 and zG=0\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}} z_G = 0.

Inverse kinematics for crouching

Kinematics of a symmetric leg

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

0=sin(q1)+sin(q1+q2)h=cos(q1)+cos(q1+q2)\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*} 0 & = \sin(q_1) + \sin(q_1 + q_2) \\ h & = \ell cos(q_1) + \ell \cos(q_1 + q_2) \end{align*}

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

cos(q1+q2)=hcos(q1)sin(q1+q2)=sin(q1)\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*} \cos(q_1 + q_2) & = \frac{h}{\ell} - \cos(q_1) \\ \sin(q_1 + q_2) & = -\sin(q_1) \end{align*}

Taking the sum of the squares of these two lines leads us to:

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

Which is great, as we now know that:

cos(q1)=h2\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}} \cos(q_1) = \frac{h}{2 \ell}

Injecting this equation back into the system gives us:

cos(q1+q2)=hcos(q1)=h2=cos(q1)sin(q1+q2)=sin(q1)\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*} \cos(q_1 + q_2) & = \frac{h}{\ell} - \cos(q_1) = \frac{h}{2 \ell} = \cos(-q_1) \\ \sin(q_1 + q_2) & = \sin(-q_1) \end{align*}

The two angles q1+q2\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}} q_1 + q_2 and q1\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}} -q_1 have the same sine and cosine, therefore they are equal up to some 2kπ,kZ\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}} 2 k \pi, k \in \mathbb{Z}. But recall our joint limit assumption:

q1<π2q2<π\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*} | q_1 | & < \frac{\pi}{2} & | q_2 | & < \pi \end{align*}

This means q1+q2(q1)=2q1+q2<2π\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}} q_1 + q_2 - (-q_1) = 2 q_1 + q_2 < 2 \pi, and the solution to our trigonometric system is unique: q2=2q1\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}} q_2 = -2 q_1. Solving for q1\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}} q_1 alone yields:

q1=arccos(h2)q2=2arccos(h2)\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*} q_1 & = \arccos\left(\frac{h}{2 \ell}\right) \\ q_2 & = -2 \arccos\left(\frac{h}{2 \ell}\right) \end{align*}

This is our inverse kinematics function (q1,q2)=IK(h)\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}} (q_1, q_2) = \mathrm{IK}(h). We can check that it matches what we expect at the two boundary cases:

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

We can feel in the latter case that there is a discontinuity where our IK function can jump and should be used with care, although we will be fine within the continuous domain q1<π2\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}} | q_1 | < \frac{\pi}{2} that we chose here. In practice the two links would collide before we reach the limit h0\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}} h \to 0, so the full crouch test here is rather an additional check for us to be convinced that our derivation is correct.

Note how we made another choice along the way: since h>0\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}} h > 0, taking q1=arccos(h/2)\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}} q_1 = \arccos(h / 2 \ell) implies that q1>0\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}} q_1 > 0 and the knee will always bend forward (as in human legs). An equally valid solution would be q1=arccos(h/2)\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}} q_1 = -\arccos(h / 2 \ell), in which case the knee will bend backward (as in bird legs).

To go further

For more complex models, it is rare that we derive the forward and inverse kinematics functions of a model analytically, although such a derivation can be done automatically for models as complex as 7-DOF robot arms. A more common approach is to write down a URDF description of the model, use fast rigid-body algorithms for forward kinematics, and numerical optimization for inverse kinematics.

Discussion

There are no comments yet. Feel free to leave a reply using the form below.

Post a comment

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

You agree to the publication of your comment on this page under the CC BY 4.0 license.

Your email address will not be published.