Inverse kinematics (IK) is the problem of computing motions (velocities,
accelerations) that achieve a given set of tasks , such as putting a foot on a
surface, moving the center of mass (CoM) to a target location, etc. These tasks
can be defined by a set x = ( x 1 , … , x N ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfx = (x_1, \ldots, x_N) x = ( x 1 , … , x N ) of points or frames
attached to the robot, with for instance x 1 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
x_1 x 1 the CoM, x 2 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
x_2 x 2 the
center of the right foot sole, etc. The problems of forward and inverse
kinematics are:
Forward kinematics: compute body motions x ( t ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfx(t) x ( t ) resulting from a
joint-angle motion q ( t ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq(t) q ( t ) . Writing F K \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
FK F K this mapping,
x ( t ) = F K ( q ( t ) ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfx(t) = FK(\bfq(t)) x ( t ) = F K ( q ( t )) .
Inverse kinematics: compute a joint-angle motion q ( t ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq(t) q ( t ) so as to
achieve a set of body motions x ( t ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfx(t) x ( t ) . If the mapping F K \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
FK F K were
invertible, we would have q ( t ) = F K − 1 ( x ( t ) ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq(t) = FK^{-1}(\bfx(t)) q ( t ) = F K − 1 ( x ( t )) .
However, the mapping F K \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
FK F K is not always invertible due to kinematic
redundancy : there may be infinitely many ways to achieve a given set of tasks.
For example, if there are only two tasks x = ( x l f , x r f ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfx = (x_{lf}, x_{rf}) x = ( x l f , x r f ) to
keep left and right foot frames at a constant location on the ground, the
humanoid can move its upper body while keeping its legs in the same
configuration. In this post, we will see a common solution for inverse
kinematics where redundancy is used achieve multiple tasks at once.
Kinematic task
Let us consider the task of bringing a point p \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfp p , located on one of
the robot’s links, to a goal position p ∗ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfp^* p ∗ , both point coordinates
being expressed in the world frame. When the robot is in configuration
q \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq q , the (position) residual of this task
is:
r ( q ) = p ∗ − p ( q ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfr(\bfq) = \bfp^* - \bfp(\bfq) r ( q ) = p ∗ − p ( q ) The goal of the task is to bring this residual to zero. Next, from forward
kinematics we know how to compute the Jacobian matrix of p \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfp p :
J ( q ) = ∂ p ∂ q ( q ) , \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ(\bfq) = \frac{\partial \bfp}{\partial \bfq}(\bfq), J ( q ) = ∂ q ∂ p ( q ) , which maps joint velocities q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd q ˙ to end-point velocities p ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfpd p ˙
via J ( q ) q ˙ = p ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ(\bfq) \qd = \bfpd J ( q ) q ˙ = p ˙ . Suppose that we apply a velocity
q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd q ˙ over a small duration δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\delta t δ t . The new residual after
δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\delta t δ t is r ′ = r − p ˙ δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfr' = \bfr - \bfpd \delta t r ′ = r − p ˙ δ t . Our goal is to
cancel it, that is r ′ = 0 ⇔ p ˙ δ t = r \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfr' = \boldsymbol{0} \Leftrightarrow \bfpd \delta t
= \bfr r ′ = 0 ⇔ p ˙ δ t = r , which leads us to define the velocity residual :
v ( q , δ t ) = d e f r ( q ) δ t = p ∗ − p ( q ) δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfv(\bfq, \delta t)
\defeq \frac{\bfr(\bfq)}{\delta t}
= \frac{\bfp^* - \bfp(\bfq)}{\delta t} v ( q , δ t ) = def δ t r ( q ) = δ t p ∗ − p ( q ) The best option is then to select q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd q ˙ such that:
J ( q ) q ˙ = p ˙ = v ( q , δ t ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ(\bfq) \qd = \bfpd = \bfv(\bfq, \delta t) J ( q ) q ˙ = p ˙ = v ( q , δ t ) If the Jacobian were invertible, we could take q ˙ = J − 1 v \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd = \bfJ^{-1} \bfv q ˙ = J − 1 v .
However, that’s usually not the case (think of a point task where J \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ J
has three rows and one column per DOF). The best solution that we can get in
the least-square sense is the solution to:
minimize q ˙ ∥ J q ˙ − v ∥ 2 , \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\underset{\qd}{\textrm{minimize}}\ \| \bfJ \qd - \bfv \|^2, q ˙ minimize ∥ J q ˙ − v ∥ 2 , and is given by the pseudo-inverse J † \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ^\dagger J † of
J \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ J : q ˙ = J † v \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd = \bfJ^\dagger \bfv q ˙ = J † v . By writing this equivalently as
( J ⊤ J ) q = J ⊤ v \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
(\bfJ^\top \bfJ) \bfq = \bfJ^\top \bfv ( J ⊤ J ) q = J ⊤ v , we see that this approach is
exactly the Gauss-Newton algorithm . (There is a
sign difference compared with the Gauss-Newton update rule, which comes from
our use of the end-effector Jacobian ∂ p / ∂ q \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\partial \bfp / \partial \bfq ∂ p / ∂ q
rather than the residual Jacobian ∂ r / ∂ q \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\partial \bfr / \partial \bfq ∂ r / ∂ q .)
Task gains
For this solution to work, the time step δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\delta t δ t should be
sufficiently small, so that the variations of the Jacobian term between
q \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq q and q + q ˙ δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq + \qd \delta t q + q ˙ δ t can be neglected. The total
variation is
J ( q + q ˙ δ t ) q ˙ − J ( q ) q ˙ = δ t q ˙ ⊤ H ( q ) q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ(\bfq + \qd \delta t) \qd - \bfJ(\bfq) \qd
= \delta t\,\qd^\top \bfH(\bfq) \qd J ( q + q ˙ δ t ) q ˙ − J ( q ) q ˙ = δ t q ˙ ⊤ H ( q ) q ˙ where H ( q ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfH(\bfq) H ( q ) is the Hessian matrix of the task. This matrix is
more expensive to compute than J \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ J . Rather than checking that the
variation above is small enough, a common practice is to multiply the
velocity residual by a proportional gain K p ∈ [ 0 , 1 ] \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
K_p \in [0, 1] K p ∈ [ 0 , 1 ] :
J ( q ) q ˙ = K p v \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfJ(\bfq) \qd = K_p\, \bfv J ( q ) q ˙ = K p v For example, K p = 0.5 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
K_p = 0.5 K p = 0.5 means that the system will (try at best to) cut
the residual by half at each time step δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\delta t δ t . Adding this gain does
not change the exponential convergence to the solution r = 0 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfr =
\boldsymbol{0} r = 0 , and helps avoid overshooting of the real solution. When you
observe instabilities in your IK tracking, reducing task gains is usually a
good idea.
Multiple tasks
So far, we have seen what happens for a single task, but redundant systems like
humanoid robots need to achieve multiple tasks at once (moving feet from
contact to contact, following with the center of mass, regulating the angular
momentum, etc.) A common practice to combine tasks is weighted combination. We
saw that a task i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
i i can be seen as minimizing ∥ J i q ˙ − K i v i ∥ 2 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\| \bfJ_i \qd - K_i
\bfv_i\|^2 ∥ J i q ˙ − K i v i ∥ 2 . Then, by associating a weight w i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
w_i w i to each task, the IK
problem becomes:
minimize q ˙ ∑ task i w i ∥ J i q ˙ − K i v i ∥ 2 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\underset{\qd}{\textrm{minimize}}\ \sum_{\textrm{task }i} w_i \| \bfJ_i \qd - K_i \bfv_i \|^2 q ˙ minimize task i ∑ w i ∥ J i q ˙ − K i v i ∥ 2 The solution to this problem can again be computed by pseudo-inverse or using a
quadratic programming (QP) solver. This formulation has some convergence
properties , but its
solutions are always a compromise between tasks, which can be a problem in
practice. For example, when a humanoid tries to make three contacts while it is
only possible to make at most two, the solution found by a weighted combination
will be somewhere “in the middle” and achieve no contact at all:
In practice, we can set implicit priorities between tasks by having e.g. w i = 1 0 4 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
w_i = 10^4 w i = 1 0 4 for the most important task, w i = 1 0 2 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
w_i = 10^2 w i = 1 0 2 for the next
one, etc. This is for instance how the pymanoid IK is used in this paper . This solution emulates the
behavior of a hierarchical quadratic program (HQP) (unfortunately,
there has been no open source HQP solver available as of 2016–2021). See this
talk by Wieber (2015) for an
deeper overview of the question.
Inequality constraints
Last but not least, we need to enforce a number of inequality constraints to
avoid solutions that violate e.g. joint limits. The overall IK problem becomes:
minimize q ˙ ∑ task i w i ∥ J i q ˙ − K i v i ∥ 2 subject to q ˙ − ≤ q ˙ ≤ q ˙ + \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\begin{align*}
\underset{\qd}{\textrm{minimize }} & \sum_{\textrm{task }i} w_i \| \bfJ_i \qd - K_i \bfv_i \|^2 \\
\textrm{subject to } & \qd^- \leq \qd \leq \qd^+
\end{align*} q ˙ minimize subject to task i ∑ w i ∥ J i q ˙ − K i v i ∥ 2 q ˙ − ≤ q ˙ ≤ q ˙ + Inequalities between vectors being taken componentwise. This time, the
pseudo-inverse solution cannot be applied (as it doesn’t handle inequalities),
but it is still possible to use a QP solver, such as CVXOPT or quadprog in Python. These solvers work on
problems with a quadratic cost function and linear inequalities:
minimize x ( 1 / 2 ) x ⊤ P x + r ⊤ x subject to G x ≤ h \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\begin{align*}
\underset{\bfx}{\textrm{minimize }} & (1/2)\,\bfx^\top \bfP \bfx + \bfr^\top \bfx \\
\textrm{subject to } & \bfG \bfx \leq \bfh
\end{align*} x minimize subject to ( 1/2 ) x ⊤ P x + r ⊤ x G x ≤ h The weighted IK problem falls under this framework.
Cost function
Consider one of the squared norms in the task summation:
∥ J i q ˙ − K i v i ∥ 2 = ( J i q ˙ − K i v i ) ⊤ ( J i q ˙ − K i v i ) = q ˙ ⊤ J i ⊤ J i q ˙ − K i v i ⊤ J i q ˙ − K i q ˙ ⊤ J i ⊤ v i + K i 2 v i ⊤ v i = q ˙ ⊤ ( J i ⊤ J i ) q ˙ − 2 ( K i v i ⊤ J i ) q ˙ + K i 2 v i ⊤ v i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\begin{align*}
\| \bfJ_i \qd - K_i \bfv_i \|^2
& = (\bfJ_i \qd - K_i \bfv_i)^\top (\bfJ_i \qd - K_i \bfv_i) \\
& = \qd^\top \bfJ_i^\top \bfJ_i \qd - K_i \bfv_i^\top \bfJ_i \qd - K_i \qd^\top \bfJ_i^\top \bfv_i + K_i^2 \bfv_i^\top \bfv_i \\
& = \qd^\top (\bfJ_i^\top \bfJ_i) \qd - 2 (K_i \bfv_i^\top \bfJ_i) \qd + K_i^2 \bfv_i^\top \bfv_i
\end{align*} ∥ J i q ˙ − K i v i ∥ 2 = ( J i q ˙ − K i v i ) ⊤ ( J i q ˙ − K i v i ) = q ˙ ⊤ J i ⊤ J i q ˙ − K i v i ⊤ J i q ˙ − K i q ˙ ⊤ J i ⊤ v i + K i 2 v i ⊤ v i = q ˙ ⊤ ( J i ⊤ J i ) q ˙ − 2 ( K i v i ⊤ J i ) q ˙ + K i 2 v i ⊤ v i where we used the fact that, for any real number x \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
x x , x ⊤ = x \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
x^\top = x x ⊤ = x .
As the term in v i ⊤ v i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfv_i^\top \bfv_i v i ⊤ v i does not depend on q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd q ˙ , the
minimum of the squared norm is the same as the minimum of ( 1 / 2 ) q ˙ ⊤ P q ˙ + r ⊤ q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
(1/2)\,
\qd^\top \bfP \qd + \bfr^\top \qd ( 1/2 ) q ˙ ⊤ P q ˙ + r ⊤ q ˙ with P i = J i ⊤ J i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfP_i = \bfJ_i^\top \bfJ_i P i = J i ⊤ J i and
r i = − K i v i ⊤ J i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfr_i = -K_i \bfv_i^\top \bfJ_i r i = − K i v i ⊤ J i . The pair ( P , r ) \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
(\bfP, \bfr) ( P , r ) for the
weighted IK problem is finally P = ∑ i w i P i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfP = \sum_i w_i \bfP_i P = ∑ i w i P i and r = ∑ i w i r i \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfr
= \sum_i w_i \bfr_i r = ∑ i w i r i .
Joint limits
Since we solve a problem in q ˙ \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\qd q ˙ , velocity limits can be directly
written as G q ˙ ≤ h \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfG \qd \leq \bfh G q ˙ ≤ h where G \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfG G stacks the matrices
+ E 3 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
+\bfE_3 + E 3 and − E 3 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
-\bfE_3 − E 3 , while h \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfh h stacks the
corresponding vectors + q ˙ + \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
+\qd^+ + q ˙ + and − q ˙ − \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
-\qd^- − q ˙ − . Next, we want to
implement limits on joint angles q − ≤ q ≤ q + \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq^- \leq \bfq \leq \bfq^+ q − ≤ q ≤ q + so that
the robot stays within its mechanical range of motion. A solution for this is
to add velocity bounds:
q ˙ − = K lim q − − q δ t q ˙ + = K lim q + − q δ t \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\begin{align*}
\qd^- & = K_{\textrm{lim}} \frac{\q^- - \q}{\delta t} \\
\qd^+ & = K_{\textrm{lim}} \frac{\q^+ - \q}{\delta t} \\
\end{align*} q ˙ − q ˙ + = K lim δ t q − − q = K lim δ t q + − q where K lim ∈ [ 0 , 1 ] \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
K_{\textrm{lim}} \in [0, 1] K lim ∈ [ 0 , 1 ] is a proportional gain. For example,
a value of K lim = 0.5 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
K_{\textrm{lim}} = 0.5 K lim = 0.5 means that a joint angle update will
not exceed half the gap separating its current value from its bounds (Kanoun,
2011) .
To go further
The formulation that we have seen here is implemented in Python in Pink , an inverse kinematics library based on Pinocchio. Pink comes with a number of examples for manipulators , wheeled biped or humanoid robots. It also runs in real-time motion control loops on the Upkie wheeled biped.
Levenberg-Marquardt damping
One efficient technique to make the IK more numerically stable is to add Levenberg-Marquardt damping , an extension of Thikonov regularization where the damping matrix is chosen proportional to the task error. This strategy is implemented e.g. by the BodyTask of Pink. See (Sugihara, 2011) for a deeper dive.
History of the expression “inverse kinematics”
Thirty-five years ago (Richard, 1981) ,
“inverse kinematics” was defined as the problem of finding joint angles
q \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
\bfq q fulfilling a set of tasks g ( q ) = 0 \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\bftheta{\boldsymbol{\theta}}
\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\defeq{\stackrel{\mathrm{def}}{=}}
\def\p{\boldsymbol{p}}
\def\qdd{\ddot{\bfq}}
\def\qd{\dot{\bfq}}
\def\q{\boldsymbol{q}}
\def\xd{\dot{x}}
\def\yd{\dot{y}}
\def\zd{\dot{z}}
g(\bfq) = 0 g ( q ) = 0 , which is a fully
geometric problem. It was solved by computing successive velocities bringing
the robot to this desired configuration, which was called “differential inverse
kinematics” (Nakamura, 1990) and
corresponds to what we call “inverse kinematics” here. Considering that
“kinematic” means motion , a number of recent papers choose to call “inverse
geometry” (or “posture generation” for humanoids) the former problem, and
reserve “kinematic” for computations including velocities or accelerations.
Discussion
You can subscribe to this Discussion's atom feed to stay tuned.
Feel free to post a comment by e-mail using the form below. Your e-mail address will not be disclosed.
© Stéphane Caron — Text and figures licensed under the CC BY 4.0 by the author.
π