Stéphane on Locomotionhttps://scaron.info/2021-12-24T00:00:00+01:00Au revoir Jean-Paul2021-12-24T00:00:00+01:002021-12-24T00:00:00+01:00Stéphane Carontag:scaron.info,2021-12-24:/blog/au-revoir-jean-paul.html<p>Un message <a class="reference external" href="https://www.laas.fr/jp-laumond/disparition">parmi tant d'autres</a> :</p>
<blockquote>
<div class="line-block">
<div class="line">Sans toi, serions-nous là ? Aurions-nous ressenti</div>
<div class="line">L'appel technologique ? Les algèbres de Lie ?</div>
<div class="line">Le fait intransigeant que <a class="reference external" href="https://www.college-de-france.fr/site/jean-paul-laumond/inaugural-lecture-2012-01-19-18h00.htm">Vulcain</a> dans son marbre</div>
<div class="line">Approuve la nouveauté et <a class="reference external" href="https://lejournal.cnrs.fr/articles/larbre-qui-deambule">donne des roues aux arbres</a>.</div>
<div class="line"><br /></div>
<div class="line">Travailler sans palabres, chercher la vérité,</div>
<div class="line">Enseigner comme on conte et d'un air naturel</div>
<div class="line">Faire sentir …</div></div></blockquote><p>Un message <a class="reference external" href="https://www.laas.fr/jp-laumond/disparition">parmi tant d'autres</a> :</p>
<blockquote>
<div class="line-block">
<div class="line">Sans toi, serions-nous là ? Aurions-nous ressenti</div>
<div class="line">L'appel technologique ? Les algèbres de Lie ?</div>
<div class="line">Le fait intransigeant que <a class="reference external" href="https://www.college-de-france.fr/site/jean-paul-laumond/inaugural-lecture-2012-01-19-18h00.htm">Vulcain</a> dans son marbre</div>
<div class="line">Approuve la nouveauté et <a class="reference external" href="https://lejournal.cnrs.fr/articles/larbre-qui-deambule">donne des roues aux arbres</a>.</div>
<div class="line"><br /></div>
<div class="line">Travailler sans palabres, chercher la vérité,</div>
<div class="line">Enseigner comme on conte et d'un air naturel</div>
<div class="line">Faire sentir, et aimer, le monde intellectuel</div>
<div class="line">Que tu nous dépeignais du tableau au café.</div>
<div class="line"><br /></div>
<div class="line">Aujourd'hui matelots sur le pont t'honorons,</div>
<div class="line">Demain dedans nos cales, de retour au charbon,</div>
<div class="line">Nous reverrons encore ton image et ta voix.</div>
<div class="line"><br /></div>
<div class="line">À tes histoires lyriques, à ton côté rieur,</div>
<div class="line">À ton regard intense de charmant professeur,</div>
<div class="line">À toi qui nous montras. À toi qui a été là.</div>
</div>
</blockquote>
<p>Au revoir Jean-Paul. 守</p>
<img alt="Jean-Paul Laumond" src="https://scaron.info/images/jpl.jpg" />
<p><a class="reference external" href="https://fr.wikipedia.org/wiki/Jean-Paul_Laumond">Jean-Paul Laumond</a>, 1953–2021</p>
Recursive Newton-Euler algorithm2021-12-06T00:00:00+01:002021-12-06T00:00:00+01:00Stéphane Carontag:scaron.info,2021-12-06:/robot-locomotion/recursive-newton-euler-algorithm.html<p>Inverse dynamics refers to the computation of forces from motions. Given the
configuration <span class="math">\(\bfq\)</span>, generalized velocity <span class="math">\(\qd\)</span> and generalized
acceleration <span class="math">\(\qdd\)</span>, it amounts to finding the joint torques
<span class="math">\(\bftau\)</span> and contact forces <span class="math">\(\bff^\mathit{ext}\)</span> such that the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd …<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>Inverse dynamics refers to the computation of forces from motions. Given the
configuration <span class="math">\(\bfq\)</span>, generalized velocity <span class="math">\(\qd\)</span> and generalized
acceleration <span class="math">\(\qdd\)</span>, it amounts to finding the joint torques
<span class="math">\(\bftau\)</span> and contact forces <span class="math">\(\bff^\mathit{ext}\)</span> such that the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff^\mathit{ext} \\
\bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = \boldsymbol{0}
\end{align}<p>Mathematically, we could formulate this operation as a function:</p>
<div class="math">
\begin{equation*}
(\bftau, \bff^\mathit{ext}) = \mathrm{ID}(\bfq, \qd, \qdd)
\end{equation*}
</div>
<p>This function is well-defined when our linear system is fully determined, for
instance for an arm with six degrees of freedom, but for mobile robots under
multiple contacts it is often under-determined. In that case, we can postpone
the calculation of external forces to <em>e.g.</em> a contact model and compute joint
torques. The recursive Newton-Euler algorithm (RNEA) gives us an efficient way
to do this:</p>
<div class="math">
\begin{equation*}
\bftau = \mathrm{RNEA}(\bfq, \qd, \qdd, \bff^\mathit{ext})
\end{equation*}
</div>
<p>The algorithm works in two passes: a forward pass which is mostly a
second-order forward kinematics, followed by a backward pass that computes
forces and joint torques.</p>
<div class="section" id="forward-kinematic-pass">
<h2>Forward kinematic pass</h2>
<p>The first pass of RNEA computes body velocities <span class="math">\(\bfv_i\)</span> and
accelerations <span class="math">\(\bfa_i\)</span>. Starting from the root <span class="math">\(i = 0\)</span> of the
kinematic tree, the motion <span class="math">\(\bfv_{i}, \bfa_i\)</span> of body <span class="math">\(i\)</span> is
computed from the motion <span class="math">\(\bfv_{\lambda(i)}, \bfa_{\lambda(i)}\)</span> of its
parent body <span class="math">\(\lambda(i)\)</span>, plus the component caused by the motion
<span class="math">\(\qd_i, \qdd_i\)</span> of the joint between them. Let's start with the body
velocity:</p>
<div class="math">
\begin{equation*}
\bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i
\end{equation*}
</div>
<p>In this equation, <span class="math">\({}^i \bfX_{\lambda(i)}\)</span> is the Plücker transform from
<span class="math">\(\lambda(i)\)</span> to <span class="math">\(i\)</span>, and <span class="math">\(\bfS_i\)</span> is the motion subspace
matrix of the joint <span class="math">\(i\)</span>. Note that <span class="math">\(\qd_i \in \mathbb{R}^k\)</span> is the
velocity of the joint, for instance <span class="math">\(k = 6\)</span> for the floating base
(<em>a.k.a.</em> free flyer) joint, <span class="math">\(k = 2\)</span> for a spherical joint, and <span class="math">\(k
= 1\)</span> for a revolute or a prismatic joint. At any rate, <span class="math">\(\qd_i\)</span> is <em>not</em>
the <span class="math">\(i^\mathrm{th}\)</span> component of the generalized velocity vector
<span class="math">\(\qd\)</span> (which would not make sense since <span class="math">\(i\)</span> is the index of a joint
whereas the vector <span class="math">\(\qd\)</span> is indexed by degrees of freedom). Consequently,
the motion subspace matrix has dimension <span class="math">\(6 \times k\)</span>.</p>
<p>In what follows, let us assume a "common" joint (revolute, prismatic, helical,
cylindrical, planar, spherical, free flyer) so that the apparent time
derivative of the motion subspace matrix is zero. Nevermind this statement
unless you are dealing with a different joint ;-) Then, the body acceleration
computed from the parent during the forward pass is:</p>
<div class="math">
\begin{equation*}
\bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i
\end{equation*}
</div>
<p>So far this forward pass is a second-order forward kinematics. The last thing we want to compute along the way are the inertial forces produced by the motion <span class="math">\(\bfv_i, \bfa_i\)</span> of body <span class="math">\(i\)</span>:</p>
<div class="math">
\begin{equation*}
\bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}
\end{equation*}
</div>
<p>We will update these body force vectors during the backward pass.</p>
</div>
<div class="section" id="backward-dynamic-pass">
<h2>Backward dynamic pass</h2>
<p>The second pass of RNEA computes body forces. Starting from leaf nodes of the
kinematic tree, the generalized force <span class="math">\(\bff_{i}\)</span> of body <span class="math">\(i\)</span> is
added to the force <span class="math">\(\bff_{\lambda(i)}\)</span> computed so far for its parent
<span class="math">\(\lambda(i)\)</span>:</p>
<div class="math">
\begin{equation*}
\bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i
\end{equation*}
</div>
<p>Once the generalized force <span class="math">\(\bff_i\)</span> on body <span class="math">\(i\)</span> is computed, we get
the corresponding joint torque <span class="math">\(\bftau_i\)</span> by projecting this 6D body
vector along the joint axis:</p>
<div class="math">
\begin{equation*}
\bftau_i = \bfS_i^\top \bff_i
\end{equation*}
</div>
<p>For a revolute joint, <span class="math">\(\bfS_i\)</span> is a <span class="math">\(6 \times 1\)</span> column vector, so
that we end with a single number <span class="math">\(\tau_i = \bfS_i^\top \bff_i\)</span>: the
actuation torque that the joint servo should provide to track <span class="math">\((\bfq,
\qd, \qdd, \bff^\mathit{ext})\)</span>. All other components correspond to the five
degrees of constraint of our revolute joint and will be provided passively by
the joint's mechanics.</p>
</div>
<div class="section" id="pseudocode">
<h2>Pseudocode</h2>
<p>Summing up the math, our function <span class="math">\(\mathrm{RNEA}(\bfq, \qd, \qdd,
\bff^\mathit{ext})\)</span> does the following:</p>
<ul class="simple">
<li><span class="math">\(\bfv_0 = 0\)</span>, <span class="math">\(\bfa_0 = \mathrm{gravity}\)</span></li>
<li><dl class="first docutils">
<dt><strong>for</strong> link <span class="math">\(i = 1\)</span> to <span class="math">\(n\)</span>:</dt>
<dd><ul class="first last">
<li><span class="math">\({}^i \bfX_{\lambda(i)}, \bfS_i, \bfI_i = \mathrm{compute\_joint}(\mathrm{joint\_type}_i, \bfq_i, \qd_i)\)</span></li>
<li><span class="math">\(\bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i\)</span></li>
<li><span class="math">\(\bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i\)</span></li>
<li><span class="math">\(\bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}\)</span></li>
</ul>
</dd>
</dl>
</li>
<li><dl class="first docutils">
<dt><strong>for</strong> link <span class="math">\(i = n\)</span> to <span class="math">\(1\)</span>:</dt>
<dd><ul class="first last">
<li><span class="math">\(\bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i\)</span></li>
<li><span class="math">\(\bftau_i = \bfS_i^\top \bff_i\)</span></li>
</ul>
</dd>
</dl>
</li>
</ul>
<p>Let us explicit a few more things by doing the same in pseudo-Python. Our
function prototype is:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">qd</span><span class="p">,</span> <span class="n">qdd</span><span class="p">,</span> <span class="n">f_ext</span><span class="p">):</span>
<span class="c1"># ...</span>
</pre>
<p>Note that <tt class="docutils literal">q</tt> is a list of generalized coordinates for each joint, not a flat
array, and that the same holds for the other parameters. With Python type
annotations, our prototype would therefore look like this:</p>
<pre class="code python literal-block">
<span class="kn">from</span> <span class="nn">typing</span> <span class="kn">import</span> <span class="n">List</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span>
<span class="n">q</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="n">qd</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="n">qdd</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="n">f_ext</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="p">)</span> <span class="o">-></span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]:</span>
<span class="c1"># ...</span>
</pre>
<p>This extra structure allows more general joints, such as spherical ones (not
common) or a free-flyer joint for the floating base of a mobile robot (common).
If all joints were revolute, then all types would coalesce to flat arrays.</p>
<p>Let us denote by <span class="math">\(\bfv_0 = \bfzero\)</span> the spatial velocity of the root link
of our kinematic tree, and <span class="math">\(\bfa_0\)</span> its spatial acceleration. We
initialize them to respectively zero and the <a class="reference external" href="https://en.wikipedia.org/wiki/Standard_gravity">standard acceleration due to
gravity</a>:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">qd</span><span class="p">,</span> <span class="n">qdd</span><span class="p">,</span> <span class="n">f_ext</span><span class="p">):</span>
<span class="n">n</span> <span class="o">=</span> <span class="nb">len</span><span class="p">(</span><span class="n">qd</span><span class="p">)</span> <span class="o">-</span> <span class="mi">1</span> <span class="c1"># number of links == number of joints - 1</span>
<span class="n">v</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">a</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">f</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">tau</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">(</span><span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span>
<span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">gravity</span>
</pre>
<p>We continue with the forward pass, which ranges from link <span class="math">\(i = 1\)</span> to the
last link <span class="math">\(i = n\)</span> of the tree:</p>
<pre class="code python literal-block">
<span class="c1"># def rnea(q, qd, qdd, f_ext):</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="c1"># p for "parent"</span>
<span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">compute_joint</span><span class="p">(</span><span class="n">joint_type</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">q</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qdd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">])</span> <span class="o">-</span> <span class="n">f_ext</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
</pre>
<p>The backward pass traverses the same range in reverse order:</p>
<pre class="code python literal-block">
<span class="c1"># def rnea(q, qd, qdd, f_ext):</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">tau</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">f</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
</pre>
<p>Wrapping it up, we get:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">qd</span><span class="p">,</span> <span class="n">qdd</span><span class="p">,</span> <span class="n">f_ext</span><span class="p">):</span>
<span class="n">n</span> <span class="o">=</span> <span class="nb">len</span><span class="p">(</span><span class="n">qd</span><span class="p">)</span>
<span class="n">v</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">a</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">f</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">tau</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">(</span><span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span>
<span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">gravity</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">compute_joint</span><span class="p">(</span><span class="n">joint_type</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">q</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qdd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">])</span> <span class="o">-</span> <span class="n">f_ext</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">tau</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">f</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="k">return</span> <span class="n">tau</span>
</pre>
<p>Lists of arrays with different lengths are typically an internal structure in
rigid body dynamics libraries or simulators. A mapping from such lists to a
flat array structure is called an <em>articulation</em>, and decides <em>e.g.</em> how to
represent orientations for spherical and free-flyer joints (unit quaternions?
full rotation matrices? or, heaven forfend, some flavor of Euler angles).</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Dually to forward kinematics being the "easy" problem relative to <a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse
kinematics</a>, inverse dynamics is "easy"
relative to <a class="reference external" href="https://scaron.info/robot-locomotion/forward-dynamics.html">forward dynamics</a>. The
recursive Newton-Euler algorithm was proposed by <a class="reference external" href="https://doi.org/10.1115/1.3139699">Walker and Orin (1982)</a>. This <a class="reference external" href="https://www.youtube.com/watch?v=ZASVKAlegfQ">video supplement from Modern
Robotics</a> gives a good overview
of it. The algorithm is implemented in rigid body dynamics libraries, such as
<a class="reference external" href="https://www.openrave.org/">OpenRAVE</a> and <a class="reference external" href="https://stack-of-tasks.github.io/pinocchio/">Pinocchio</a>, as well as dynamic simulators,
such as <a class="reference external" href="https://raisim.com/">Raisim</a>. It is summed up in concise form in
Table 5.1 of Featherstone's <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid body dynamics algorithms</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Point mass model2021-11-03T00:00:00+01:002021-11-03T00:00:00+01:00Stéphane Carontag:scaron.info,2021-11-03:/robot-locomotion/point-mass-model.html<p>The point mass model is a recurring reduced model in physics in general, and in
locomotion modeling in particular. It is a common ancestor to the inverted
pendulum and <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> models. In this post, we will
review the assumptions that define it, and some of their not-so-intuitive
consequences …</p><p>The point mass model is a recurring reduced model in physics in general, and in
locomotion modeling in particular. It is a common ancestor to the inverted
pendulum and <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> models. In this post, we will
review the assumptions that define it, and some of their not-so-intuitive
consequences.</p>
<div class="section" id="sufficient-actuation">
<h2>Sufficient actuation</h2>
<p>The point mass model can be used as a reduced model assuming first that the
robot has enough joint torque to provide for the actuated part of its
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> (Assumption 1). This
way, we focus on the remaining <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton-Euler equations</a> that correspond to the six unactuated
coordinates of the robot's floating base:</p>
<div class="math">
\begin{equation*}
\begin{bmatrix} m \bfpdd_G \\ \dot{\bfL}_G \end{bmatrix}
=
\begin{bmatrix} \bff \\ \bftau_G \end{bmatrix}
+
\begin{bmatrix} m \bfg \\ \boldsymbol{0} \end{bmatrix}
\end{equation*}
</div>
<p>where on the left-hand side <span class="math">\(\bfp_G\)</span> is the position of the center
of mass (CoM) and <span class="math">\(\bfL_G\)</span> is the net angular momentum around the CoM,
while on the right-hand side <span class="math">\(\bff\)</span> is the resultant of contact forces,
<span class="math">\(\bftau_G\)</span> is the moment of contact forces around the CoM, <span class="math">\(m\)</span> is
the robot mass and <span class="math">\(\bfg\)</span> is the gravity vector.</p>
</div>
<div class="section" id="concentrated-mass">
<h2>Concentrated mass</h2>
<p>The second key assumption is that all of the robot's mass is concentrated at
the CoM, yet contact forces can still act on the robot via massless limbs. A
non-straightforward consequence of this is hypothesis is that the reaction
force <em>has to</em> go through the center of mass, where the net angular momentum is
zero. At Assumption 1, Newton-Euler equations leave us with:</p>
<div class="math">
\begin{equation*}
\dot{\bfL}_G = \bftau_G = (\bfp_Z - \bfp_G) \times \bff
\end{equation*}
</div>
<p>where <span class="math">\(Z\)</span> is the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">ZMP</a>, which
we can think of in what follows as the point where the resultant force is
applied (for more precision, see the ZMP <a class="reference external" href="https://scaron.info/robotics/zero-tilting-moment-point.html#definition">definition as a non-central axis</a>). If
our reduced model was a rigid body, its angular velocity would be such that</p>
<div class="math">
\begin{equation*}
\bfI_G \dot{\bfomega} = \dot{\bfL}_G = (\bfp_Z - \bfp_G) \times \bff
\end{equation*}
</div>
<p>where the (locked) inertia matrix <span class="math">\(\bfI_G\)</span> is a positive definite matrix.
We see that the resultant force <span class="math">\(\bff\)</span> does not have to go through the
CoM, and that its deviation from the ZMP-CoM axis yields angular accelerations
<span class="math">\(\dot{\bfomega}\)</span>. For the sake of our thought experiment (this is not a
proper proof), let's assume we keep our angular acceleration constant and look
at what happens when we and concentrate all the robot's mass at its CoM. The
inertia matrix <span class="math">\(\bfI_G\)</span> is obtained by summing up all body inertias:</p>
<div class="math">
\begin{equation*}
\bfI_G = \sum_{\mathrm{body} \ i} \bfI_i + m_i (\bfp_i - \bfp_G)_\times (\bfp_i - \bfp_G)^\top_\times
\end{equation*}
</div>
<p>where <span class="math">\(\bfI_i\)</span> is the standard inertia matrix at body <span class="math">\(i\)</span>'s center
of mass, as is <em>e.g.</em> described in the <a class="reference external" href="https://wiki.ros.org/urdf/XML/link">inertial element of the URDF format</a>. It is calculated by summing similarly
over all volumes of the body with non-zero mass density, with <a class="reference external" href="https://en.wikipedia.org/wiki/List_of_moments_of_inertia">formulas
available</a> for
primitive volumes.</p>
<p>Now, what happens as we shift away all of the mass to the CoM? Body inertia
matrices <span class="math">\(\bfI_i \to \bfzero\)</span> as all bodies become massless and their
respective mass density functions become zero. Distances <span class="math">\(\| \bfp_i -
\bfp_G \| \to 0\)</span> for non-zero masses become zero as well, since all the mass is
moved to the CoM. We then end up, at the limit, with <span class="math">\(\bfI_G \to
\bfzero\)</span>. Since for the sake of the example we are keeping
<span class="math">\(\dot{\bfomega}\)</span> at a fixed value, this means:</p>
<div class="math">
\begin{equation*}
(\bfp_Z - \bfp_G) \times \bff = \bfI_G \dot{\bfomega} \to \bfzero
\end{equation*}
</div>
<p>That is, <span class="math">\(\bff\)</span> and <span class="math">\((\bfp_Z - \bfp_G)\)</span> become aligned, and the
resultant force goes through the center of mass. Here is a visual summary of
this property:</p>
<img alt="From rigid body to point mass model" class="center max-width-80pct" src="https://scaron.info/figures/rigid-body-to-point-mass.png" />
<p>In the limit of this process, angular coordinates have vanished from our
equations of motion and we are left with:</p>
\begin{align}
m \bfpdd_G & = \bff \\
(\bfp_Z - \bfp_G) \times \bff & = \bfzero
\end{align}<p>This property that the resultant force goes through the center of mass is
common to all point mass models, including the inverted pendulum and linear
inverted pendulum models.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The point mass model is frequently used in legged locomotion via the <em>inverted
pendulum</em> or <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> models. It is also used
directly, for instance for <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01574687/file/tro_capture_point.pdf">fall prediction</a> of
limbed robots making multiple contacts with their environment.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Simple linear regression with online updates2021-10-07T00:00:00+02:002021-10-07T00:00:00+02:00Stéphane Carontag:scaron.info,2021-10-07:/blog/simple-linear-regression-with-online-updates.html<p>Simple linear regression is a particular case of linear regression where we
assume that the output <span class="math">\(y \in \mathbb{R}\)</span> is an affine transform of the
input <span class="math">\(x \in \mathbb{R}\)</span>:</p>
<div class="math">
\begin{equation*}
y = \alpha + \beta x
\end{equation*}
</div>
<p>We gather observations <span class="math">\((x_i, y_i)\)</span> and search for the parameters
<span class="math">\((\alpha …</span></p><script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>Simple linear regression is a particular case of linear regression where we
assume that the output <span class="math">\(y \in \mathbb{R}\)</span> is an affine transform of the
input <span class="math">\(x \in \mathbb{R}\)</span>:</p>
<div class="math">
\begin{equation*}
y = \alpha + \beta x
\end{equation*}
</div>
<p>We gather observations <span class="math">\((x_i, y_i)\)</span> and search for the parameters
<span class="math">\((\alpha, \beta)\)</span> that minimize a loss function such as the residual sum
of squares. Graphically, we are fitting the line that minimizes the sum of
squared distances to every point:</p>
<img alt="Simple linear regression" class="center" src="https://scaron.info/figures/simple-linear-regression.png" />
<p>Compared to a full-fledged <a class="reference external" href="https://en.wikipedia.org/wiki/Linear_regression">linear regression</a>, simple linear regression
has a number of properties we can use for faster computations and online
updates. In this post, we will see how to implement it in Python, first with a
fixed dataset, then with online updates, and finally with a sliding-window
variant where recent observations have more weight than older ones.</p>
<div class="section" id="id1">
<h2>Linear regression</h2>
<p>Let us start with a vanilla solution using an ordinary linear regression
solver. We stack our inputs into a vector <span class="math">\(\bfx = [x_1, \ldots,
x_N]^\top\)</span> and their corresponding outputs into <span class="math">\(\bfy = [y_1, \ldots,
y_N]^\top\)</span>. Our objective function is the residual sum of squares:</p>
<div class="math">
\begin{equation*}
\underset{\alpha, \beta}{\mathrm{minimize}} \ \| \bfy - \alpha \bfone - \beta \bfx \|_2^2
\end{equation*}
</div>
<p>where <span class="math">\(\bfone\)</span> is the column vector filled with ones that has the same
dimension as <span class="math">\(\bfx\)</span> and <span class="math">\(\bfy\)</span>. The parameter <span class="math">\(\alpha\)</span> is
called the <em>intercept</em> while <span class="math">\(\beta\)</span> is the <em>slope</em> (or coefficient).</p>
<div class="section" id="using-a-machine-learning-library">
<h3>Using a machine learning library</h3>
<p>For our tests, we will generate data using the
same model as for the figure above:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="k">def</span> <span class="nf">generate_dataset</span><span class="p">(</span><span class="n">size</span><span class="o">=</span><span class="mi">10000</span><span class="p">,</span> <span class="n">alpha</span><span class="o">=</span><span class="mf">1.5</span><span class="p">,</span> <span class="n">beta</span><span class="o">=</span><span class="mf">3.15</span><span class="p">):</span>
<span class="n">x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">random</span><span class="p">(</span><span class="n">size</span><span class="p">)</span>
<span class="n">y</span> <span class="o">=</span> <span class="n">alpha</span> <span class="o">+</span> <span class="n">beta</span> <span class="o">*</span> <span class="n">x</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mf">0.4</span><span class="p">,</span> <span class="n">size</span><span class="o">=</span><span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span>
<span class="k">return</span> <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
</pre>
<p>Any machine learning library will happily estimate the intercept and
coefficients of a linear model from such observation data. Try it out with your
favorite one! Here is for example how linear regression goes with <a class="reference external" href="https://scikit-learn.org/stable/modules/linear_model.html#ordinary-least-squares">scikit-learn</a>:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">sklearn.linear_model</span>
<span class="k">def</span> <span class="nf">simple_linear_regression_sklearn</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
<span class="n">regressor</span> <span class="o">=</span> <span class="n">sklearn</span><span class="o">.</span><span class="n">linear_model</span><span class="o">.</span><span class="n">LinearRegression</span><span class="p">()</span>
<span class="n">regressor</span><span class="o">.</span><span class="n">fit</span><span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">),</span> <span class="n">y</span><span class="p">)</span>
<span class="n">intercept</span> <span class="o">=</span> <span class="n">regressor</span><span class="o">.</span><span class="n">intercept_</span>
<span class="n">slope</span> <span class="o">=</span> <span class="n">regressor</span><span class="o">.</span><span class="n">coef_</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
<span class="k">return</span> <span class="p">(</span><span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span><span class="p">)</span>
</pre>
<p>Yet, our problem is simpler than a general linear regression. What if we solved
the <a class="reference external" href="https://scaron.info/blog/linear-least-squares-in-python.html">least squares</a> problem
directly?</p>
</div>
<div class="section" id="using-a-least-squares-solver">
<h3>Using a least squares solver</h3>
<p>We can rewrite our objective function in matrix form as:</p>
<div class="math">
\begin{equation*}
\| \bfy - \alpha \bfone - \beta \bfx \|_2^2
= \left\|
\begin{bmatrix} \bfone & \bfx \end{bmatrix}
\begin{bmatrix} \alpha \\ \beta \end{bmatrix}
- \bfy
\right\|_2^2
= \| \bfR \bfx - \bfy \|_2^2
\end{equation*}
</div>
<p>with <span class="math">\(\bfR\)</span> the <span class="math">\(2 \times N\)</span> matrix with a first column of ones and
<span class="math">\(\bfx\)</span> as second column. We can give these matrices to any least squares
solver, for instance in Python using the <a class="reference external" href="https://scaron.info/doc/qpsolvers/least-squares.html#qpsolvers.solve_ls">solve_ls</a>
function from <a class="reference external" href="https://pypi.org/project/qpsolvers/">qpsolvers</a>:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">qpsolvers</span>
<span class="k">def</span> <span class="nf">simple_linear_regression_ls</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="s2">"quadprog"</span><span class="p">):</span>
<span class="n">shape</span> <span class="o">=</span> <span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="mi">1</span><span class="p">)</span>
<span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">np</span><span class="o">.</span><span class="n">ones</span><span class="p">(</span><span class="n">shape</span><span class="p">),</span> <span class="n">x</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="n">shape</span><span class="p">)])</span>
<span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span> <span class="o">=</span> <span class="n">qpsolvers</span><span class="o">.</span><span class="n">solve_ls</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">s</span><span class="o">=</span><span class="n">y</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="n">solver</span><span class="p">)</span>
<span class="k">return</span> <span class="p">(</span><span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span><span class="p">)</span>
</pre>
<p>This results in a faster implementation than the previous one, for instance on
my machine:</p>
<pre class="code python literal-block">
<span class="n">In</span> <span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span> <span class="o">=</span> <span class="n">generate_dataset</span><span class="p">(</span><span class="mi">1000</span> <span class="o">*</span> <span class="mi">1000</span><span class="p">)</span>
<span class="n">In</span> <span class="p">[</span><span class="mi">2</span><span class="p">]:</span> <span class="o">%</span><span class="n">timeit</span> <span class="n">simple_linear_regression_sklearn</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="mf">21.2</span> <span class="n">ms</span> <span class="err">±</span> <span class="mf">22.8</span> <span class="n">µs</span> <span class="n">per</span> <span class="n">loop</span> <span class="p">(</span><span class="n">mean</span> <span class="err">±</span> <span class="n">std</span><span class="o">.</span> <span class="n">dev</span><span class="o">.</span> <span class="n">of</span> <span class="mi">7</span> <span class="n">runs</span><span class="p">,</span> <span class="mi">10</span> <span class="n">loops</span> <span class="n">each</span><span class="p">)</span>
<span class="n">In</span> <span class="p">[</span><span class="mi">3</span><span class="p">]:</span> <span class="o">%</span><span class="n">timeit</span> <span class="n">simple_linear_regression_ls</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="s2">"quadprog"</span><span class="p">)</span>
<span class="mf">11.4</span> <span class="n">ms</span> <span class="err">±</span> <span class="mf">73.9</span> <span class="n">µs</span> <span class="n">per</span> <span class="n">loop</span> <span class="p">(</span><span class="n">mean</span> <span class="err">±</span> <span class="n">std</span><span class="o">.</span> <span class="n">dev</span><span class="o">.</span> <span class="n">of</span> <span class="mi">7</span> <span class="n">runs</span><span class="p">,</span> <span class="mi">100</span> <span class="n">loops</span> <span class="n">each</span><span class="p">)</span>
</pre>
<p>The improvement may be due to avoiding extra conversions inside
<tt class="docutils literal"><span class="pre">scikit-learn</span></tt>, or because the solver it uses internally is less efficient
than <a class="reference external" href="https://pypi.org/project/quadprog/">quadprog</a> for this type of
problems. It is more significant for small datasets, and ultimately settles
around <span class="math">\(2\times\)</span> for large datasets. Yet, we are still relying on a
general least squares solution (we can add columns to <span class="math">\(\bfR\)</span> for more
input variables), which means we are not squeezing out all the improvements we
can get from <em>simple</em> linear regression.</p>
</div>
</div>
<div class="section" id="simple-linear-regression">
<h2>Simple linear regression</h2>
<p>Let us expand our objective function using the vector dot product:</p>
\begin{align}
& \| \bfy - (\alpha \bfone + \beta \bfx) \|_2^2 \\
& = ((\alpha \bfone + \beta \bfx) - \bfy) \cdot ((\alpha \bfone + \beta \bfx) - \bfy) \\
& = \| \alpha \bfone + \beta \bfx \|_2^2 - 2 \bfy \cdot (\alpha \bfone + \beta \bfx) + (\bfy \cdot \bfy) \\
& = \alpha^2 (\bfone \cdot \bfone) + 2 \alpha \beta (\bfone \cdot \bfx) + \beta^2 (\bfx \cdot \bfx) - 2 (\bfone \cdot \bfy) \alpha - 2 (\bfx \cdot \bfy) \beta + (\bfy \cdot \bfy)
\end{align}<p>Since this function is strictly convex, its global minimum is the (unique)
<a class="reference external" href="https://en.wikipedia.org/wiki/Critical_point_(mathematics)#Several_variables">critical point</a>
where its gradient vanishes:</p>
\begin{align}
\frac{\partial}{\partial \alpha} \| \bfy - (\alpha \bfone + \beta \bfx) \|_2^2 & = 0 &
\frac{\partial}{\partial \beta} \| \bfy - (\alpha \bfone + \beta \bfx) \|_2^2 & = 0
\end{align}<p>Using the dot product expansion, this condition becomes:</p>
\begin{align}
2 (\bfone \cdot \bfone) \alpha + 2 (\bfone \cdot \bfx) \beta + 0 - 2 (\bfone \cdot \bfy) + 0 + 0 & = 0 \\
0 + 2 (\bfone \cdot \bfx) \alpha + 2 (\bfx \cdot \bfx) \beta - 0 - 2 (\bfx \cdot \bfy) + 0 & = 0
\end{align}<p>We end up with a <span class="math">\(2 \times 2\)</span> system of linear equations:</p>
\begin{align}
(\bfone \cdot \bfone) \alpha + (\bfone \cdot \bfx) \beta & = (\bfone \cdot \bfy) \\
(\bfone \cdot \bfx) \alpha + (\bfx \cdot \bfx) \beta & = (\bfx \cdot \bfy)
\end{align}<p>We know its analytical solution by (manual Gaussian elimination or remembering
the formula for) <a class="reference external" href="https://en.wikipedia.org/wiki/Cramer%27s_rule">Cramer's rule</a>:</p>
\begin{align}
\alpha & = \frac{(\bfone \cdot \bfy) (\bfx \cdot \bfx) - (\bfx \cdot \bfy) (\bfone \cdot \bfx)}{(\bfone \cdot \bfone) (\bfx \cdot \bfx) - (\bfone \cdot \bfx)^2} &
\beta & = \frac{(\bfone \cdot \bfone) (\bfx \cdot \bfy) - (\bfone \cdot \bfx) (\bfone \cdot \bfy)}{(\bfone \cdot \bfone) (\bfx \cdot \bfx) - (\bfone \cdot \bfx)^2}
\end{align}<p>The numerators and denominator of these two expressions have a statistical
interpretation detailed in <a class="reference external" href="https://en.wikipedia.org/w/index.php?title=Simple_linear_regression&oldid=1046955640#Fitting_the_regression_line">fitting the regression line</a>
(Wikipedia). The denominator is positive by the <a class="reference external" href="https://en.wikipedia.org/wiki/Cauchy%E2%80%93Schwarz_inequality">Cauchy-Schwarz inequality</a>. It can be
zero in the degenerate case where <span class="math">\(\bfx = x \bfone\)</span>, that is, when all
input values are equal, but we will assume that's not the case in what follows.
All in all, the Python code corresponding to this solution needs only NumPy and
floating-point arithmetic:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">simple_linear_regression_cramer</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
<span class="n">dot_1_1</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="c1"># == np.dot(ones, ones)</span>
<span class="n">dot_1_x</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == np.dot(x, ones)</span>
<span class="n">dot_1_y</span> <span class="o">=</span> <span class="n">y</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == np.dot(y, ones)</span>
<span class="n">dot_x_x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">x</span><span class="p">)</span>
<span class="n">dot_x_y</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">**</span> <span class="mi">2</span> <span class="c1"># not checking det == 0</span>
<span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_y</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_x_y</span> <span class="o">*</span> <span class="n">dot_1_x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_y</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">*</span> <span class="n">dot_1_y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="k">return</span> <span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span>
</pre>
<p>This implementation is not only faster than the previous ones, it's also the
first one where we leveraged some particular structure of our problem:</p>
<pre class="code python literal-block">
<span class="n">In</span> <span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span> <span class="o">=</span> <span class="n">generate_dataset</span><span class="p">(</span><span class="mi">1000</span> <span class="o">*</span> <span class="mi">1000</span><span class="p">)</span>
<span class="n">In</span> <span class="p">[</span><span class="mi">2</span><span class="p">]:</span> <span class="o">%</span><span class="n">timeit</span> <span class="n">simple_linear_regression_ls</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="s2">"quadprog"</span><span class="p">)</span>
<span class="mf">11.4</span> <span class="n">ms</span> <span class="err">±</span> <span class="mi">36</span> <span class="n">µs</span> <span class="n">per</span> <span class="n">loop</span> <span class="p">(</span><span class="n">mean</span> <span class="err">±</span> <span class="n">std</span><span class="o">.</span> <span class="n">dev</span><span class="o">.</span> <span class="n">of</span> <span class="mi">7</span> <span class="n">runs</span><span class="p">,</span> <span class="mi">100</span> <span class="n">loops</span> <span class="n">each</span><span class="p">)</span>
<span class="n">In</span> <span class="p">[</span><span class="mi">3</span><span class="p">]:</span> <span class="o">%</span><span class="n">timeit</span> <span class="n">simple_linear_regression_cramer</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="mf">3.72</span> <span class="n">ms</span> <span class="err">±</span> <span class="mi">160</span> <span class="n">µs</span> <span class="n">per</span> <span class="n">loop</span> <span class="p">(</span><span class="n">mean</span> <span class="err">±</span> <span class="n">std</span><span class="o">.</span> <span class="n">dev</span><span class="o">.</span> <span class="n">of</span> <span class="mi">7</span> <span class="n">runs</span><span class="p">,</span> <span class="mi">100</span> <span class="n">loops</span> <span class="n">each</span><span class="p">)</span>
</pre>
<p>We can use some IPython magic to compare the three approaches we have seen so
far in a quick benchmark:</p>
<pre class="code python literal-block">
<span class="n">sizes</span> <span class="o">=</span> <span class="nb">sorted</span><span class="p">([</span><span class="n">k</span> <span class="o">*</span> <span class="mi">10</span> <span class="o">**</span> <span class="n">d</span> <span class="k">for</span> <span class="n">k</span> <span class="ow">in</span> <span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">5</span><span class="p">,</span> <span class="mi">10</span><span class="p">]</span> <span class="k">for</span> <span class="n">d</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">6</span><span class="p">)])</span>
<span class="k">for</span> <span class="n">size</span> <span class="ow">in</span> <span class="n">sizes</span><span class="p">:</span>
<span class="n">x</span><span class="p">,</span> <span class="n">y</span> <span class="o">=</span> <span class="n">generate_dataset</span><span class="p">(</span><span class="n">size</span><span class="p">)</span>
<span class="n">instructions</span> <span class="o">=</span> <span class="p">[</span>
<span class="s2">"simple_linear_regression_sklearn(x, y)"</span><span class="p">,</span>
<span class="s1">'simple_linear_regression_ls(x, y, solver="quadprog")'</span><span class="p">,</span>
<span class="s2">"simple_linear_regression_cramer(x, y)"</span><span class="p">,</span>
<span class="p">]</span>
<span class="k">for</span> <span class="n">instruction</span> <span class="ow">in</span> <span class="n">instructions</span><span class="p">:</span>
<span class="nb">print</span><span class="p">(</span><span class="n">instruction</span><span class="p">)</span> <span class="c1"># helps manual labor to gather the numbers ;)</span>
<span class="n">get_ipython</span><span class="p">()</span><span class="o">.</span><span class="n">magic</span><span class="p">(</span><span class="sa">f</span><span class="s2">"timeit </span><span class="si">{</span><span class="n">instruction</span><span class="si">}</span><span class="s2">"</span><span class="p">)</span>
</pre>
<p>Here is the outcome in a plot (raw data in the image's <tt class="docutils literal">alt</tt> attribute):</p>
<img alt="Benchmark of computation times for all three solutions. Here is the raw data: [(221e-6, 36.2e-6, 5.55e-6), (218e-6, 37.3e-6, 5.56e-6), (217e-6, 36.3e-6, 5.62e-6), (216e-6, 36.5e-6, 5.64e-6), (219e-6, 36.8e-6, 5.74e-6), (219e-6, 37.2e-6, 5.7e-6), (222e-6, 38.3e-6, 6.01e-6), (230e-6, 39.8e-6, 6.32e-6), (241e-6, 42.7e-6, 7.09e-6), (251e-6, 47.6e-6, 8.42e-6), (288e-6, 141e-6, 12.4e-6), (346e-6, 190e-6, 19.5e-6), (452e-6, 283e-6, 52.4e-6), (775e-6, 544e-6, 83.1e-6), (2.32e-3, 987e-6, 138e-6), (3.94e-3, 2.02e-3, 232e-6), (13.2e-3, 5.66e-3, 1.41e-3), (21.8e-3, 11.7e-3, 4.0e-3)]." class="center" src="https://scaron.info/figures/simple-linear-regression-benchmark.png" />
<p>We now have a reasonably efficient solution for simple linear regression over
small- to medium-sized (millions of entries) datasets. All the solutions we
have seen so far have been offline, meaning the dataset was generated then
processed all at once. Let us now turn to the <em>online</em> case, where new
observations <span class="math">\((x_i, y_i)\)</span> come one after the other.</p>
</div>
<div class="section" id="online-simple-linear-regression">
<h2>Online simple linear regression</h2>
<p>The dot products we use to calculate the intercept <span class="math">\(\alpha\)</span> and slope
<span class="math">\(\beta\)</span> are ready for recursive updates. Suppose we already now their
values for <span class="math">\(k\)</span> previous observations. When a new observation
<span class="math">\((x_{k+1}, y_{k+1})\)</span> comes in, we can update them by:</p>
\begin{align}
(\bfone \cdot \bfone)_{k+1} & = (\bfone \cdot \bfone)_k + 1 \\
(\bfone \cdot \bfx)_{k+1} & = (\bfone \cdot \bfx)_k + x_{k+1} \\
(\bfone \cdot \bfy)_{k+1} & = (\bfone \cdot \bfy)_k + y_{k+1} \\
(\bfx \cdot \bfx)_{k+1} & = (\bfx \cdot \bfx)_k + x_{k+1}^2 \\
(\bfx \cdot \bfy)_{k+1} & = (\bfx \cdot \bfy)_k + x_{k+1} y_{k+1}
\end{align}<p>The same logic generalizes to a new batch of observations <span class="math">\((\bfx_{k+1},
\bfy_{k+1})\)</span>:</p>
\begin{align}
(\bfone \cdot \bfone)_{k+1} & = (\bfone \cdot \bfone)_k + \mathrm{dim}(\bfx_{k+1}) \\
(\bfone \cdot \bfx)_{k+1} & = (\bfone \cdot \bfx)_k + (\bfone \cdot \bfx_{k+1}) \\
(\bfone \cdot \bfy)_{k+1} & = (\bfone \cdot \bfy)_k + (\bfone \cdot \bfy_{k+1}) \\
(\bfx \cdot \bfx)_{k+1} & = (\bfx \cdot \bfx)_k + (\bfx_{k+1} \cdot \bfx_{k+1}) \\
(\bfx \cdot \bfy)_{k+1} & = (\bfx \cdot \bfy)_k + (\bfx_{k+1} \cdot \bfy_{k+1})
\end{align}<p>Keep in mind that, although we write them in vector form, dot products by
<span class="math">\(\bfone\)</span> are simply sums. We can wrap these update rules in a Python
class for a simple linear regressor that keeps track of dot products
internally, and outputs <span class="math">\(\alpha\)</span> and <span class="math">\(\beta\)</span> upon new observations:</p>
<pre class="code python literal-block">
<span class="k">class</span> <span class="nc">SimpleLinearRegressor</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="kc">None</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="kc">None</span>
<span class="k">def</span> <span class="nf">update</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">+=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span>
<span class="p">[</span>
<span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>
<span class="n">x</span><span class="o">.</span><span class="n">sum</span><span class="p">(),</span>
<span class="n">y</span><span class="o">.</span><span class="n">sum</span><span class="p">(),</span>
<span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">x</span><span class="p">),</span>
<span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">),</span>
<span class="p">]</span>
<span class="p">)</span>
<span class="n">size</span><span class="p">,</span> <span class="n">sum_x</span><span class="p">,</span> <span class="n">sum_y</span><span class="p">,</span> <span class="n">sum_xx</span><span class="p">,</span> <span class="n">sum_xy</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">dots</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">size</span> <span class="o">*</span> <span class="n">sum_xx</span> <span class="o">-</span> <span class="n">sum_x</span> <span class="o">**</span> <span class="mi">2</span>
<span class="k">if</span> <span class="n">det</span> <span class="o">></span> <span class="mf">1e-10</span><span class="p">:</span> <span class="c1"># determinant may be zero initially</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">sum_xx</span> <span class="o">*</span> <span class="n">sum_y</span> <span class="o">-</span> <span class="n">sum_xy</span> <span class="o">*</span> <span class="n">sum_x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">sum_xy</span> <span class="o">*</span> <span class="n">size</span> <span class="o">-</span> <span class="n">sum_x</span> <span class="o">*</span> <span class="n">sum_y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
</pre>
<p>This solution is equivalent to <a class="reference external" href="https://stackoverflow.com/a/52071283">this answer from StackOverflow</a>. It treats all observations the same,
which is pertinent for non-time series data. But we can also tweak our
derivation so far to the more specific use case of sliding-window regression
over time series data.</p>
</div>
<div class="section" id="sliding-window-simple-linear-regression">
<h2>Sliding-window simple linear regression</h2>
<p>Suppose now that single observations <span class="math">\((x_i, y_i)\)</span> come in succession
every <span class="math">\(\delta t\)</span> timesteps, and we want to estimate the <em>instantaneous</em>
slope <span class="math">\(\beta\)</span> and intercept <span class="math">\(\alpha\)</span> of our recent data. Similarly
to the exponential moving average filter, we can define a <em>forgetting factor</em>
<span class="math">\(\lambda \in (0, 1)\)</span> as:</p>
<div class="math">
\begin{equation*}
\lambda = \exp\left(-\frac{\delta t}{T}\right)
\end{equation*}
</div>
<p>where <span class="math">\(T\)</span> is the time constant that characterizes the span of our sliding
window. Note that, by the <a class="reference external" href="https://en.wikipedia.org/wiki/Nyquist%E2%80%93Shannon_sampling_theorem">Nyquist–Shannon sampling theorem</a>,
<span class="math">\(T\)</span> has to be at least <span class="math">\(2 \delta t\)</span> to avoid aliasing. We can use
this forgetting factor in our objective function as a discount rate applied to
previous observations:</p>
<div class="math">
\begin{equation*}
\mathit{fit}(\alpha, \beta, \bfx_k, \bfy_k) = \sum_{i=0}^{k-1} \lambda^{i} (y_{k-i} - \alpha - \beta x_{k-i})^2
\end{equation*}
</div>
<p>To calculate this exponentially discounted residual sum of squares efficiently,
we can modify the definition of our vectors <span class="math">\(\bfx_k\)</span>, <span class="math">\(\bfy_k\)</span>, and
<span class="math">\(\bfone_k\)</span>, to incorporate the forgetting factor:</p>
\begin{align}
\bfx_{k+1} & = \begin{bmatrix} \sqrt{\lambda} \bfx_k \\ x_{k+1} \end{bmatrix} &
\bfy_{k+1} & = \begin{bmatrix} \sqrt{\lambda} \bfy_k \\ y_{k+1} \end{bmatrix} &
\bfone_{k+1} & = \begin{bmatrix} \sqrt{\lambda} \bfone_k \\ 1 \end{bmatrix}
\end{align}<p>The benefit of this approach is that the squared norms of our new vectors now
correspond to the exponentially discounted residual sum of squares:</p>
\begin{align}
\| \bfy_{k+1} - \alpha \bfone_{k+1} - \beta \bfx_{k+1} \|_2^2
& = (y_{k+1} - \alpha - \beta x_{k+1})^2 + \lambda \| \bfy_{k} - \alpha \bfone_k - \beta \bfx_k \|_2^2 \\
& = \ \ldots \\
& = \sum_{i=0}^{k} \lambda^{i} (y_{k+1-i} - \alpha - \beta x_{k+1-i})^2 \\
& = \mathit{fit}(\alpha, \beta, \bfx_{k+1}, \bfy_{k+1})
\end{align}<p>Granted, we took some liberties with the notation <span class="math">\(\bfone_k\)</span> as it's not
a vector of ones any more ;-) But the expression <span class="math">\((\bfone_k \cdot
\bfv_k)\)</span> keeps the semantic of "summing the coordinates of <span class="math">\(\bfv_k\)</span>",
although is now in an exponentially discounted sense. In terms of dot products,
our update rules translate to:</p>
\begin{align}
(\bfone \cdot \bfone)_{k+1} & = \lambda (\bfone \cdot \bfone)_k + 1 \\
(\bfone \cdot \bfx)_{k+1} & = \lambda (\bfone \cdot \bfx)_k + x_{k+1} \\
(\bfone \cdot \bfy)_{k+1} & = \lambda (\bfone \cdot \bfy)_k + y_{k+1} \\
(\bfx \cdot \bfx)_{k+1} & = \lambda (\bfx \cdot \bfx)_k + x_{k+1}^2 \\
(\bfx \cdot \bfy)_{k+1} & = \lambda (\bfx \cdot \bfy)_k + x_{k+1} y_{k+1}
\end{align}<p>This leads to a regressor class similar to the previous one:</p>
<pre class="code python literal-block">
<span class="k">class</span> <span class="nc">SlidingWindowLinearRegressor</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">dt</span><span class="p">,</span> <span class="n">time_constant</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="kc">None</span>
<span class="bp">self</span><span class="o">.</span><span class="n">forgetting_factor</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">exp</span><span class="p">(</span><span class="o">-</span><span class="n">dt</span> <span class="o">/</span> <span class="n">time_constant</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="kc">None</span>
<span class="k">def</span> <span class="nf">update</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">:</span> <span class="nb">float</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="nb">float</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">*=</span> <span class="bp">self</span><span class="o">.</span><span class="n">forgetting_factor</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">+=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">1</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">x</span> <span class="o">*</span> <span class="n">x</span><span class="p">,</span> <span class="n">x</span> <span class="o">*</span> <span class="n">y</span><span class="p">])</span>
<span class="n">dot_11</span><span class="p">,</span> <span class="n">dot_1x</span><span class="p">,</span> <span class="n">dot_1y</span><span class="p">,</span> <span class="n">dot_xx</span><span class="p">,</span> <span class="n">dot_xy</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">dots</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">dot_11</span> <span class="o">*</span> <span class="n">dot_xx</span> <span class="o">-</span> <span class="n">dot_1x</span> <span class="o">**</span> <span class="mi">2</span>
<span class="k">if</span> <span class="n">det</span> <span class="o">></span> <span class="mf">1e-10</span><span class="p">:</span> <span class="c1"># determinant is zero initially</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_xx</span> <span class="o">*</span> <span class="n">dot_1y</span> <span class="o">-</span> <span class="n">dot_xy</span> <span class="o">*</span> <span class="n">dot_1x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_xy</span> <span class="o">*</span> <span class="n">dot_11</span> <span class="o">-</span> <span class="n">dot_1x</span> <span class="o">*</span> <span class="n">dot_1y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
</pre>
<p>Let's evaluate its performance on synthetic data:</p>
<pre class="code python literal-block">
<span class="n">dt</span> <span class="o">=</span> <span class="mf">1e-3</span> <span class="c1"># [s]</span>
<span class="n">T</span> <span class="o">=</span> <span class="mf">5e-2</span> <span class="c1"># [s]</span>
<span class="n">sliding_window_regressor</span> <span class="o">=</span> <span class="n">SlidingWindowLinearRegressor</span><span class="p">(</span><span class="n">dt</span><span class="p">,</span> <span class="n">T</span><span class="p">)</span>
<span class="k">for</span> <span class="n">t</span> <span class="ow">in</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">100.</span><span class="p">,</span> <span class="n">dt</span><span class="p">):</span>
<span class="n">alpha</span> <span class="o">=</span> <span class="mf">1.5</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="mf">0.1</span> <span class="o">*</span> <span class="n">t</span><span class="p">)</span>
<span class="n">beta</span> <span class="o">=</span> <span class="mf">3.15</span> <span class="o">+</span> <span class="mf">0.5</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="mf">1.1</span> <span class="o">*</span> <span class="n">t</span><span class="p">)</span>
<span class="n">x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">t</span><span class="p">)</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mf">0.05</span><span class="p">)</span>
<span class="n">y</span> <span class="o">=</span> <span class="n">alpha</span> <span class="o">+</span> <span class="n">beta</span> <span class="o">*</span> <span class="n">x</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mf">0.1</span><span class="p">)</span>
<span class="n">sliding_window_regressor</span><span class="o">.</span><span class="n">update</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
</pre>
<p>This small test results in the following observations and estimates:</p>
<img alt="Estimates during sliding-window simple linear regression." class="center" src="https://scaron.info/figures/sliding-window-simple-linear-regression.png" />
<p>Contrary to regular simple linear regression, our filter now has a
time-constant parameter <span class="math">\(T\)</span> that defines the length of our sliding
window. Its ideal value depends on our data. In this example, it would depend
on all the inline parameters <tt class="docutils literal">[1.5, 0.1, 3.15, 0.5, 1.1, 0.05, 0.1]</tt> we use
to produce <span class="math">\(\alpha(t), \beta(t)\)</span> and the white noise on <span class="math">\(x(t),
y(t)\)</span>. Although, mainly, it will depend on the two constants <tt class="docutils literal">[0.1, 1.1]</tt>
that define how fast <span class="math">\(\alpha(t)\)</span> and <span class="math">\(\beta(t)\)</span> change. If we
choose it too small, our estimates will be snappy but noisy. For example with
<tt class="docutils literal">T = <span class="pre">5e-3</span></tt> s:</p>
<img alt="Estimates during sliding-window linear regression with a small time constant." class="center" src="https://scaron.info/figures/sliding-window-simple-linear-regression-small-time-constant.png" />
<p>If we choose it too large, our estimates will be smooth but delayed. For
example with <tt class="docutils literal">T = 5.0</tt> s:</p>
<img alt="Estimates during sliding-window linear regression with a large time constant." class="center" src="https://scaron.info/figures/sliding-window-simple-linear-regression-large-time-constant.png" />
<p>In this instance, the filter may be acceptable for <span class="math">\(\alpha(t)\)</span> but it
fails to capture the time variations of <span class="math">\(\beta(t)\)</span> (if we crank <span class="math">\(T\)</span>
up further we can even average them out completely). How to tune it from there
depends on the desired behavior.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Sliding window linear regression combines statistics and signal processing. Its
parameter <span class="math">\(T\)</span> is the same time constant as in a <a class="reference external" href="https://en.wikipedia.org/wiki/Low-pass_filter">low-pass filter</a>, and it can be selected by
specifying a <a class="reference external" href="https://en.wikipedia.org/wiki/Frequency_response">frequency response</a>, which does formally what
we commented on informally in our example when selecting <span class="math">\(T\)</span> to "capture"
or not the time-varying dynamics of <span class="math">\(\alpha(t)\)</span> and <span class="math">\(\beta(t)\)</span>. The
overall algorithm itself is a simplification of the <a class="reference external" href="https://en.wikipedia.org/wiki/Recursive_least_squares_filter">recursive least squares
filter</a> to our
use case.</p>
<p>When deriving our system of linear inequalities for offline regression, the
condition we used that the gradient vanishes at the global minimum of the
objective function is a particular case of <a class="reference external" href="https://en.wikipedia.org/wiki/Karush%E2%80%93Kuhn%E2%80%93Tucker_conditions">Karush–Kuhn–Tucker (KKT) conditions</a>
applied to our unconstrained problem. KKT conditions provide the general
solution when the problem also has inequality or equality constraints.</p>
<div class="section" id="bonus-simple-linear-regression-with-jax">
<h3>Bonus: simple linear regression with JAX</h3>
<p>For fun, we can look at the performance of the offline algorithm using <a class="reference external" href="https://github.com/google/jax">JAX</a>'s just-in-time compiler. JAX also gives us a
convenient way to try running the function on the CPU or GPU, so let's try
both:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">jax</span>
<span class="kn">import</span> <span class="nn">jax.numpy</span> <span class="k">as</span> <span class="nn">jnp</span>
<span class="k">def</span> <span class="nf">simple_linear_regression_cramer_jax</span><span class="p">(</span><span class="n">x</span><span class="p">:</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">):</span>
<span class="n">dot_1_1</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="c1"># == jnp.dot(ones, ones)</span>
<span class="n">dot_1_x</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == jnp.dot(x, ones)</span>
<span class="n">dot_1_y</span> <span class="o">=</span> <span class="n">y</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == jnp.dot(y, ones)</span>
<span class="n">dot_x_x</span> <span class="o">=</span> <span class="n">jnp</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">x</span><span class="p">)</span>
<span class="n">dot_x_y</span> <span class="o">=</span> <span class="n">jnp</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">**</span> <span class="mi">2</span> <span class="c1"># not checking det == 0</span>
<span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_y</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_x_y</span> <span class="o">*</span> <span class="n">dot_1_x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_y</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">*</span> <span class="n">dot_1_y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="k">return</span> <span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span>
<span class="n">cpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">devices</span><span class="p">(</span><span class="s2">"cpu"</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>
<span class="n">gpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">devices</span><span class="p">(</span><span class="s2">"gpu"</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>
<span class="n">cramer_jax_cpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">jit</span><span class="p">(</span><span class="n">simple_linear_regression_cramer_jax</span><span class="p">,</span> <span class="n">device</span><span class="o">=</span><span class="n">cpu</span><span class="p">)</span>
<span class="n">cramer_jax_gpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">jit</span><span class="p">(</span><span class="n">simple_linear_regression_cramer_jax</span><span class="p">,</span> <span class="n">device</span><span class="o">=</span><span class="n">gpu</span><span class="p">)</span>
<span class="k">for</span> <span class="n">size</span> <span class="ow">in</span> <span class="n">sizes</span><span class="p">:</span>
<span class="n">x</span><span class="p">,</span> <span class="n">y</span> <span class="o">=</span> <span class="n">generate_dataset</span><span class="p">(</span><span class="n">size</span><span class="p">)</span>
<span class="n">jx</span><span class="p">,</span> <span class="n">jy</span> <span class="o">=</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">x</span><span class="p">),</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">y</span><span class="p">)</span>
<span class="n">instructions</span> <span class="o">=</span> <span class="p">[</span><span class="s2">"cramer_jax_cpu(jx, jy)"</span><span class="p">,</span> <span class="s2">"cramer_jax_gpu(jx, jy)"</span><span class="p">]</span>
<span class="k">for</span> <span class="n">instruction</span> <span class="ow">in</span> <span class="n">instructions</span><span class="p">:</span>
<span class="n">get_ipython</span><span class="p">()</span><span class="o">.</span><span class="n">magic</span><span class="p">(</span><span class="sa">f</span><span class="s2">"timeit </span><span class="si">{</span><span class="n">instruction</span><span class="si">}</span><span class="s2">"</span><span class="p">)</span>
</pre>
<p>Here are the results of this benchmark on my machine (CPU: Intel® Core™
i7-6700K at 4.00 GHz, GPU: NVIDIA GeForce GTX 1070 with CUDA 10.1), combined
with the ones from the previous benchmark above:</p>
<img alt="Benchmark of computation times for all three solutions. Here is the raw data: [(221e-6, 36.2e-6, 4.86e-6, 36.9e-6, 31.0e-6), (218e-6, 37.3e-6, 4.86e-6, 33.7e-6, 30.0e-6), (217e-6, 36.3e-6, 4.92e-6, 36.6e-6, 32.4e-6), (216e-6, 36.5e-6, 4.86e-6, 34.6e-6, 30.9e-6), (219e-6, 36.8e-6, 4.96e-6, 35.0e-6, 33.0e-6), (219e-6, 37.2e-6, 4.97e-6, 35.3e-6, 33.2e-6), (222e-6, 38.3e-6, 5.11e-6, 36.2e-6, 32.0e-6), (230e-6, 39.8e-6, 5.48e-6, 39.7e-6, 33.8e-6), (241e-6, 42.7e-6, 6.12e-6, 42.0e-6, 33.7e-6), (251e-6, 47.6e-6, 7.10e-6, 43.2e-6, 35.7e-6), (288e-6, 141e-6, 10.1e-6, 74.8e-6, 34.4e-6), (346e-6, 190e-6, 14.0e-6, 80.3e-6, 33.2e-6), (452e-6, 283e-6, 46.9e-6, 126e-6, 32.9e-6), (775e-6, 544e-6, 80.1e-6, 257e-6, 32e-6), (2.32e-3, 987e-6, 138e-6, 470e-6, 31.4e-6), (3.94e-3, 2.02e-3, 269e-6, 895e-6, 32.9e-6), (13.2e-3, 5.66e-3, 772e-6, 2.13e-3, 36.4e-6), (21.8e-3, 11.7e-3, 2.53e-3, 4.75e-3, 56.6e-6)]." class="center" src="https://scaron.info/figures/simple-linear-regression-benchmark-jax.png" />
<p>Nothing beats simple NumPy on small datasets, and the turning point when
transferring to GPU becomes attractive seems to be around 20,000 observations.</p>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Joint torques and Jacobian transpose2021-09-29T00:00:00+02:002021-09-29T00:00:00+02:00Stéphane Carontag:scaron.info,2021-09-29:/blog/joint-torques-and-jacobian-transpose.html<p>Some concepts, when you learn them, seem to connect with what you already know
and build new objects that make sense, that is, with the approval of your
reason. This is the ideal scenario. It is likely that things flow nicely
because the intellectual path has been "polished" by previous …</p><p>Some concepts, when you learn them, seem to connect with what you already know
and build new objects that make sense, that is, with the approval of your
reason. This is the ideal scenario. It is likely that things flow nicely
because the intellectual path has been "polished" by previous travelers.
(Sometimes the fruit of centuries of effort and heated debates, such as the
conciliation of Euclidean and analytical geometry in mathematics.) And then,
there are the unpolished ones, those ideas and techniques that are still rough
around the edges. Papers and code seem to take for granted, so most likely they
work, yet there's no consensual explanation easy to find in the literature.</p>
<p>At the time I was a graduate student, the following equation was one of these
rough edges for me. Every once in a while I would run into a paper on humanoid
robots that would compute the joint torques cause by contact forces using the
transpose of the contact Jacobian:</p>
<div class="math">
\begin{equation*}
\bftau_c = \bfJ_c(\bfq)^\top \bff
\end{equation*}
</div>
<p>with <span class="math">\(\bftau_c\)</span> the vector of joint torques caused by contact forces,
<span class="math">\(\bfJ_c\)</span> the Jacobian of the contact constraint (for instance foot
contacts with the ground for a biped), <span class="math">\(\bfq\)</span> the configuration vector of
joint angles and floating-base coordinates, and <span class="math">\(\bff\)</span> the vector of
contact forces applied by the environment onto the robot. Papers would mention
it derives from the <a class="reference external" href="https://scaron.info/robot-locomotion/principle-of-virtual-work.html">principle of virtual work</a>, sometimes with no reference,
sometimes with a reference to a textbook on fixed-base manipulators (humanoids
are floating-base robots).</p>
<p>This equation is correct, yet I've been on the lookout for years before finding
a proof that did not cost me a leap of faith. (Are the assumptions clear? Is
every step of the demonstration correct?) Part of this is due to the evolution
of the literature from manipulators to floating-base robots: while the
confusion was not a big deal with the former, with the latter we should bear in
mind that <span class="math">\(\bftau_c\)</span> is <em>not</em> the vector of actuated joint torques of the
robot, but only the <em>contribution</em> to these torques caused by contact forces.
As of today, the clearest demonstration I could find is the <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html#principle-of-least-constraint">derivation from
the principle of least constraint</a>.
It leads us to the constrained equation of motion, where we can see other
contributions from gravity, the centripetal and Coriolis effects:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bfJ_c(q)^\top \bff
\end{equation*}
</div>
<p>Enough with the history! Here is a bonus question to see if you understand
these concepts better from today's literature: did you know that the vector of
joint torques caused by gravity can in fact be written as follows?</p>
<div class="math">
\begin{equation*}
\bftau_g(\bfq) = m \bfJ_{\mathrm{CoM}}(\bfq)^\top \bfg
\end{equation*}
</div>
<p>Here <span class="math">\(\bfg\)</span> is the three-dimensional acceleration due to gravity,
<span class="math">\(m\)</span> is the total mass of the robot and <span class="math">\(\bfJ_{\mathrm{CoM}}\)</span> is the
Jacobian of the center-of-mass position. Hint: the answer can be derived
directly from the principle of least action. It is also written down somewhere
linked from this website ;-)</p>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Principle of virtual work2021-09-28T00:00:00+02:002021-09-28T00:00:00+02:00Stéphane Carontag:scaron.info,2021-09-28:/robot-locomotion/principle-of-virtual-work.html<p>Although derivations based on <a class="reference external" href="https://en.wikipedia.org/wiki/Gauss%27s_principle_of_least_constraint">Gauss's principle of least constraint</a> are
now more commonplace (thank goodness!), the principle of virtual work is an
equivalent approach that we can still find in the literature. Also called
<a class="reference external" href="https://en.wikipedia.org/wiki/D%27Alembert%27s_principle">d'Alembert's principle</a>, it relates
constrained motions to their constraint forces.</p>
<div class="section" id="virtual-displacements">
<h2>Virtual displacements</h2>
<p>Consider the <a class="reference external" href="https://en.wikipedia.org/wiki/Holonomic_constraints">holonomic</a>
constraint …</p></div><p>Although derivations based on <a class="reference external" href="https://en.wikipedia.org/wiki/Gauss%27s_principle_of_least_constraint">Gauss's principle of least constraint</a> are
now more commonplace (thank goodness!), the principle of virtual work is an
equivalent approach that we can still find in the literature. Also called
<a class="reference external" href="https://en.wikipedia.org/wiki/D%27Alembert%27s_principle">d'Alembert's principle</a>, it relates
constrained motions to their constraint forces.</p>
<div class="section" id="virtual-displacements">
<h2>Virtual displacements</h2>
<p>Consider the <a class="reference external" href="https://en.wikipedia.org/wiki/Holonomic_constraints">holonomic</a>
constraint <span class="math">\(\bfc(\bfq) = \bfzero\)</span>, which stacks a set of equations that
our system must follow troughout its motions. It could be for instance a biped
keeping its feet in contact with the ground, or a humanoid pushing against a
wall. The corresponding constraint Jacobian is:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \qd = \frac{\partial \bfc}{\partial \bfq}(\bfq) \qd = \bfzero
\end{equation*}
</div>
<p>Let us define a <em>virtual displacement</em> <span class="math">\(\Delta \bfq\)</span> as any vector in the
nullspace of the constraint Jacobian:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \Delta \bfq = \bfzero
\end{equation*}
</div>
<p>Note that a virtual displacement <span class="math">\(\Delta \bfq\)</span> is not the same thing as
an actual displacement of the robot. At the acceleration level, a virtual
displacement <span class="math">\((\Delta \bfq, \Delta \qd)\)</span> only satisfies the constraint:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \Delta \qd + \Delta \bfq^\top \bfH_c(\bfq) \Delta \bfq = \bfzero
\end{equation*}
</div>
<p>Whereas an actual displacement <span class="math">\((\qd, \qdd)\)</span> satisfies both the
constraint and the equation of motion:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^T \bftau + \bftau_g(\bfq) + \bfJ_c(\bfq)^\top \bff \\
\bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd & = \bfzero
\end{align}<p>This distinction shows that virtual displacements are more general than actual
displacements. It is important to keep in mind for what follows.</p>
</div>
<div class="section" id="virtual-work">
<h2>Virtual work</h2>
<p>The principle of virtual work applies to <a class="reference external" href="https://en.wikipedia.org/wiki/Udwadia%E2%80%93Kalaba_formulation#Non-ideal_constraints">ideal</a>
constraints, <em>i.e.</em>, constraints whose virtual displacements are reversible. In
robotics, this basically means all constraints except those that involve
friction. The principle states that constraint forces, <em>i.e.</em>, the joint
torques <span class="math">\(\bftau_c\)</span> that maintain the constraint <span class="math">\(\bfc(\bfq) =
\bfzero\)</span> along a virtual displacement <span class="math">\(\Delta \bfq\)</span>, do not produce any
virtual work. That is:</p>
<div class="math">
\begin{equation*}
\bftau_c^\top \Delta \bfq = \bfzero
\end{equation*}
</div>
<p>Since <span class="math">\(\Delta \bfq\)</span> can be any virtual displacement in the nullspace
<span class="math">\({\cal N}(\bfJ_c)\)</span> of the constraint Jacobian, this means
<span class="math">\(\bftau_c\)</span> belongs to the orthogonal to this nullspace, which is also
<a class="reference external" href="https://math.stackexchange.com/a/2568906">equal to</a> the range of the
Jacobian transpose:</p>
<div class="math">
\begin{equation*}
\bftau_c \in {\cal N}(\bfJ_c)^\bot = {\cal R}(\bfJ_c^\top)
\end{equation*}
</div>
<p>As a consequence, there exists a vector <span class="math">\(\bff\)</span> of dual vectors such that
<span class="math">\(\bftau_c = \bfJ_c^\top \bff\)</span>. We <em>define</em> constraint forces as these
dual vectors. To continue our example, if the constraint is that the biped's
feet stay in contact with the ground, then <span class="math">\(\bff\)</span> are the ground contact
forces; if our constraint is that the humanoid's hands push against the wall,
then <span class="math">\(\bff\)</span> are the wall contact forces.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The alternative <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html#principle-of-least-constraint">Lagrangian derivation from the principle of least constraint</a>
characterizes forces further than what we did above. As a matter of fact, the
principle of virtual work can be seen as the critical condition associated with
the principle of least constraint: for the latter, motions minimize an
objective function (deviation from unconstrained motion), for the former,
motions are in the nullspace of the gradient of this objective function.
Informally, since the critical point of a strictly <a class="reference external" href="https://en.wikipedia.org/wiki/Convex_function">convex function</a> is its global optimum, the
two principles are equivalent.</p>
<p>The principle of virtual work is not always applicable: it only works with
ideal constraints where forces do not work along virtual displacements. This is
in particular not true for sliding friction where forces are opposite
end-effector velocities. In that case, the most straightforward solution is to
use a model of the friction force and map it through the Jacobian transpose in
the constrained equations of motion. For a proof of why this is valid, see for
instance equation (13) in Wieber's <a class="reference external" href="https://hal.inria.fr/inria-00390428/document">comments on the structure of the dynamics
of articulated motion</a>.
Alternatively, the <a class="reference external" href="https://en.wikipedia.org/wiki/Udwadia%E2%80%93Kalaba_formulation">Udwadia-Kalaba formulation</a> provides
an equivalent generalization of the principle of virtual work to <em>non-ideal</em>
constraints. It also derives from the principle of least constraint, yet it
involves pseudoinverses which are not as numerically stable as working directly
with the constrained equation of motion.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Forward dynamics2021-06-03T00:00:00+02:002021-06-03T00:00:00+02:00Stéphane Carontag:scaron.info,2021-06-03:/robot-locomotion/forward-dynamics.html<p>Forward dynamics (FD) refers to the computation of motions from forces. Given
the configuration <span class="math">\(\bfq\)</span>, generalized velocity <span class="math">\(\qd\)</span>, joint torques
<span class="math">\(\bftau\)</span> and contact forces <span class="math">\(\bff\)</span> acting on our robot, forward
dynamics computes the joint accelerations <span class="math">\(\qdd\)</span> such that the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl …</div><script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>Forward dynamics (FD) refers to the computation of motions from forces. Given
the configuration <span class="math">\(\bfq\)</span>, generalized velocity <span class="math">\(\qd\)</span>, joint torques
<span class="math">\(\bftau\)</span> and contact forces <span class="math">\(\bff\)</span> acting on our robot, forward
dynamics computes the joint accelerations <span class="math">\(\qdd\)</span> such that the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = & \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \\
\bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = & \boldsymbol{0}
\end{array}
\end{equation*}
</div>
<p>Mathematically, we can write forward dynamics as a function <span class="math">\(\mathrm{FD}\)</span>
such that:</p>
<div class="math">
\begin{equation*}
\qdd = \mathrm{FD}(\bfq, \qd, \bftau, \bff)
\end{equation*}
</div>
<p>Note that this function implicitly depends on our robot model. Different robots
will have <em>e.g.</em> different inertias, thus different inertia matrices
<span class="math">\(\bfM(\bfq)\)</span>, thus different <span class="math">\(\mathrm{FD}\)</span> functions in a given
configuration <span class="math">\(\bfq\)</span>.</p>
<div class="section" id="algorithms">
<h2>Algorithms</h2>
<p>There are two common ways to compute forward dynamics. The first one relies on
the articulated body algorithm:</p>
<div class="math">
\begin{equation*}
\qdd = \mathrm{ABA}(\bfq, \qd, \bftau, \bff^\mathit{ext})
\end{equation*}
</div>
<p>The second one relies on the composite rigid body algorithm:</p>
<div class="math">
\begin{equation*}
\qdd = \bfM^{-1} (\bftau - \bftau_0)
\end{equation*}
</div>
<p>where <span class="math">\(\bftau_0 = \mathrm{RNEA}(\bfq, \qd, \bfzero, \bff^\mathit{ext})\)</span>
is computed using the <a class="reference external" href="https://scaron.info/robot-locomotion/recursive-newton-euler-algorithm.html">recursive Newton-Euler algorithm</a> and <span class="math">\(\bfM =
\mathrm{CRBA}(\bfq)\)</span> relies on the composite rigid body algorithm.</p>
<div class="section" id="articulated-body-algorithm">
<h3>Articulated body algorithm</h3>
<p>The articulated body algorithm (ABA) computes the <em>unconstrained</em> forward dynamics:</p>
<div class="math">
\begin{equation*}
\qdd = \mathrm{ABA}(\bfq, \qd, \bftau, \bff)
\end{equation*}
</div>
<p>Its output <span class="math">\(\qdd\)</span> satisfies the equation of motion <span class="math">\(\bfM(\bfq) \qdd
+ \ldots = \ldots + \bfJ^\top \bff\)</span>, but it doesn't take into account the
contact constraint <span class="math">\(\bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd =
\boldsymbol{0}\)</span>.</p>
<p>... this article is a stub, ping me if I don't come back to it ;) ...</p>
</div>
<div class="section" id="composite-rigid-body-algorithm">
<h3>Composite rigid body algorithm</h3>
<p>The composite rigid body algorithm (CRBA) computes the joint inertia matrix
<span class="math">\(\bfM(\bfq)\)</span> from the joint configuration <span class="math">\(\bfq\)</span>.</p>
<p>... this article is a stub, ping me if I don't come back to it ;) ...</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Both ABA and CRBA are described in Featherstone's <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid body dynamics
algorithms</a>, a seminal book that
has been implemented in rigid body dynamics libraries, such as <a class="reference external" href="https://stack-of-tasks.github.io/pinocchio/">Pinocchio</a> or <a class="reference external" href="https://rbdl.github.io/">RBDL</a>, as well as dynamic simulators, such as <a class="reference external" href="https://pybullet.org/">Bullet</a> or <a class="reference external" href="https://dartsim.github.io/">Dart</a>. ABA is the
main algorithm in both Bullet and Dart.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Kinematics jargon2021-05-01T00:00:00+02:002021-06-03T00:00:00+02:00Stéphane Carontag:scaron.info,2021-05-01:/robot-locomotion/kinematics-jargon.html<p>Like any field with a big body of knowledge, robotics has its own jargon that is ever evolving and takes practice to get used to. Also, some concepts are close to each other, yet different, and they tend to be more easily confused. The goal of this page is to …</p><p>Like any field with a big body of knowledge, robotics has its own jargon that is ever evolving and takes practice to get used to. Also, some concepts are close to each other, yet different, and they tend to be more easily confused. The goal of this page is to act like a cheat sheet, stressing the differences between concepts and giving you the pointers to dig deeper.</p>
<div class="section" id="lexicon">
<h2>Lexicon</h2>
<p>Let us first look at the most important concepts.</p>
<dl class="docutils">
<dt>Configuration:</dt>
<dd>A vector <span class="math">\(\bfq\)</span> of the <a class="reference external" href="https://en.wikipedia.org/wiki/Configuration_space_(physics)">configuration space</a>, that is, a
vector of generalized coordinates (a.k.a. joint coordinates) that
characterizes the positions of all points in the multi-body model of our
robot.</dd>
<dt>Orientation:</dt>
<dd>The angular coordinates of a rigid body in space. Not exactly the same
thing as a rotation, although there is an isomorphism between rotations and
orientations (similarly to the isomorphism between positions and
translations). Rotation matrices are one way to represent orientation,
others are listed below.</dd>
<dt>Pose:</dt>
<dd>Both position and orientation of a rigid body. Not exactly the same thing
as a transformation, although there is an isomorphism between poses and
their transformation group.</dd>
<dt>Position:</dt>
<dd>The linear coordinates <span class="math">\(\bfp \in \mathbb{R}^3\)</span> of a body in space.
Not exactly the same thing as a translation, although there is an
isomorphism between the Euclidean space <span class="math">\(\mathbb{R}^3\)</span> and its
<a class="reference external" href="https://en.wikipedia.org/wiki/Translation_(geometry)#As_a_group">translation group</a>.</dd>
<dt>Rotation:</dt>
<dd>A transformation <span class="math">\(R \in SO(3)\)</span> that acts vectors <span class="math">\(\bfe \in
\mathbb{R}^3\)</span> and preserves their lengths and the angles between them. The
rotation matrix <span class="math">\(\bfR_{AB}\)</span> represents equivalently the orientation
of frame <span class="math">\(B\)</span> in frame <span class="math">\(A\)</span> or the rotation <span class="math">\(R : {}_B \bfe
\mapsto {}_A \bfe\)</span>.</dd>
<dt>Translation:</dt>
<dd>A transformation <span class="math">\(t : \mathbb{R}^3 \to \mathbb{R}^3\)</span> that maps the
position <span class="math">\(\bfp\)</span> of a rigid body to a new position <span class="math">\(t(\bfp)\)</span>.</dd>
<dt>Transform:</dt>
<dd>Shorthand for a <em>Direct isometry</em> <span class="math">\(T \in SE(3)\)</span>. Due to the bijection
between <em>Poses</em> and <span class="math">\(SE(3)\)</span>, it is common to read "the transform of a
rigid body", although more precise expressions are "the pose of a rigid
body" and "the transform from frame A to frame B".</dd>
</dl>
<div class="section" id="full-package">
<h3>Full package</h3>
<p>Here comes a wider list of expressions you will find in the literature. Follow the links or scroll down for extra info on each:</p>
<dl class="docutils">
<dt>Affine transformation:</dt>
<dd>A geometric transformation <span class="math">\(A : \mathbb{R}^3 \to \mathbb{R}^3\)</span> that
preserves lines and parallelism. Direct isometries <span class="math">\(SE(3)\)</span> used to
represent the <em>Pose</em> of a rigid body are, in particular, affine
transformations. Affine transformations are conveniently represented by
<span class="math">\(4 \times 4\)</span> matrices of <em>Homogenous coordinates</em>.</dd>
<dt>Attitude:</dt>
<dd>Synonym for orientation, this word is more common in aeronautics such as in
<a class="reference external" href="https://en.wikipedia.org/wiki/Attitude_and_heading_reference_system">attitude and heading reference system</a>.</dd>
<dt>Body:</dt>
<dd>Shorthand for <em>Rigid body</em>.</dd>
<dt>Direct isometry:</dt>
<dd>A geometric transformation <span class="math">\(T \in SE(3)\)</span> that preserves distances and
handedness. Direct isometries form a mathematical group <span class="math">\(SE(3)\)</span>
called the special Euclidean group. They are the most commonly used
representation for the <em>Pose</em> of rigid bodies.</dd>
<dt>Displacement:</dt>
<dd>Synonym for <em>Direct isometry</em>.</dd>
<dt>Heading:</dt>
<dd>A vector that hints at the direction of motion, especially for systems such
as cars or humands that have a preferred direction of motion (forward). For
these two, a heading vector can be any vector in the intersection between
the <a class="reference external" href="https://en.wikipedia.org/wiki/Sagittal_plane">sagittal plane</a> and
<a class="reference external" href="https://en.wikipedia.org/wiki/Median_plane">median plane</a>.</dd>
<dt>Homogeneous coordinates:</dt>
<dd>Convenient way to <a class="reference external" href="https://en.wikipedia.org/wiki/Transformation_matrix#Affine_transformations">represent</a>
an <em>Affine transformation</em> as a <span class="math">\(4 \times 4\)</span> matrix.</dd>
<dt>Joint:</dt>
<dd>Connection between two rigid bodies that constrains their relative degree
of freedom. The most common in robotics is the <a class="reference external" href="https://en.wikipedia.org/wiki/Revolute_joint">revolute joint</a> actuated by a servomotor.</dd>
<dt>Joint coordinates:</dt>
<dd>Synonym for <em>Configuration</em>. Sometimes used to distinguish actuated
coordinates (<em>e.g.</em> a robot's joints) from unactuated degrees of freedom
(<em>e.g.</em> the floating base of a legged robot).</dd>
<dt>Link:</dt>
<dd>Rigid body of a robot, connected to at least one <em>Joint</em>.</dd>
<dt>Transformation:</dt>
<dd>In general, this expression may refer to any <a class="reference external" href="https://en.wikipedia.org/wiki/Geometric_transformation">geometric transformation</a>, for example an
<em>Affine transformation</em>. In robotics, this expression is generally used as
a shorthand for a <em>Direct isometry</em> <span class="math">\(T \in SE(3)\)</span>.</dd>
</dl>
</div>
</div>
<div class="section" id="representing-orientation">
<h2>Representing orientation</h2>
<p>Because all points on a rigid body hold, by definition, the same relative positions between each other, we can describe them all with just six numbers: the <em>position</em> of any one of them, and the three degrees of freedom of the body's <em>orientation</em>. There are several ways to represent orientations. Three common ones are:</p>
<ul class="simple">
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Euler_angles">Euler angles</a>: only three angles, but tricky to work with because of the <a class="reference external" href="https://en.wikipedia.org/wiki/Gimbal_lock">gimbal lock</a> and the many different conventions (intrinsic or extrinsic rotations? upward or downward z-axis?)</li>
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Versor">Unit quaternions</a>: four numbers, better behaved than Euler angles with only one convention to figure out (are the coordinates ordered as <span class="math">\([w\ x\ y\ z]\)</span> or <span class="math">\([x\ y\ z\ w]\)</span>?).</li>
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Rotation_matrix">Rotation matrices</a>: nine numbers, no confusion regarding conventions, applies to position coordinates by direct matrix product.</li>
</ul>
<p>It is best practice to <em>avoid</em> using Euler angles in software (colorful experimental and bug-hunting stories abound). In legged robotics, the most common convention is roll-pitch-yaw convention with upward z-axis. It corresponds to intrinsic Z-Y-X angles, or equivalently extrinsic x-y-z angles: first roll around the fixed x-axis of the parent frame, then pitch around the fixed y-axis of the parent frame, finally yaw around the fixed z-axis of the parent frame.</p>
<p>To go further, check out <em>e.g.</em> <a class="reference external" href="https://www.astro.rug.nl/software/kapteyn-beta/_downloads/attitude.pdf">Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors</a> by James Diebel.</p>
</div>
<div class="section" id="representing-transformations">
<h2>Representing transformations</h2>
<p>The minimum number of coordinates necessary to describe the displacement of a rigid body is six, yet 6D displacement vectors include a flavor of Euler angles to represent orientations and the slew of problems they carry along.</p>
<ul class="simple">
<li>Pose vectors: 7D vectors that combine a quaternion with a position vector. This is for instance the representation used in <a class="reference external" href="https://github.com/rdiankov/openrave">OpenRAVE</a>.</li>
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Transformation_matrix">Transformation matrices</a>: <span class="math">\(4 \times 4\)</span> matrices that combine a rotation matrix with a position vector.</li>
<li>Plücker matrices: <span class="math">\(6 \times 6\)</span> that act on the <a class="reference external" href="https://scaron.info/robot-locomotion/screw-theory.html">screws</a> that describe the motions and forces of rigid bodies. This is for instance the representation used in <a class="reference external" href="https://jrl-umi3218.github.io/mc_rtc/">mc_rtc</a>.</li>
</ul>
<p>To go further, check out <em>e.g.</em> <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid Body Dynamics Algorithms</a> by Roy Featherstone.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Humanoid Control Under Interchangeable Fixed and Sliding Unilateral Contacts2021-04-08T00:00:00+02:002021-04-08T00:00:00+02:00Stéphane Carontag:scaron.info,2021-04-08:/publications/ral-2021.html<p class="authors"><strong>Saeid Samadi</strong>, <strong>Julien Roux</strong>, <strong>Arnaud Tanguy</strong>, <strong>Stéphane Caron</strong> and
<strong>Abderrahmane Kheddar</strong>. Robotics and Automation Letters. April 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this letter, we propose a whole-body control strategy for humanoid robots in
multi-contact settings that enables switching between fixed and sliding
contacts under active balance. We compute, in real-time, a safe …</p></div><p class="authors"><strong>Saeid Samadi</strong>, <strong>Julien Roux</strong>, <strong>Arnaud Tanguy</strong>, <strong>Stéphane Caron</strong> and
<strong>Abderrahmane Kheddar</strong>. Robotics and Automation Letters. April 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this letter, we propose a whole-body control strategy for humanoid robots in
multi-contact settings that enables switching between fixed and sliding
contacts under active balance. We compute, in real-time, a safe center-of-mass
position and wrench distribution of the contact points based on the Chebyshev
center. Our solution is formulated as a quadratic programming problem without a
priori computation of balance regions. We assess our approach with experiments
highlighting switches between fixed and sliding contact modes in multi-contact
configurations. A humanoid robot demonstrates such contact interchanges from
fully-fixed to multi-sliding and also shuffling of the foot. The scenarios
illustrate the performance of our control scheme in achieving the desired
forces, CoM position attractor, and planned trajectories while actively
maintaining balance.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-03158364/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/LRA.2021.3066965">10.1109/LRA.2021.3066965</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/SaeidSamadi/Multi-sliding-Contacts">Source code of the controller</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">samadi2021ral</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Samadi, Saeid and Roux, Julien and Tanguy, Arnaud and Caron, Stéphane and Kheddar, Abderrahmane}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Robotics and Automation Letters}</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Humanoid Control Under Interchangeable Fixed and Sliding Unilateral Contacts}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2021}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">apr</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{6}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{2}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{4032-4039}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/LRA.2021.3066965}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Conversion from least squares to quadratic programming2020-11-08T00:00:00+01:002020-11-08T00:00:00+01:00Stéphane Carontag:scaron.info,2020-11-08:/blog/conversion-from-least-squares-to-quadratic-programming.html<p>Casting a linear least squares to a quadratic program is a common question for
newcomers, who see this operation routinely mentioned or taken for granted in
writing. Let us review the details of this derivation using linear algebra over
<a class="reference external" href="https://en.wikipedia.org/wiki/Normed_vector_space">normed vector spaces</a>.</p>
<div class="section" id="definitions">
<h2>Definitions</h2>
<div class="section" id="least-squares">
<h3>Least Squares</h3>
<p>A constrained weighted linear least …</p></div></div><p>Casting a linear least squares to a quadratic program is a common question for
newcomers, who see this operation routinely mentioned or taken for granted in
writing. Let us review the details of this derivation using linear algebra over
<a class="reference external" href="https://en.wikipedia.org/wiki/Normed_vector_space">normed vector spaces</a>.</p>
<div class="section" id="definitions">
<h2>Definitions</h2>
<div class="section" id="least-squares">
<h3>Least Squares</h3>
<p>A constrained weighted linear least squares (LS) problem is written as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\underset{x \in \mathbb{R}^n}{\mathrm{minimize}} & \frac{1}{2} \| R x - s \|^2_W = \frac{1}{2} (R x - s)^T W (R x - s) \\
\mathrm{subject\ to} & G x \leq h \\ & A x = b
\end{array}
\end{equation*}
</div>
<p>This problem seeks the vector <span class="math">\(x\)</span> of optimization variables such that
<span class="math">\(R x\)</span> is as "close" as possible to a prescribed vector <span class="math">\(s\)</span>,
meanwhile satisfying a set of linear inequality and equality constraints
defined by the matrix-vector couples <span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span>,
respectively. Vector inequalities apply coordinate by coordinate. How close the
two vectors <span class="math">\(R x\)</span> and <span class="math">\(s\)</span> are is measured via the weighted norm
<span class="math">\(\| y \|_W = \sqrt{y^T W y}\)</span>, where the weight matrix <span class="math">\(W\)</span> is
<a class="reference external" href="https://en.wikipedia.org/wiki/Definite_symmetric_matrix">positive semi-definite</a> and usually
diagonal. The overall function <span class="math">\(x \mapsto (1/2) \| R x - s \|^2_W\)</span> is
called the <em>objective function</em> (<em>a.k.a.</em> cost function) of the problem.</p>
</div>
<div class="section" id="quadratic-programming">
<h3>Quadratic Programming</h3>
<p>A quadratic program (QP) is written in standard form as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\underset{x \in \mathbb{R}^n}{\mathrm{minimize}} & \frac12 x^T P x + q^T x \\
\mathrm{subject\ to} & G x \leq h \\ & A x = b
\end{array}
\end{equation*}
</div>
<p>This problem seeks the vector <span class="math">\(x\)</span> of optimization variables such that the
quadratic objective function defined by the (positive semi-definite) matrix
<span class="math">\(P\)</span> and vector <span class="math">\(q\)</span> is minimized, meanwhile satisfying a set of
linear inequality and equality constraints defined by the matrix-vector couples
<span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span>, respectively. Vector inequalities apply
coordinate by coordinate.</p>
</div>
<div class="section" id="equivalence-between-optimization-problems">
<h3>Equivalence between optimization problems</h3>
<p>Let us define formally what it means to "cast" a least squares to a quadratic
program. Since they have the same inequality and equality constraints, the only
difference between them lies in their objective functions. We will say that two
objective functions <span class="math">\(f(x)\)</span> and <span class="math">\(g(x)\)</span> are <em>equivalent</em>, written as
<span class="math">\(f(x) \propto g(x)\)</span>, if they have the same optimum:</p>
<div class="math">
\begin{equation*}
f(x) \propto g(x)
\ \Leftrightarrow \ \underset{x \in \mathbb{R}^n}{\mathrm{argmin}}
\left\{ f(x) | G x \leq h, A x = b \right\}
=
\underset{x \in \mathbb{R}^n}{\mathrm{argmin}}
\left\{ g(x) | G x \leq h, A x = b \right\}
\end{equation*}
</div>
<p>Informally, the symbol <span class="math">\(\propto\)</span> thus means "does not change the optimum
of the optimization problem".</p>
</div>
</div>
<div class="section" id="conversion-to-a-quadratic-objective">
<h2>Conversion to a quadratic objective</h2>
<p>Our goal, starting from the LS objective function <span class="math">\(f(x) = (1/2) \| R x -
s \|^2_W\)</span>, is to find a QP objective function <span class="math">\(g(x) = (1/2) x^T P x + q^T
x\)</span> such that <span class="math">\(f(x) \propto g(x)\)</span>. The one-half multiplicative constant
being here by convention (and not affecting the optimum in any way), we can
forget it for now and expand the weighted norm as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\| R x - s \|_W^2
& = & (R x - s)^T W (R x - s) \\
& = & x^T R^T W R x - x^T R^T W s - s^T W R x + s^T W s \\
\end{array}
\end{equation*}
</div>
<p>We can discard the constant term <span class="math">\(s^T W s\)</span> as it does not depend on the
optimization vector <span class="math">\(x\)</span> and therefore doesn't affect the optimum of our
problem. We then have:</p>
<div class="math">
\begin{equation*}
\| R x - s \|_W^2 \propto x^T R^T W R x - x^T R^T W s - s^T W R x
\end{equation*}
</div>
<p>Next, recall that <span class="math">\(W\)</span> is positive semi-definite, hence in particular
symmetric: <span class="math">\(W^T = W\)</span>. The number <span class="math">\(s^T W R x\)</span> being real, it is
equal to its own transpose, that is:</p>
<div class="math">
\begin{equation*}
s^T W R x = (s^T W R x)^T = x^T R^T W^T s = x^T R^T W s
\end{equation*}
</div>
<p>Back to our rewriting of the objective function, we get:</p>
<div class="math">
\begin{equation*}
\| R x - s \|_W^2 \propto x^T (R^T W R) x - 2 (R^T W s)^T x
\end{equation*}
</div>
<p>Multiplying by the positive one-half constant yields:</p>
<div class="math">
\begin{equation*}
\frac12 \| R x - s \|_W^2 \propto \frac12 x^T P x + q^T x
\end{equation*}
</div>
<p>with <span class="math">\(P = R^T W R\)</span> and <span class="math">\(q = - R^T W s\)</span>.</p>
<div class="section" id="example-in-python">
<h3>Example in Python</h3>
<p>The <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a> Python module
for quadratic programming provides a <tt class="docutils literal">solve_ls</tt> function alongside its main
<tt class="docutils literal">solve_qp</tt> function. This function boils down to:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">solve_ls</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">s</span><span class="p">,</span> <span class="n">G</span><span class="p">,</span> <span class="n">h</span><span class="p">,</span> <span class="n">A</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">lb</span><span class="p">,</span> <span class="n">ub</span><span class="p">,</span> <span class="n">W</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="s1">'quadprog'</span><span class="p">):</span>
<span class="n">WR</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</span><span class="n">W</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
<span class="n">P</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</span><span class="n">R</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">WR</span><span class="p">)</span>
<span class="n">q</span> <span class="o">=</span> <span class="o">-</span><span class="n">dot</span><span class="p">(</span><span class="n">s</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">WR</span><span class="p">)</span>
<span class="k">return</span> <span class="n">solve_qp</span><span class="p">(</span><span class="n">P</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">G</span><span class="p">,</span> <span class="n">h</span><span class="p">,</span> <span class="n">A</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">lb</span><span class="p">,</span> <span class="n">ub</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="n">solver</span><span class="p">)</span>
</pre>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>LS and QP are not exactly equivalent (thanks to <a class="reference external" href="https://sites.google.com/site/adrienescandehomepage/">Adrien Escande</a> for correcting this in
an earlier version of this post): LS can be cast to QP, but QP can only be cast
to LS when the vector <span class="math">\(q\)</span> is in the image of the matrix <span class="math">\(P\)</span>.</p>
<p>The post on <a class="reference external" href="https://scaron.info/blog/linear-least-squares-in-python.html">least squares</a>
gives more details on <em>e.g.</em> using the weight matrix to steer the optimum of
the problem, or switching to lexicographic optimization when LS is not
expressive enough. For more focus on implementation, you can also check out the
more practical blog post on <a class="reference external" href="https://scaron.info/blog/quadratic-programming-in-python.html">quadratic programming in Python</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Linear least squares in Python2020-11-02T00:00:00+01:002020-11-02T00:00:00+01:00Stéphane Carontag:scaron.info,2020-11-02:/blog/linear-least-squares-in-python.html<p>Least squares is a widely used class of convex optimization that can be applied
to solve a variety of problems, for instance: in statistics for curve fitting,
in machine learning to compute <a class="reference external" href="https://en.wikipedia.org/wiki/Support_vector_machine">support vector machines</a>, in robotics to solve
<a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse kinematics</a>,
etc. They are related to <a class="reference external" href="https://scaron.info/blog/quadratic-programming-in-python.html">quadratic programming</a> (QP), having …</p><p>Least squares is a widely used class of convex optimization that can be applied
to solve a variety of problems, for instance: in statistics for curve fitting,
in machine learning to compute <a class="reference external" href="https://en.wikipedia.org/wiki/Support_vector_machine">support vector machines</a>, in robotics to solve
<a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse kinematics</a>,
etc. They are related to <a class="reference external" href="https://scaron.info/blog/quadratic-programming-in-python.html">quadratic programming</a> (QP), having a slightly more
intuitive objective function, and the first step beyond linear programming (LP)
in convex optimization.</p>
<div class="section" id="standard-form">
<h2>Standard form</h2>
<p>A constrained weighted linear least squares (LS) problem is written as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\underset{x \in \mathbb{R}^n}{\mathrm{minimize}} & \frac12 \| R x - s \|^2_W = \frac12 (R x - s)^T W (R x - s) \\
\mathrm{subject\ to} & G x \leq h \\ & A x = b
\end{array}
\end{equation*}
</div>
<p>This problem seeks the vector <span class="math">\(x\)</span> of optimization variables such that
<span class="math">\(R x\)</span> is as "close" as possible to a prescribed vector <span class="math">\(s\)</span>,
meanwhile satisfying a set of linear inequality and equality constraints
defined by the matrix-vector couples <span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span>,
respectively. Vector inequalities apply coordinate by coordinate. How close the
two vectors <span class="math">\(R x\)</span> and <span class="math">\(s\)</span> are is measured via the weighted norm
<span class="math">\(\| y \|_W = \sqrt{y^T W y}\)</span>, where the weight matrix <span class="math">\(W\)</span> is
<a class="reference external" href="https://en.wikipedia.org/wiki/Definite_symmetric_matrix">positive semi-definite</a> and usually
diagonal. The overall function <span class="math">\(x \mapsto (1/2) \| R x - s \|^2_W\)</span> is
called the <em>objective function</em> of the problem.</p>
</div>
<div class="section" id="graphical-interpretation">
<h2>Graphical interpretation</h2>
<p>Let us illustrate the intuition behind this in 2D:</p>
<img alt="LS is about finding the minimum of a weighted norm (circles) over a linear set (polygon)" class="center" src="https://scaron.info/figures/least-squares.png" />
<p>In the figure above, the level sets of the weighted norm are represented by
ellipses centered on the target <span class="math">\(s\)</span>, while the linear set of inequality
constraints corresponds to the orange polygon. (There is no equality constraint
in this example.) Since the target <span class="math">\(s\)</span> of the objective function is
outside of the polygon, the optimum <span class="math">\(x^*\)</span> of the LS lies at the boundary
of the linear set.</p>
<p>In this example, the matrix <span class="math">\(R\)</span> of the objective function is the
identity, <em>i.e.</em> <span class="math">\(x\)</span> tries to be as "close" as possible to <span class="math">\(s\)</span>, and
the norm to measure how close the two vectors are is defined by the weight
matrix <span class="math">\(W = \mathrm{diag}(w_1, w_2)\)</span>:</p>
<div class="math">
\begin{equation*}
\| x - s \|^2_W = w_1 (x_1 - s_1)^2 + w_2 (x_2 - s_2)^2
\end{equation*}
</div>
<p>The weights <span class="math">\(w_1\)</span> and <span class="math">\(w_2\)</span> shape the relative importance of
proximity along the first coordinate <span class="math">\((x_1 - s_1)^2\)</span> compared to
proximity along the second coordinate <span class="math">\((x_2 - s_2)^2\)</span>. Note that the
absolute value of these weights has no effect on the optimum of our problem:
the only thing that matters is their relative value <span class="math">\(w_1 / w_2\)</span>. The
higher this ration, the more the optimum will trade a larger <span class="math">\((x_2 -
s_2)^2\)</span> for a smaller <span class="math">\((x_1 - s_1)^2\)</span>.</p>
<p><em>Question</em>: in the figure above, what is the largest weight, <span class="math">\(w_1\)</span> or
<span class="math">\(w_2\)</span>?</p>
</div>
<div class="section" id="python-example">
<h2>Python example</h2>
<p>Suppose we have access to a <tt class="docutils literal">solve_ls</tt> function that solves least squares
problems in standard form. For instance, the <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a> module provides one directly if
you are using Python. Let us then see how to solve the following LS:</p>
<div class="math">
\begin{align*}
\begin{array}{rl}
\mathrm{minimize} & \left\| \left[\begin{array}{ccc}
1 & 2 & 0 \\
-8 & 3 & 2 \\
0 & 1 & 1 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\
x_3\end{array}\right] - \left[\begin{array}{c} 3 \\ 2 \\
3\end{array}\right] \right\|^2 \\
\mathrm{subject\ to} & \left[\begin{array}{ccc}
1 & 2 & 1 \\
2 & 0 & 1 \\
-1 & 2 & -1 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\
x_3\end{array}\right] \leq \left[\begin{array}{c}
3 \\ 2 \\ -2 \end{array} \right]
\end{array}
\end{align*}
</div>
<p>First, we write our LS matrices in proper format:</p>
<pre class="code python literal-block">
<span class="n">R</span> <span class="o">=</span> <span class="n">array</span><span class="p">([[</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">],</span> <span class="p">[</span><span class="o">-</span><span class="mf">8.</span><span class="p">,</span> <span class="mf">3.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">]])</span>
<span class="n">s</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">3.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="mf">3.</span><span class="p">])</span>
<span class="n">G</span> <span class="o">=</span> <span class="n">array</span><span class="p">([[</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">],</span> <span class="p">[</span><span class="mf">2.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">],</span> <span class="p">[</span><span class="o">-</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="o">-</span><span class="mf">1.</span><span class="p">]])</span>
<span class="n">h</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">3.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="o">-</span><span class="mf">2.</span><span class="p">])</span>
<span class="n">W</span> <span class="o">=</span> <span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
</pre>
<p>Finally, we compute the solution using the <tt class="docutils literal">solve_ls</tt> function:</p>
<pre class="code python literal-block">
<span class="n">In</span> <span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">solve_ls</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">s</span><span class="p">,</span> <span class="n">G</span><span class="p">,</span> <span class="n">h</span><span class="p">,</span> <span class="n">W</span><span class="o">=</span><span class="n">W</span><span class="p">)</span>
<span class="n">Out</span><span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">array</span><span class="p">([</span> <span class="mf">0.12997347</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.06498674</span><span class="p">,</span> <span class="mf">1.74005305</span><span class="p">])</span>
</pre>
</div>
<div class="section" id="problem-shaping">
<h2>Problem shaping</h2>
<p>In fields such as optimal control, a lot of human work lies in solving the
inverse problem: think of a desired behavior, formulate it as a numerical
optimization problem, run the system, realize that the optimal solution to the
problem is not what one wanted in the first place, update the problem
accordingly, iterate. This process challenges us to get better at problem
shaping: how to modify the problem to steer the behavior of its optimum towards
what we want?</p>
<div class="section" id="diagonal-weight-matrix">
<h3>Diagonal weight matrix</h3>
<p>The first trick in the book is to use a diagonal weight matrix, and play with
its weight to adjust the relative importance of one sub-objective, that is, one
term <span class="math">\((R_i x - s_i)^2\)</span> of the objective function, compared to the others.
Consider a diagonal weight matrix:</p>
<div class="math">
\begin{equation*}
W = \mathrm{diag}(w_1, \dots, w_k) =
\begin{bmatrix}
w_1 & 0 & \cdots & 0 \\
0 & w_2 & \ddots & \vdots \\
\vdots & \ddots & \ddots & 0 \\
0 & \cdots & 0 & w_k
\end{bmatrix}
\end{equation*}
</div>
<p>This weighting produces the objective function:</p>
<div class="math">
\begin{equation*}
\| R x - s \|^2_W = \sum_{i=1}^k w_i (R_i x - s_i)^2
\end{equation*}
</div>
<p>If the behavior of the solution is too poor on one sub-objective <span class="math">\(i\)</span>
among our <span class="math">\(k\)</span> sub-objectives, meaning <span class="math">\((R_i x^* - s_i)^2\)</span> is larger
than what we want at the optimum <span class="math">\(x^*\)</span>, we can improve it by increasing
<span class="math">\(w_i\)</span>. However, this will come at the price of decreasing the performance
on some other sub-objectives <span class="math">\(j \neq i\)</span>.</p>
<p>To explain this informally, let us get rid of the scale invariance of these
weights. The facts that all weights are positive makes the expression <span class="math">\(\|
R x - s \|^2_W\)</span> a <a class="reference external" href="https://en.wikipedia.org/wiki/Conical_combination">conical combination</a> of squared residuals
<span class="math">\((R_i x - s_i)^2\)</span>. We can account for this invariance by choosing weights
as a <a class="reference external" href="https://en.wikipedia.org/wiki/Convex_combination">convex combination</a>:</p>
<div class="math">
\begin{equation*}
\sum_{i=1}^k w_i = 1
\end{equation*}
</div>
<p>Given any weighted LS problem, we can always renormalize the weights (dividing
them by their collective sum) to get to an equivalent problem where weights
form a convex combination. We can then focus without loss of generality on
problems whose weights form a convex combination.</p>
<p>Intuitively, increasing a weight <span class="math">\(w_i\)</span> results in improved performance of
the corresponding objective, that is, a decrease in its corresponding squared
residual <span class="math">\((R_i x^* - s_i)^2\)</span> at the optimum. But now, the sum of weights
being equal to 1, we see how an increase in <span class="math">\(w_i\)</span> must be balanced by
decreasing other weights <span class="math">\(w_j\)</span> <span class="math">\((j \neq i)\)</span>, and thus lower the
performance on other sub-objectives. Overall, there is no free lunch: when it
is not possible to solve all sub-objectives perfectly (either because they
contradict each other or because of inequality and equality constraints), the
next best thing we can do is trade some sub-objectives for others.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Least squares is covered in Chapter 10 of Nocedal and Wright's <a class="reference external" href="https://doi.org/10.1007/978-0-387-40065-5">Numerical
Optimization</a>, as well as in
Section 1.2 of Boyd and Vandenberghe's <a class="reference external" href="https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf">Convex Optimization</a>. It is closely
related to quadratic programming, and we can <a class="reference external" href="https://scaron.info/blog/conversion-from-least-squares-to-quadratic-programming.html">cast LS problems to QP</a> in
order to use readily available QP solvers.</p>
<p>Using relative weights in a linearly constrained optimization is a particular
instance of <a class="reference external" href="https://hal.archives-ouvertes.fr/file/index/docid/751924/filename/m.pdf">lexicographic optimization</a>
with two levels of priority. Thinking in terms of levels of priority sheds a
new light on problem shaping. For instance, when one weight gets predominant
over the others, our LS problem becomes (increasingly difficult to solve and)
closer to a three-level optimization. In that case, we can get better
performance by using a proper lexicographic solver. See Section 2 of <a class="reference external" href="https://hal.inria.fr/hal-01418462/document">Geometric
and numerical aspects of redundancy</a> for a concrete discussion on
this.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Position and coordinate systems2020-09-27T00:00:00+02:002020-09-27T00:00:00+02:00Stéphane Carontag:scaron.info,2020-09-27:/robot-locomotion/position-and-coordinate-systems.html<p>The simplest object we can study in kinematics is the <em>point</em>, also known as
<em>particle</em> in classical mechanics. We can describe a point <span class="math">\(A\)</span> in the
Euclidean space by providing a vector <span class="math">\({}^\calB \bfp_A \in \mathbb{R}^3\)</span>
of <em>coordinates</em> in the reference frame <span class="math">\(\calB = (B, {}^\calB \bfe_x,
{}^\calB \bfe_y …</span></p><script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>The simplest object we can study in kinematics is the <em>point</em>, also known as
<em>particle</em> in classical mechanics. We can describe a point <span class="math">\(A\)</span> in the
Euclidean space by providing a vector <span class="math">\({}^\calB \bfp_A \in \mathbb{R}^3\)</span>
of <em>coordinates</em> in the reference frame <span class="math">\(\calB = (B, {}^\calB \bfe_x,
{}^\calB \bfe_y, {}^\calB \bfe_z)\)</span>.</p>
<div class="section" id="cartesian-coordinates">
<h2>Cartesian coordinates</h2>
<p>The three vectors <span class="math">\(({}^\calB \bfe_x, {}^\calB \bfe_y, {}^\calB \bfe_z)\)</span>
form an <a class="reference external" href="https://en.wikipedia.org/wiki/Orthonormal_basis">orthonormal basis</a>
of <span class="math">\(\mathbb{R}^3\)</span>. We can use it to express the <em>Cartesian coordinates</em>
of <span class="math">\(A\)</span>, which consist of:</p>
<ul class="simple">
<li>The signed distance <span class="math">\(x\)</span> along the x-axis.</li>
<li>The signed distance <span class="math">\(y\)</span> along the y-axis.</li>
<li>The signed distance <span class="math">\(z\)</span> along the z-axis.</li>
</ul>
<p>Mathematically:</p>
<div class="math">
\begin{equation*}
{}^\calB \bfp_A = \begin{bmatrix} x \\ y \\ z \end{bmatrix} = x\,{}^\calB \bfe_x + y\,{}^\calB \bfe_y + z\,{}^\calB \bfe_z
\end{equation*}
</div>
<p>Graphically:</p>
<img alt="Cartesian coordinates in the Euclidean space" class="align-center max-height-200px" src="https://scaron.info/figures/cartesian-coordinates.png" />
<p>On a side note, the figure above introduces a color convention from computer
graphics where the x-axis (sagittal) is red, the y-axis (lateral) green and the
z-axis (vertical) blue. This convention is common in off-the-shelf robotics
software such as <a class="reference external" href="https://wiki.ros.org/rviz">RViz</a> and <a class="reference external" href="https://github.com/rdiankov/openrave">OpenRAVE</a>.</p>
</div>
<div class="section" id="cylindrical-coordinates">
<h2>Cylindrical coordinates</h2>
<p>Cylindrical coordinates represent the position of a point relative to an axis
of interest, typically the vertical z-axis, and a direction of interest,
typically the sagittal x-axis. They consist of three numbers:</p>
<ul class="simple">
<li>The distance <span class="math">\(\rho\)</span> from <span class="math">\(A\)</span> to the z-axis.</li>
<li>The angle <span class="math">\(\varphi\)</span>, called <em>azimuthal angle</em> (or just <em>the azimuth</em>, which sounds way cooler), between the vector from <span class="math">\(B\)</span> to <span class="math">\(A\)</span> and <span class="math">\(\bfe_x\)</span>.</li>
<li>The axis coordinate <span class="math">\(z\)</span>.</li>
</ul>
<p>Mathematically, we can map the vector <span class="math">\({}^\cal B\bfchi_A\)</span> of cylindrical coordinates to Cartesian ones by:</p>
<div class="math">
\begin{equation*}
{}^\calB \bfchi_A = \begin{bmatrix} \rho \\ \varphi \\ z \end{bmatrix} \ \Longrightarrow
\ {}^\calB \bfp_A({}^\calB \bfchi_A) = \begin{bmatrix} \rho \cos \varphi \\ \rho \sin \varphi \\ z \end{bmatrix}
\end{equation*}
</div>
<p>Graphically:</p>
<img alt="Cylindrical coordinates in the Euclidean space" class="align-center max-height-200px" src="https://scaron.info/figures/cylindrical-coordinates.png" />
<p>These coordinates are sometimes useful to analyze quantities that act on a body
frame of a car or mobile robot. For example, when <a class="reference external" href="https://doi.org/10.1109/AMC.2012.6197026">an external disturbance is
applied to a humanoid robot</a>,
looking at the direction and amplitude of the disturbance in the horizontal
plane can be more insightful than looking at individual x- and y-coordinates.</p>
</div>
<div class="section" id="spherical-coordinates">
<h2>Spherical coordinates</h2>
<p>Spherical coordinates represent the position of a point by:</p>
<ul class="simple">
<li>Its distance <span class="math">\(r\)</span> to the origin <span class="math">\(B\)</span> of the reference frame.</li>
<li>The angle <span class="math">\(\varphi\)</span>, the azimuth™, identical to the one used in cylindrical coordinates.</li>
<li>The angle <span class="math">\(\theta\)</span>, called <em>polar</em>, corresponding to the amount of rotation to apply in the resulting plane to go from the "pole" to the desired point.</li>
</ul>
<p>Mathematically, we can map the vector <span class="math">\({}^\cal B\bfsigma_A\)</span> of spherical coordinates to Cartesian ones by:</p>
<div class="math">
\begin{equation*}
{}^\calB \bfsigma_A = \begin{bmatrix} r \\ \varphi \\ \theta \end{bmatrix} \ \Longrightarrow
\ {}^\calB \bfp_A({}^\calB \bfsigma_A) = \begin{bmatrix} r \cos \varphi \sin \theta \\ r \sin \varphi \sin \theta \\ r \cos \theta \end{bmatrix}
\end{equation*}
</div>
<p>Graphically:</p>
<img alt="Spherical coordinates in the Euclidean space" class="align-center max-height-200px" src="https://scaron.info/figures/spherical-coordinates.png" />
</div>
<div class="section" id="observations">
<h2>Observations</h2>
<div class="section" id="difference-between-the-object-and-its-coordinates">
<h3>Difference between the object and its coordinates</h3>
<p>These three coordinate systems can be used equivalently to represent the
position of our point <span class="math">\(A\)</span> in the reference frame <span class="math">\(\calB\)</span>. Note how
we have made, and will consistently make, the distinction between the point
itself <span class="math">\(A\)</span> and the vector <span class="math">\({}^\calB \bfp_A\)</span> (of coordinates (with
respect to a given reference frame)) used to represent this point
mathematically. The same point can for instance be written <span class="math">\({}^\calC p_A\)</span>
in another reference frame <span class="math">\(\calC\)</span> translated with respect to
<span class="math">\(\calB\)</span>. This mental gymnastics of keeping track of both the object and
its representation will become all the more important as we move on to linear
velocities, angular velocities, <a class="reference external" href="https://scaron.info/robot-locomotion/screw-theory.html">twists</a>, etc.</p>
</div>
<div class="section" id="omitting-the-reference-frame">
<h3>Omitting the reference frame</h3>
<p>When there is no ambiguity, the reference frame exponent is often dropped and
the position of point <span class="math">\(A\)</span> is abbreviated as <span class="math">\(\bfp_A\)</span>. Yet, physical
quantities are always expressed with respect to a <a class="reference external" href="https://en.wikipedia.org/wiki/Frame_of_reference">reference frame</a>, so implicitly there is
always an underlying frame to any vector <span class="math">\(\bfp_A\)</span> of position
coordinates.</p>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>ZMP support area2020-07-05T00:00:00+02:002021-10-16T00:00:00+02:00Stéphane Carontag:scaron.info,2020-07-05:/robot-locomotion/zmp-support-area.html<p>When a legged robot walks over a regular terrain, we can simplify its dynamics
to a reduced model like the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> where the center of mass (CoM)
is controlled via the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point</a> (ZMP) of contact forces with the
ground. To be physically feasible, the ZMP must …</p><p>When a legged robot walks over a regular terrain, we can simplify its dynamics
to a reduced model like the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> where the center of mass (CoM)
is controlled via the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point</a> (ZMP) of contact forces with the
ground. To be physically feasible, the ZMP must lie in a <em>support area</em>, also
known as <em>support polygon</em>, which is roughly "the area between the feet" in
simple cases. In more complex cases, such as a hand on a wall or a knee on the
ground, the area is a bit more complex but it is still computable.</p>
<div class="section" id="modeling">
<h2>Modeling</h2>
<p>The <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> describe the dynamics of our
articulated system without simplification:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = & \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ(\bfq)^\top \bff \\
\bfF(\bfq) \bff & \leq & \bfzero
\end{array}
\end{equation*}
</div>
<p>The configuration <span class="math">\(\bfq\)</span> consists of its <span class="math">\(n\)</span> actuated degrees of
freedom and 6 unactuated floating base coordinates. Contact forces from the
environment are stacked in the vector <span class="math">\(\bff\)</span> and constrained by the
matrix <span class="math">\(\bfF(\bfq)\)</span> of <a class="reference external" href="https://scaron.info/robot-locomotion/friction-cones.html">Coulomb friction cones</a>. The interest in switching to a simplified
model like the linear inverted pendulum (LIP) is to reduce the dimension of
this equation. The constrained equations of motion for the LIP are line-by-line
similar to those of the whole-body:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfpdd_G & = & \omega^2 (\bfp_G - \bfp_Z) \\
\bfA(\bfp_G) \bfp_Z & \leq & \bfb(\bfp_G)
\end{array}
\end{equation*}
</div>
<p>The configuration <span class="math">\(\bfp_G\)</span> consists of the the 2D horizontal position of
the center of mass. Contact forces from the environment are aggregated in the
position <span class="math">\(\bfp_Z\)</span> of the ZMP and constrained by the second line of
<a class="reference external" href="https://scaron.info/blog/polyhedra-and-polytopes.html#halfspace-representation">half-space inequalities</a>. The
matrix <span class="math">\(\bfA(\bfp_G)\)</span> and vector <span class="math">\(\bfb(\bfp_G)\)</span> define the support
area, which is a polygon as soon as we take into account actuation limits of
the robot. They can be computed from the contact geometry and friction
information encoded in <span class="math">\(\bfJ(\bfq)\)</span> and <span class="math">\(\bfF(\bfq)\)</span> by <a class="reference external" href="https://scaron.info/blog/projecting-polytopes.html">polyhedral
projection</a>. Further technical
details on how to do this are given in <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02108589/document">Section IV of this paper</a>.</p>
</div>
<div class="section" id="simplification-on-flat-floors">
<h2>Simplification on flat floors</h2>
<p>In pre-2010 robotics papers, we often see the ZMP support area defined as the
"convex hull of ground contact points", where <span class="math">\(\bfA\)</span> and <span class="math">\(\bfb\)</span> do
not depend on <span class="math">\(\bfp_G\)</span> any more and can be readily computed from ground
contact points by a convex hull algorithm (a particular case of polyhedral
projection). This simplification is only valid if we make two assumptions:</p>
<ul class="simple">
<li>All ground contact points are coplanar.</li>
<li>The coefficient of friction between robot feet and the ground is infinite.</li>
</ul>
<p>If we follow the general algorithm from the previous section, which takes the
CoM position <span class="math">\(\bfp_G\)</span> as an input, it is not clear how the ZMP support
area becomes independent from <span class="math">\(\bfp_G\)</span> under these two conditions. Let us
detail this step by step.</p>
<div class="section" id="infinite-friction">
<h3>Infinite friction</h3>
<p>Let us consider for simplicity of calculations that our flat ground contact
plane is horizontal (othogonal to gravity). In the absence of friction
constaints, our friction cone inequalities are reduced to contact unilaterality
constraints:</p>
<div class="math">
\begin{equation*}
\forall \textrm{ contact } i, \ f_i^z > 0
\end{equation*}
</div>
<p>This means we can exert arbitrary horizontal forces <span class="math">\(f_i^x\)</span> and
<span class="math">\(f_i^y\)</span> at every contact point <span class="math">\(i\)</span>.</p>
</div>
<div class="section" id="newton-euler-equations">
<h3>Newton-Euler equations</h3>
<p>Our contact forces are bound to the CoM and ZMP by <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton and Euler equations</a>. First, Newton equation gives us:</p>
<div class="math">
\begin{equation*}
\sum_{\textrm{contact } i} \bff_i = \bff = \frac{m g}{h} (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>with <span class="math">\(m\)</span> the total mass, <span class="math">\(g\)</span> the gravity constant and <span class="math">\(h\)</span> the
CoM height above ground. Second, Euler equation (with no angular momentum at
the center of mass since we are in a pendular model) gives us:</p>
<div class="math">
\begin{equation*}
\sum_{\textrm{ contact } i} \bfp_i \times \bff_i = \bfp_G \times \bff
\end{equation*}
</div>
<p>Let's unwrap it to understand better what it says. On flat floor, we have
<span class="math">\(z_i = 0\)</span> for all contact points <span class="math">\(i\)</span>, so that the vector cross
products expand to:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\sum_{i} y_i f_i^z & = & y_G f^z - h f^y \\
\sum_{i} -x_i f_i^z & = & -x_G f^z + h f^x \\
\sum_{i} (x_i f_i^y - y_i f_i^x) & = & x_G f^y - y_G f^x
\end{array}
\end{equation*}
</div>
<p>Since we are not limited by friction, the third equation always has a
solution and we can trim it out. In the other two equations, we can replace
<span class="math">\(\bff\)</span> by its expression <span class="math">\(mg (\bfp_G - p_Z) / h\)</span> from the linear
inverted pendulum model, for instance:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
y_G f^z & = & y_G m g (h - 0) / h = y_G m g \\
h f^y & = & m g (y_G - y_Z) \\
y_G f^z - h f^y & = & m g y_Z \\
mg y_Z & = & \sum_i {f_i^z} y_i
\end{array}
\end{equation*}
</div>
<p>and similarly for the other equation. Defining <span class="math">\(\alpha_i =
\frac{f_i^z}{mg}\)</span>, we get:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfp_Z & = & \sum_{i} \alpha_i \bfp_i \\
\forall i, \alpha_i & \geq & 0
\end{array}
\end{equation*}
</div>
<p>which is the <a class="reference external" href="https://scaron.info/blog/polyhedra-and-polytopes.html#vertex-representation">vertex representation</a> of the
convex hull of ground contact points <span class="math">\(\bfp_i\)</span>.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>ZMP support areas can be derived in any <a class="reference external" href="https://scaron.info/publications/tro-2016.html">multi-contact configurations</a>, but in this case they depend
(nonlinearly) on the position of the center of mass. Simplified ZMP support
areas can be applied for walking over both horizontal and inclined. For
instance, they have been used by <a class="reference external" href="https://scaron.info/publications/icra-2019.html">biped robots to climb stairs</a> and <a class="reference external" href="https://doi.org/10.3929/ethz-b-000221541">quadruped robots to trot over
inclined surfaces</a>. Joint torque
limits can also be included to compute <a class="reference external" href="https://arxiv.org/pdf/1903.07999.pdf">actuation-aware ZMP support polygons</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model2020-05-31T00:00:00+02:002020-05-31T00:00:00+02:00Stéphane Carontag:scaron.info,2020-05-31:/publications/icra-2020.html<p class="authors"><strong>Stéphane Caron</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The variable-height inverted pendulum (VHIP) model enables a new balancing
strategy by height variations of the center of mass, in addition to the
well-known ankle strategy. We propose a biped stabilizer based on linear
feedback of the VHIP that is simple to implement …</p></div><p class="authors"><strong>Stéphane Caron</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The variable-height inverted pendulum (VHIP) model enables a new balancing
strategy by height variations of the center of mass, in addition to the
well-known ankle strategy. We propose a biped stabilizer based on linear
feedback of the VHIP that is simple to implement, coincides with the
state-of-the-art for small perturbations and is able to recover from larger
perturbations thanks to this new strategy. This solution is based on
"best-effort" pole placement of a 4D divergent component of motion for the VHIP
under input feasibility and state viability constraints. We complement it with
a suitable whole-body admittance control law and test the resulting stabilizer
on the HRP-4 humanoid robot.</p>
</div>
<div class="section" id="videos">
<h2>Videos</h2>
<div class="section" id="chatty-presentation">
<h3>Chatty Presentation</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/icra-2020.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="tl-dw">
<h3>TL;DW</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/vhip-stabilization.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02289919/document">Paper</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/icra-2020/slides.pdf">Slides</a></td>
</tr>
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://scaron.info/videos/icra-2020.mp4">Presentation given at ICRA 2020</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/talks/jrl-2019.html">Presentation given at JRL</a> on 29 October 2019</td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/vhip_walking_controller/">Source code of the controller</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA40945.2020.9196715">10.1109/ICRA40945.2020.9196715</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">caron2020icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-02289919}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA40945.2020.9196715}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
<div class="section" id="discussion">
<h2>Discussion</h2>
<p>Here is a transcript of the discussion from the ICRA 2020 Slack channel about
this work. See also the discussions that followed the presentation on
<a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent components of motion</a> at JRL.</p>
<p><strong>When you use the QP, this considers only the instantaneous error, not along a
time horizon, is this correct?</strong></p>
<blockquote>
With DCMs, we are looking at an infinite-time horizon, with the DCM
converging to its target value only as time goes to infinity. But this is
similar to the <a class="reference external" href="https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_continuous-time_LQR">infinite-horizon linear quadratic regulator</a>:
even though we look at an infinite-time horizon, the optimal feedback
control law that we get is only based on the instantaneous error.</blockquote>
<p><strong>How can we generate reference trajectories?</strong></p>
<blockquote>
<p>There are several solutions:</p>
<ul class="simple">
<li>The seminal work by <a class="reference external" href="https://doi.org/10.1109/TRO.2015.2405592">Englsberger et al. (2015)</a> provides a closed-form
solution, which is also handy for <em>e.g.</em> step timing adaption.</li>
<li>In this work, I used a <a class="reference external" href="https://hal.inria.fr/inria-00390462/document">linear model predictive control</a> trajectory optimization
during experiments with HRP-4 (see the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/blob/master/include/lipm_walking/ModelPredictiveControl.h">C++ implementation</a>),
that it to say, reference trajectories were simply based on the linear
inverted pendulum model. Note that this is only for walking. While
standing, the reference has a constant center of mass position
<span class="math">\(c^d\)</span> and <span class="math">\(\dot{c}^d = 0\)</span>.</li>
<li>We can also generate VHIP references using the <a class="reference external" href="https://github.com/jrl-umi3218/CaptureProblemSolver/">CaptureProblemSolver</a>, which is a
custom sequential quadratic programming (SQP) implementation tailored to
this model. The algorithm is described <a class="reference external" href="/publications/tro-2019.html">here</a>.</li>
<li>We can also use a general optimal control framework to cast the full
trajectory generation problem. Examples of such frameworks today include
<a class="reference external" href="https://web.casadi.org/">CasADi</a>, <a class="reference external" href="https://github.com/loco-3d/crocoddyl">Crocoddyl</a> and <a class="reference external" href="https://github.com/jrl-umi3218/tvm">tvm</a>.</li>
</ul>
</blockquote>
<p><strong>You mention a reference trajectory, but your experiment does not seem to
follow a particular trajectory. In the context of balancing, what kind of
reference trajectory should we care about?</strong></p>
<blockquote>
Yes, in both the <a class="reference external" href="https://github.com/stephane-caron/pymanoid/blob/master/examples/vhip_stabilization.py">pymanoid example</a>
and HRP-4 experiment the reference trajectory is <span class="math">\(c^d =
\mathit{constant}\)</span> and <span class="math">\(\dot{c}^d = 0\)</span> (with the corresponding
<span class="math">\(\lambda^d\)</span> and <span class="math">\(r^d\)</span> computed for static equilibrium). For the
balance controller there is no concept of "standing" or "walking", as you
can see in the following block diagram: Balance Control consists of Reduced
Model Tracking and Force Control, while the switch between standing and
walking happens in Trajectory Generation.</blockquote>
<img alt="Block diagram illustrating balance control for legged robots." class="center max-height-125px" src="https://scaron.info/figures/balance-control-overview.png" />
<p><strong>In the video, why do you only formulate desired error dynamics a spring
system, instead of spring and damper?</strong></p>
<blockquote>
<p>Actually <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2012.6651601">Morisawa et al. (2012)</a> (reference 3 in the
video) has PID desired error dynamics. When I cite it in the video, I'm
only referring to the fact that pole placement means definiing your desired
error dynamics.</p>
<p>In this work we focus on the proportional (spring) term because it's the
most important in practice. The derivative (damping) term has usually a
(very) low gain because state estimators tend to yield noisy DCM
derivatives. On HRP-4 we set it to zero, and on HRP-2Kai we set it to a
<a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/blob/a21cd71bba0cdaa389c9350fcbc1b5f19731b86c/etc/LIPMWalking.conf.cmake#L134">small value</a>
(not zero because the stiffness of its flexible joint between sole and
ankle sole is lower than for HRP-4, and damping helps compensate the
ensuing vibrations at high P gain). This might evolve if somebody manages
to design a <strong>good DCM state estimator</strong> ;)</p>
</blockquote>
<p><strong>Let’s say we are in the wild searching for more “ducks” (i.e., higher-order
generalizations of the DCM). What characteristics must these quantities have to
be considered a DCM?</strong></p>
<blockquote>
<p>I see no definitive answer, but I'd venture to say:</p>
<ol class="arabic simple">
<li>They need to be "divergent". The trajectory of a DCM <span class="math">\(\xi\)</span> is unbounded unless the input <span class="math">\(u\)</span> satisfies a specific (dynamics-dependent) condition (the <a class="reference external" href="/teaching/capture-point.html#boundedness-condition">boundedness condition</a>). Alternatively, we can take inspiration from Coppel and lower-bound this unboundedness by exponentials, but we may need to look farther than exponential in general (<em>e.g.</em> in the 4D DCM the <span class="math">\(\dot{\omega} = \omega^2\)</span> component diverges super-exponentially).</li>
<li>They should decouple our second-order system into two consecutive first-order systems. This feels less like a property of the system and more like something we want. Here, when we choose to use Mike Hopkins's time-varying DCM, we are making sure CoM dynamics depend only on the DCM (<span class="math">\(\dot{c} = \omega (\xi - c)\)</span>). Secondly, we make sure the DCM depends only on the contact wrench input to get a decoupling similar to the LIP case (replacing "ZMP" by "contact wrench" and "capture point" by "4D DCM"):</li>
</ol>
</blockquote>
<img alt="Decoupling of second-order dynamics into two first-order system." class="center max-height-100px" src="https://scaron.info/figures/zmp-cp-com.png" />
<p><strong>It seems like the 4D DCM in this case is a consequence of your control parameterization in the sense that if you didn’t have a virtual stiffness lambda, you wouldn’t have the Riccati equation pop out for omega. Do you agree?</strong></p>
<p><strong>Where I’m pointing with this is that you could alternatively consider variation dynamics for the CoM directly, with some other parameterization for the forces. For instance if you just used the force as a control input, the CoM dynamics would be linear themselves. And so I’m curious what advantages we have by essentially lifting the CoM dynamics to this higher dimension with the addition of omega. Is it that constraints are easier to enforce?</strong></p>
<blockquote>
<p>I totally agree.</p>
<p>Looking back at the LIP, the main driver to go this way is that it extends constraints from short-term input feasibility to long-term state <a class="reference external" href="https://hal.inria.fr/inria-00390555/document">viability</a>. If we take a feedback controller using the force as control input <span class="math">\(F = k_p \Delta c + k_d \Delta \dot{c}\)</span>, we know from <a class="reference external" href="https://doi.org/10.1109/ROBOT.2009.5152284">Sugihara (2009)</a> that <span class="math">\(k_d = \frac{k_p}{\omega} - m \omega\)</span> (minus <span class="math">\(m \omega\)</span> because we use the force) is the choice that yields maximum capturability (<em>i.e.</em> the linear controller with the largest basin of attraction) under ZMP constraints. There we get our connection from short-term constraints to long-term: ZMP inequality constraints prompt us to express dynamics with the ZMP as input, whereupon a constant <span class="math">\(\omega\)</span> appears. This constant determines the feedback gains that maximize capturability.</p>
<p>Intuitively, the reason why this controller catches all capturable states is that it spends no input trying to control the CCM; everything goes to the DCM. Although we don't know yet a proof of maximum capturability for 4D DCM feedback, the controller behaves with similar parsimony: it only spends input on the DCM, and adds height variations only when it has to.</p>
<p>Going back to the first part of your question, the benefit of this control parameterization is that we optimize infinite-time horizon trajectories in a quadratic program (under variation dynamics, no guarantee that we maximize capturability for the full nonlinear system). "Solving infinite-time horizon" is a practical way to extend input feasibility constraints to state viability ;-) If we use the force as control input, our dynamics are simplified, but as you point out force constraints become CoM-dependent and we might have a hard time solving over an infinite horizon.</p>
</blockquote>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Constrained equations of motion2020-04-12T00:00:00+02:002021-10-11T00:00:00+02:00Stéphane Carontag:scaron.info,2020-04-12:/robot-locomotion/constrained-equations-of-motion.html<p>Let us take a closer look at how <em>Jacobian-transpose</em> term appears in the
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> of legged robots in
contact with their environment:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff
\end{equation*}
</div>
<p>The Jacobian-transpose term …</p><script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>Let us take a closer look at how <em>Jacobian-transpose</em> term appears in the
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> of legged robots in
contact with their environment:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff
\end{equation*}
</div>
<p>The Jacobian-transpose term maps external contact forces to joint torques (for
actuated coordinates) and to the net floating-base wrench (for unactuated
coordinates). We will see how to derive it from the principle of least
constraint, using the method of Lagrange multipliers.</p>
<div class="section" id="constraint-derivatives">
<h2>Constraint derivatives</h2>
<img alt="Humanoid whole-body dynamics involve contact forces and the dynamic momentum" class="right" src="https://scaron.info/figures/humanoid-2d-eom.png" />
<p>We consider a legged robot, for example the humanoid depicted in the figure to
the right, making <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#point-contacts-and-contact-forces">point contacts</a> or
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#surface-contacts-and-contact-wrenches">surface contacts</a>
with its environment. These kinematic constraints can be summarized into a
<a class="reference external" href="https://en.wikipedia.org/wiki/Holonomic_constraints">holonomic constraint</a>
<span class="math">\(\bfc(\bfq) = \bfzero\)</span>. In the case of point contacts, it would be the
stack of contact point equality constraints <span class="math">\(\bfp_C(\bfq) = \bfp_{D}\)</span>,
with <span class="math">\(\bfp_C\)</span> the mobile point on the robot's foot and <span class="math">\(\bfp_D\)</span> the
ground contact point in the inertial frame. Differentiating <span class="math">\(\bfc(\bfq) =
\bfzero\)</span> twice with respect to time yields:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}
\end{equation*}
</div>
<p>where the constraint Jacobian and Hessian matrices are defined by:</p>
\begin{align}
\bfJ_c(\bfq) & = \frac{\partial \bfc}{\partial \bfq}(\bfq) &
\bfH_c(\bfq) & = \frac{\partial^2 \bfc}{\partial \bfq^2}(\bfq)
\end{align}<p>The Jacobian is a <span class="math">\(3 k \times n\)</span> matrix, with <span class="math">\(k\)</span> the number of
contact poins and with <span class="math">\(n\)</span> the degree of freedom of our robot (number of
actuated joints plus six), while the Hessian is an <span class="math">\(n \times 3 k \times
n\)</span> tensor. Notations are the same for surface contacts, except that the degree
of constraint for each contact is six rather than three.</p>
</div>
<div class="section" id="principle-of-least-constraint">
<h2>Principle of least constraint</h2>
<p>Remember how, in the absence of constraint, the <em>equations of free motion</em> of
our system would be:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd_{\mathrm{free}} + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_{\mathit{ext}}
\end{equation*}
</div>
<p>where:</p>
<ul class="simple">
<li><span class="math">\(\bfM(\q)\)</span> is the <span class="math">\(n \times n\)</span> joint-space inertia matrix,</li>
<li><span class="math">\(\bfC(\q)\)</span> is the <span class="math">\(n \times n \times n\)</span> joint-space Coriolis tensor,</li>
<li><span class="math">\(\bftau\)</span> is the <span class="math">\(n\)</span>-dimensional vector of actuated joint torques,</li>
<li><span class="math">\(\bftau_g(\q)\)</span> is the <span class="math">\(n\)</span>-dimensional vector of gravity torques,</li>
<li><span class="math">\(\bftau_{\mathit{ext}}\)</span> is an optional vector of external forces and torques.</li>
</ul>
<p>We know how to <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#inertia-matrix-and-coriolis-tensor">compute these matrices</a> and
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#gravity-torques-and-external-forces">vectors</a> from
link inertias and Jacobians. However, when the robot is in contact, its motion
doesn't follow the vector <span class="math">\(\qdd_{\mathrm{free}}\)</span> defined by this equation
because of the additional contact constraint: the actual <span class="math">\(\qdd\)</span> has to be
such that</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}
\end{equation*}
</div>
<p><a class="reference external" href="https://en.wikipedia.org/wiki/Gauss%27s_principle_of_least_constraint">Gauss's principle of least constraint</a>
states that the robot's actual acceleration <span class="math">\(\qdd\)</span> is <em>as close as
possible</em> to the free acceleration <span class="math">\(\qdd_{\mathrm{free}}\)</span>, subject to
this constraint:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\qdd = \arg\min_{\bfa} & \frac{1}{2} \| \bfa - \qdd_{\mathrm{free}}\|_{\bfM}^2 \\
\textrm{subject to} & \bfJ_c(\bfq) \bfa + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}
\end{array}
\end{equation*}
</div>
<p>Note how the inertia matrix appears in the objective function to define what
<em>as close as possible</em> means here. The inertia matrix is positive definite so
this objective function is strictly convex.</p>
<div class="section" id="introducing-contact-forces">
<h3>Introducing contact forces</h3>
<p>We can write the Lagrangian of this equality-constrained least-squares problem
as:</p>
<div class="math">
\begin{equation*}
\calL(\bfa, \bflambda) = \frac{1}{2} (\bfa - \qdd_{\mathrm{free}})^\top \bfM (\bfa -
\qdd_{\mathrm{free}}) - \bflambda^\top \left(\bfJ_c \bfa + \qd^\top \bfH_c \qd\right)
\end{equation*}
</div>
<p>where we omit dependencies on <span class="math">\(\bfq\)</span> for concision. Dual variables
stacked in the vector <span class="math">\(\bflambda\)</span> correspond to contact forces. Why? By
definition! In <a class="reference external" href="https://en.wikipedia.org/wiki/Lagrangian_mechanics">Lagrangian mechanics</a>, there are two kinds of
forces: constraint and non-constraint forces. Non-constraint forces are somehow
easier to deal with: they correspond to the vectors <span class="math">\(\bftau_g(\bfq)\)</span> and
<span class="math">\(\bftau_{\mathit{ext}}\)</span> in the equations of free motion, and we already
know how to compute them. Contact forces are the constraint forces arising from
our kinematic contact constraint.</p>
<p>We can think of forces as dual variables that appear in the equations of motion
to ensure that the system's motion ends up satisfying the kinematic constraint.
Our assumption here is that the constraint will hold, <em>i.e.</em> that the robot
will stay in contact with its environment. In practice this is not true for any
command: if we send maximum joint torques, our robot will likely shake and jump
all over the place, breaking the assumption of fixed contact. To make sure that
this assumption holds, we will complement our kinematic constraint with a
dynamic <a class="reference external" href="https://scaron.info/robot-locomotion/contact-stability.html">contact stability</a> condition, but
this is another story. Let us get back to our derivation.</p>
</div>
<div class="section" id="method-of-lagrange-multipliers">
<h3>Method of Lagrange multipliers</h3>
<p>From the <a class="reference external" href="https://en.wikipedia.org/wiki/Lagrange_multiplier">method of Lagrange multipliers</a>, the optimum
<span class="math">\((\bfa^*, \bflambda^*)\)</span> of our constrained problem is a critical point of
the Lagrange function:</p>
<div class="math">
\begin{equation*}
\begin{array}{ccc}
\displaystyle \frac{\partial \calL}{\partial \bfa}(\bfa^*, \bflambda^*) = \bfzero
& \wedge &
\displaystyle \frac{\partial \calL}{\partial \bflambda}(\bfa^*, \bflambda^*) = \bfzero
\end{array}
\end{equation*}
</div>
<p>The second equation is, by construction, our contact constraint. The first one
gives us:</p>
<div class="math">
\begin{equation*}
\bfM (\bfa^* - \qdd_{\mathrm{free}}) - \bfJ_c^T \bflambda^* = \bfzero
\end{equation*}
</div>
<p><em>In fine</em>, replacing <span class="math">\(\bfa^*\)</span> by our notation for joint accelerations
<span class="math">\(\qdd\)</span> and similarly <span class="math">\(\bflambda^*\)</span> by <span class="math">\(\bff\)</span>, we obtain the
constrained equations of motion in joint space:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff
\end{equation*}
</div>
<p>with <span class="math">\(\bfM(\q)\)</span> the joint-space inertia matrix, <span class="math">\(\bfC(\q)\)</span> the
joint-space Coriolis tensor, <span class="math">\(\bftau_g(\q)\)</span> the vector of gravity
torques, <span class="math">\(\bftau\)</span> the vector of actuated joint torques, <span class="math">\(\bfJ_c\)</span> the
stacked Jacobian of kinematic contact constraints and <span class="math">\(\bff\)</span> the
corresponding stacked vector of contact forces.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>This page is based on Wieber's <a class="reference external" href="https://hal.inria.fr/inria-00390428/document">comments on the structure of the dynamics of
articulated motion</a>, as well as
on a very good summary in <a class="reference external" href="https://arxiv.org/pdf/1904.05072.pdf">Section IV.A of this paper</a> by Budhiraja <em>et al</em>.</p>
<p>Forces are virtual quantities that balance out our calculations. Defining them
as the dual multipliers of motion constraints is a key step in Lagrangian
mechanics: constraints "pull" on the system, which aspires to be somewhere else
according to the principle of least motion, and multipliers can be seen as a
measure of how hard the constraints need to pull to keep the system on track.
Check out Jensen's explanation on <a class="reference external" href="http://www.slimy.com/~steuard/teaching/tutorials/Lagrange.html#Meaning">the meaning of the multiplier</a> for
some great examples to build up intuition around this.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Feasible Region: an Actuation-Aware Extension of the Support Region2020-02-22T00:00:00+01:002020-02-22T00:00:00+01:00Stéphane Carontag:scaron.info,2020-02-22:/publications/tro-2020.html<p class="authors"><strong>Romeo Orsolino</strong>, <strong>Michele Focchi</strong>, <strong>Stéphane Caron</strong>, <strong>Gennaro Raiola</strong>,
<strong>Victor Barasuol</strong> and <strong>Claudio Semini</strong>. IEEE Transactions on Robotics.
Submitted March 2019. Accepted February 2020. Published August 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In legged locomotion the support region is defined as the 2D horizontal convex
area where the robot is able to support its own …</p></div><p class="authors"><strong>Romeo Orsolino</strong>, <strong>Michele Focchi</strong>, <strong>Stéphane Caron</strong>, <strong>Gennaro Raiola</strong>,
<strong>Victor Barasuol</strong> and <strong>Claudio Semini</strong>. IEEE Transactions on Robotics.
Submitted March 2019. Accepted February 2020. Published August 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In legged locomotion the support region is defined as the 2D horizontal convex
area where the robot is able to support its own body weight in static
conditions. Despite this definition, when the joint-torque limits (actuation
limits) are hit, the robot can be unable to carry its own body weight, even
when the projection of its Center of Mass (CoM) lies inside the support region.
In this manuscript we overcome this inconsistency by defining the Feasible
Region, a revisited support region that guarantees both global static stability
of the robot and the existence of a set of joint torques that are able to
sustain the body weight. Thanks to the usage of an Iterative Projection (IP)
algorithm, we show that the Feasible Region can be efficiently employed for
online motion planning of loco-manipulation tasks for both humanoids and
quadrupeds. Unlike the classical support region, the Feasible Region represents
a local measure of the robots robustness to external disturbances and it must
be recomputed at every configuration change. For this, we also propose a global
extension of the Feasible Region that is configuration independent and only
needs to be recomputed at every stance change.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://arxiv.org/pdf/1903.07999.pdf">Paper</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/orsoromeo/jet-leg">Jet-Leg library</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/TRO.2020.2983318">10.1109/TRO.2020.2983318</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">orsolino2020tro</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Feasible Region: an Actuation-Aware Extension of the Support Region}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Orsolino, Romeo and Focchi, Michele and Caron, St{\'e}phane and Raiola, Gennaro and Barasuol, Victor and Semini, Claudio}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Transactions on Robotics}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">aug</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{36}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{4}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{1239--1255}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://arxiv.org/pdf/1903.07999.pdf}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{IEEE}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/TRO.2020.2983318}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Humanoid Robots in Aircraft Manufacturing: The Airbus Use Cases2019-10-31T00:00:00+01:002019-10-31T00:00:00+01:00Stéphane Carontag:scaron.info,2019-10-31:/publications/humanoids-aircraft-manufacturing.html<p class="authors"><strong>Abderrahmane Kheddar</strong>, <strong>Stéphane Caron</strong>, <strong>Pierre Gergondet</strong>, <strong>Andrew
Comport</strong>, <strong>Arnaud Tanguy</strong>, <strong>Christian Ott</strong>, <strong>Bernd Henze</strong>, <strong>George
Mesesan</strong>, <strong>Johannes Englsberger</strong>, <strong>Máximo A. Roa</strong>, <strong>Pierre-Brice
Wieber</strong>, <strong>François Chaumette</strong>, <strong>Fabien Spindler</strong>, <strong>Giuseppe Oriolo</strong>,
<strong>Leonardo Lanari</strong>, <strong>Adrien Escande</strong>, <strong>Kevin Chappellet</strong>, <strong>Fumio
Kanehiro</strong> and <strong>Patrice Rabaté</strong>. IEEE Robotics and Automation Magazine.
December 2019. Best Paper Award …</p><p class="authors"><strong>Abderrahmane Kheddar</strong>, <strong>Stéphane Caron</strong>, <strong>Pierre Gergondet</strong>, <strong>Andrew
Comport</strong>, <strong>Arnaud Tanguy</strong>, <strong>Christian Ott</strong>, <strong>Bernd Henze</strong>, <strong>George
Mesesan</strong>, <strong>Johannes Englsberger</strong>, <strong>Máximo A. Roa</strong>, <strong>Pierre-Brice
Wieber</strong>, <strong>François Chaumette</strong>, <strong>Fabien Spindler</strong>, <strong>Giuseppe Oriolo</strong>,
<strong>Leonardo Lanari</strong>, <strong>Adrien Escande</strong>, <strong>Kevin Chappellet</strong>, <strong>Fumio
Kanehiro</strong> and <strong>Patrice Rabaté</strong>. IEEE Robotics and Automation Magazine.
December 2019. Best Paper Award.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We report results from a collaborative project that investigated the deployment
of <a class="reference external" href="https://cordis.europa.eu/article/id/386867-humanoids-may-soon-conquer-airplane-assembly">humanoid robotic solutions in aircraft manufacturing</a>
for some assembly operations where access is not possible for wheeled or
rail-ported robotic platforms. Recent developments in multi-contact planning
and control, bipedal walking, embedded SLAM, whole-body multi-sensory task
space optimization control, and contact detection and safety, suggest that
humanoids could be a plausible solution for automation given the specific
requirements in such large-scale manufacturing sites. The main challenge is to
integrate these scientific and technological advances into two existing
humanoid platforms: the position controlled HRP-4 and the torque controlled
TORO. This integration effort was demonstrated in a bracket assembly operation
inside a 1:1 scale A350 mockup of the front part of the fuselage at the Airbus
Saint-Nazaire site. We present and discuss the main results that have been
achieved in this project and provide recommendations for future work.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal-lirmm.ccsd.cnrs.fr/lirmm-02303117/document">Paper</a></td>
</tr>
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://ieeexplore.ieee.org/ielx7/100/4600619/8889461/mra2943395-kheddar-mm.zip?tp=&arnumber=8889461">Demonstration at the Airbus Saint-Nazaire factory</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">Walking controller of HRP-4</a> (Section IV.A)</td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/MRA.2019.2943395">10.1109/MRA.2019.2943395</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">kheddar2019ram</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Humanoid robots in aircraft manufacturing}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Kheddar, Abderrahmane and Caron, St{\'e}phane and Gergondet, Pierre and Comport, Andrew and Tanguy, Arnaud and Ott, Christian and Henze, Bernd and Mesesan, George and Englsberger, Johannes and Roa, M{\'a}ximo A and Wieber, Pierre-Brice and Chaumette, Fran{\c c}ois and Spindler, Fabien and Oriolo, Giuseppe and Lanari, Leonardo and Escande, Adrien and Chappellet, K{\'e}vin and Kanehiro, Fumio and Rabat{\'e}, Patrice}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Robotics and Automation Magazine}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{IEEE}</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{26}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{4}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2019}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">dec</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/MRA.2019.2943395}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Divergent components of motion2019-10-29T00:00:00+01:002019-10-29T00:00:00+01:00Stéphane Carontag:scaron.info,2019-10-29:/talks/jrl-2019.html<p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 29 October 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Some nonlinear control systems admit an <a class="reference external" href="https://en.wikipedia.org/wiki/Exponential_dichotomy">exponential dichotomy</a> (Coppel, 1966), that is
to say, their dynamics can be decomposed into (exponentially) stable and
unstable components. Walking robots fall into this category, and we call their
unstable …</p></div><p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 29 October 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Some nonlinear control systems admit an <a class="reference external" href="https://en.wikipedia.org/wiki/Exponential_dichotomy">exponential dichotomy</a> (Coppel, 1966), that is
to say, their dynamics can be decomposed into (exponentially) stable and
unstable components. Walking robots fall into this category, and we call their
unstable components <em>divergent components of motion</em> (DCM). The concept of DCM
has been fruitfully applied to the linear inverted pendulum (LIP) for both
walking pattern generation and balance feedback control. But DCMs can be found
for other models as well! In this talk, we will discuss DCMs for the
variable-height inverted pendulum (VHIP), an extension of the LIP where the
controller can add height variations. Ideally, we would like our robot to
behave as a LIP (nominal height) unless some perturbation occurs and the robot
resorts to the height-variation strategy, if it has to. Deciding when to use or
not this strategy may seem "smart" or predictive, but we will see that it can
be implemented straightforwardly as linear feedback over a 4D DCM.</p>
</div>
<div class="section" id="videos">
<h2>Videos</h2>
<div class="section" id="chatty-presentation">
<h3>Chatty Presentation</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/icra-2020.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="tl-dw">
<h3>TL;DW</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/vhip-stabilization.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/jrl-2019.pdf">Slides</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02289919/document">Feedback control of a 4D DCM for the variable-height inverted pendulum</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">Walking pattern generation with height variations</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="discussion">
<h2>Discussion</h2>
<p>Thanks to all the people who attended the presentations and asked meaningful
questions about it. Feel free to write me directly if you have any other
question related to this talk.</p>
<p><strong>What matrices did you use to generate the figure on slide 4?</strong></p>
<blockquote>
<p>This figure corresponds to:</p>
<ul class="simple">
<li><span class="math">\(A = \begin{bmatrix} +2 & 1 \\ 0 & +1 \end{bmatrix}\)</span> for <span class="math">\(\mathrm{eig}(A) = \{2, 1\}\)</span></li>
<li><span class="math">\(A = \begin{bmatrix} -2 & 1 \\ 0 & +1 \end{bmatrix}\)</span> for <span class="math">\(\mathrm{eig}(A) = \{-2, 1\}\)</span></li>
<li><span class="math">\(A = \begin{bmatrix} -2 & 1 \\ 0 & -1 \end{bmatrix}\)</span> for <span class="math">\(\mathrm{eig}(A) = \{-2, -1\}\)</span></li>
</ul>
</blockquote>
<p><strong>Why did you seem to doubt that ω is a DCM, isn't it clearly divergent?</strong></p>
<blockquote>
Yes, the point I had doubts on is about the "of motion" part. Previously,
when the DCM was directly computed by linear combination of the CoM
position and velocity, it was clear that "it diverges" and "it is a
component of motion" imply that it is a DCM. But here, <span class="math">\(\omega\)</span>
appears as a technical choice we make in order to diagonalize the
state-transition matrix after changing variable.</blockquote>
<p><strong>How do you choose the remaining proportional gain on slide 12?</strong></p>
<blockquote>
On the real robot, it will depend on your control cycle and in particular
on the bandwidth of the force control loop (admittance control on our
robots, see slide 26). DCM and force control gains are coupled when the two
are run at roughly the same frequency, as is the case here, and we are not
modeling this coupling. For practical advice, check out this note on
<a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/wiki/Tuning-the-stabilizer">tuning stabilizer gains</a>.</blockquote>
<p><strong>How did you select the poles in the final least-squares formulation?</strong></p>
<blockquote>
In general we could have four gains on the diagonal of the closed-loop
state-transition matrix, but in practice we often use the same gains for
different directions (for instance, the same gain for both sagittal and
lateral DCM feedback in the LIP). I followed this practice, using a single
normalized gain <span class="math">\(k > 1\)</span> and scaling it on each axis by a factor
consistent with the equations of motion.</blockquote>
<p><strong>Is there unicity of the DCMs or exponential dichotomy?</strong></p>
<blockquote>
No! For instance, in a <a class="reference external" href="/publications/icra-2018.html">previous work</a> we
had used a different DCM for the VHIP whose formula included the ZMP as
well. Multiplying a DCM by a non-zero scalar also yields a DCM, there may
be "classes" of equivalent DCMs for some equivalence relation, but I wonder
what it could be...</blockquote>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Lower body control of a semi-autonomous avatar in Virtual Reality: Balance and Locomotion of a 3D Bipedal Model2019-10-15T00:00:00+02:002019-10-15T00:00:00+02:00Stéphane Carontag:scaron.info,2019-10-15:/publications/vrst-2019.html<p class="authors"><strong>Vincent Thomasset</strong>, <strong>Stéphane Caron</strong> and <strong>Vincent Weistroffer</strong>. ACM
Symposium on Virtual Reality Software and Technology, Parramatta, Australia,
November 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Animated virtual humans may rely on full-body tracking system to reproduce user
motions. In this paper, we reduce tracking to the upper-body and reconstruct
the lower body to follow autonomously …</p></div><p class="authors"><strong>Vincent Thomasset</strong>, <strong>Stéphane Caron</strong> and <strong>Vincent Weistroffer</strong>. ACM
Symposium on Virtual Reality Software and Technology, Parramatta, Australia,
November 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Animated virtual humans may rely on full-body tracking system to reproduce user
motions. In this paper, we reduce tracking to the upper-body and reconstruct
the lower body to follow autonomously its upper counterpart. Doing so reduces
the number of sensors required, making the application of virtual humans
simpler and cheaper. It also enable deployment in cluttered scenes where the
lower body is often hidden. The contribution here is the inversion of the
well-known capture problem for bipedal walking. It determines footsteps rather
than center-of-mass motions and yet can be solved with an off-the-shelf capture
problem solver. The quality of our method is assessed in real-time tracking
experiments on a wide variety of movements.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal-cea.archives-ouvertes.fr/cea-02385996/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1145/3359996.3364240">10.1145/3359996.3364240</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">thomasset2019vrst</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Lower body control of a semi-autonomous avatar in Virtual Reality: Balance and Locomotion of a 3D Bipedal Model}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Thomasset, Vincent and Caron, St{\'e}phane and Weistroffer, Vincent}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{ACM Symposium on Virtual Reality Software and Technology}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2019}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">nov</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1145/3359996.3364240}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Capture point2019-10-14T00:00:00+02:002019-10-14T00:00:00+02:00Stéphane Carontag:scaron.info,2019-10-14:/robot-locomotion/capture-point.html<p>The <em>capture point</em> is a characteristic point of the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum
model</a>. It was coined by <a class="reference external" href="https://doi.org/10.1109/ICHR.2006.321385">Pratt
et al. (2006)</a> to address a
question of push recovery: where should the robot step <em>right now</em> to eliminate
linear momentum and come <em>asymptotically</em> to a stop?</p>
<div class="section" id="derivation">
<h2>Derivation</h2>
<p>Let us start from …</p></div><p>The <em>capture point</em> is a characteristic point of the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum
model</a>. It was coined by <a class="reference external" href="https://doi.org/10.1109/ICHR.2006.321385">Pratt
et al. (2006)</a> to address a
question of push recovery: where should the robot step <em>right now</em> to eliminate
linear momentum and come <em>asymptotically</em> to a stop?</p>
<div class="section" id="derivation">
<h2>Derivation</h2>
<p>Let us start from the equation of motion of the linear inverted pendulum, where
all the mass is concentrated at the center of mass <span class="math">\(\bfp_G\)</span>:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \omega^2 (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>We assume that the robot steps instantly at time <span class="math">\(t=0\)</span> and maintains its
<a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">ZMP</a> <span class="math">\(\bfp_Z\)</span> at a constant
location in its new foothold, so that <span class="math">\(\bfp_Z\)</span> is stationary. Since the
natural frequency <span class="math">\(\omega\)</span> of the pendulum is also a model constant, we
can solve this second-order linear differential equation as:</p>
<div class="math">
\begin{equation*}
\bfp_G(t) = \displaystyle \bfp_Z + \frac{e^{\omega t}}{2} \left[\bfp_G(0) +
\frac{\bfpd_G(0)}{\omega} - \bfp_Z\right] + \frac{e^{-\omega t}}{2}
\left[\bfp_G(0) - \frac{\bfpd_G(0)}{\omega} - \bfp_Z\right]
\end{equation*}
</div>
<p>This function is the sum of a stationary term <span class="math">\(\bfp_Z\)</span>, a convergent term
factored by <span class="math">\(e^{-\omega t}\)</span> that vanishes as <span class="math">\(t \to \infty\)</span>, and a
term factored by <span class="math">\(e^{\omega t}\)</span> that diverges as <span class="math">\(t \to \infty\)</span>.
Let us define the <em>capture point</em> as:</p>
<div class="math">
\begin{equation*}
\bfp_C \defeq \bfp_G + \frac{\bfpd_G}{\omega}
\end{equation*}
</div>
<p>The divergent term in <span class="math">\(\bfp_G(t)\)</span> is then <span class="math">\(e^{\omega t}/2
(\bfp_C(0) - \bfp_Z)\)</span>. In particular, the <em>only</em> way for the center of mass
trajectory to be bounded is for the stationary ZMP to be equal to the
instantaneous capture point:</p>
<div class="math">
\begin{equation*}
\bfp_Z = \bfp_C(0) \ \Longrightarrow \ \bfp_G(t) \underset{t \to
\infty}{\longrightarrow} \bfp_C(0)
\end{equation*}
</div>
<p>We can thus interpret the capture point as a point where the robot should step
(shift its ZMP) in order to come (asymptotically) to a stop.</p>
</div>
<div class="section" id="discussion">
<h2>Discussion</h2>
<p>The capture point is a <a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent component of motion</a> of the linear inverted pendulum. Shifting the
ZMP to the capture point prevents divergence from the unstable dynamics of the
model, but does not control the other (stable) component. In effect, the system
comes to a stop following its <em>natural</em> dynamics:</p>
<div class="math">
\begin{equation*}
\bfpd_G = \omega (\bfp_C - \bfp_G)
\end{equation*}
</div>
<p>This phenomenon is noticable in <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html#walking-stabilization">balance controllers</a> based on
capture point feedback such as <a class="reference external" href="https://doi.org/10.1109/IROS.2011.6094435">Englsberger et al. (2011)</a> and <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2012.6651601">Morisawa et al. (2012)</a>. Take the robot standing,
push it in a given direction and sustain your push, then suddenly release it:
the robot will come back to its reference standing position following its
natural dynamics (which only depend on <span class="math">\(\omega\)</span>, <em>i.e.</em> gravity and the
height of the center of mass), regardless of the values of the various feedback
gains used in the balance controller. You can for instance test this behavior
in dynamic simulations with the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">lipm_walking_controller</a>.</p>
<p>This behavior highlights how balance controllers based on capture-point
feedback are not trying to come to a stop as fast as possible. Rather, they
focus on preventing divergence, and leverage passive dynamics to absorb
undesired linear momentum. When using linear feedback, <a class="reference external" href="https://doi.org/10.1109/ROBOT.2009.5152284">Sugihara (2009)</a> showed that this approach
maximizes the basin of attraction of the resulting controller.</p>
</div>
<div class="section" id="boundedness-condition">
<h2>Boundedness condition</h2>
<p>The derivation above can be generalized to the case where <span class="math">\(\bfp_Z(t)\)</span> is
time-varying rather than time-invariant. Consider the equation of motion split
as follows into divergent and convergent components:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfpd_C & = & \omega (\bfp_C - \bfp_Z) \\
\bfpd_G & = & \omega (\bfp_C - \bfp_G)
\end{array}
\end{equation*}
</div>
<p>The capture point diverges away from the ZMP while the center of mass is
attracted to the capture point:</p>
<img alt="Decoupled dynamics between the ZMP, capture point and center of mass" class="center max-height-200px max-width-90pct" src="https://scaron.info/figures/zmp-cp-com.png" />
<p>As the center-of-mass dynamics are convergent, the system diverges <em>if and only
if</em> its capture point diverges. We can therefore focus on the
capture point dynamics alone.</p>
<p>The solution to a first-order linear time-varying differential equation is:</p>
<div class="math">
\begin{equation*}
\dot{\bfy}(t) - a(t) \bfy(t) = \bfb(t) \ \Longrightarrow
\ \bfy(t) = e^{A(t)} \left(\bfy(0) + \int_{\tau=0}^t \bfb(\tau) e^{-A(\tau)}
{\rm d} \tau \right)
\end{equation*}
</div>
<p>where <span class="math">\(A\)</span> is the antiderivative of <span class="math">\(a\)</span> such that <span class="math">\(A(0)=0\)</span>.
Applied to capture point dynamics, this formula becomes:</p>
<div class="math">
\begin{equation*}
\bfp_C(t) = e^{\omega t} \left(\bfp_C(0) - \omega \int_{\tau=0}^t \bfp_Z(t)
e^{-\omega \tau} {\rm d}\tau\right)
\end{equation*}
</div>
<p>We can check how, in the previous case where <span class="math">\(\bfp_Z\)</span> is stationary, this
formula becomes:</p>
<div class="math">
\begin{equation*}
\bfp_C(t) = \bfp_Z + e^{\omega t} (\bfp_C(0) - \bfp_Z)
\end{equation*}
</div>
<p>The capture point trajectory is then bounded if and only if <span class="math">\(\bfp_Z =
\bfp_C(0)\)</span>, which is indeed the result we obtained above. In the general case,
the capture point stays bounded if and only if:</p>
<div class="math">
\begin{equation*}
\bfp_C(0) = \omega \int_{\tau=0}^t \bfp_Z(t) e^{-\omega \tau} {\rm d}\tau
\end{equation*}
</div>
<p>This condition was coined <em>boundedness condition</em> by <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2014.7041478">Lanari et al. (2014)</a>. It relates <em>future</em> system
inputs to the present state, and characterizes the subset of these inputs that
will actually stabilize the system in the long run. The boundedness condition
is, for instance, a core component of the walking pattern generator from
<a class="reference external" href="https://arxiv.org/pdf/1901.08505.pdf">Scianca et al. (2019)</a>. It can also be
applied to more general reduced models such as the <a class="reference external" href="/publications/tro-2019.html">variable-height inverted
pendulum</a>.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The notion of <a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent component of motion</a>
behind the capture point reaches beyond the linear inverted pendulum model.
Check it out for extensions to more advanced balance control.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Linear inverted pendulum model2019-10-14T00:00:00+02:002019-10-14T00:00:00+02:00Stéphane Carontag:scaron.info,2019-10-14:/robot-locomotion/linear-inverted-pendulum-model.html<p>The linear inverted pendulum is a <a class="reference external" href="https://scaron.info/robot-locomotion/point-mass-model.html">point-mass model</a> that focuses on the translational dynamics
of a legged robot's locomotion.</p>
<div class="section" id="assumptions">
<h2>Assumptions</h2>
<p>Both fixed and mobile robots are usually modeled as rigid bodies connected by
actuated joints. The general <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> for such a system are:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd …</div></div><script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>The linear inverted pendulum is a <a class="reference external" href="https://scaron.info/robot-locomotion/point-mass-model.html">point-mass model</a> that focuses on the translational dynamics
of a legged robot's locomotion.</p>
<div class="section" id="assumptions">
<h2>Assumptions</h2>
<p>Both fixed and mobile robots are usually modeled as rigid bodies connected by
actuated joints. The general <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> for such a system are:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \sum_{i=1}^N \bfJ_{C_i}^\top \bff_i,
\end{equation*}
</div>
<p>where <span class="math">\(\bfq\)</span> is the vector of actuated and unactuated coordinates.
Actuated coordinates correspond to joint angles directly controlled by motors.
Unactuated coordinates correspond to the six degrees of freedom for the
position and orientation of the <em>floating base</em> (a frame attached to any of the
robot's bodies) with respect to the inertial fame. The vector <span class="math">\(\bfq\)</span> is
typically high-dimensional.</p>
<div class="section" id="centroidal-dynamics">
<h3>Centroidal dynamics</h3>
<p>The first working assumption to simplify this model is (Assumption 1) that the
robot has enough joint torques to realize the actuated part of the equation,
and focus on the <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton-Euler equations</a> that correspond to the six unactuated
coordinates:</p>
<div class="math">
\begin{equation*}
\begin{bmatrix} m \bfpdd_G \\ \dot{\bfL}_G \end{bmatrix}
=
\begin{bmatrix} \bff \\ \bftau_G \end{bmatrix}
+
\begin{bmatrix} m \bfg \\ \boldsymbol{0} \end{bmatrix}
\end{equation*}
</div>
<p>where on the left-hand side <span class="math">\(\bfp_G\)</span> is the position of the center
of mass (CoM) and <span class="math">\(\bfL_G\)</span> is the net angular momentum around the CoM,
while on the right-hand side <span class="math">\(\bff\)</span> is the resultant of contact forces,
<span class="math">\(\bftau_G\)</span> is the moment of contact forces around the CoM, <span class="math">\(m\)</span> is
the robot mass and <span class="math">\(\bfg\)</span> is the gravity vector. This model is called
<em>centroidal dynamics</em>.</p>
</div>
<div class="section" id="linearized-dynamics">
<h3>Linearized dynamics</h3>
<p>Angular momentum or height variations make centroidal dynamics nonlinear. This
means for instance that, to generate a trajectory for this system, one needs to
solve a nonlinear optimization. An alternative to linearize this system is to
make two assumptions:</p>
<ul class="simple">
<li><em>Assumption 2:</em> there is no angular momentum around the center of mass
<span class="math">\((\dot{\bfL}_G=\boldsymbol{0})\)</span>. This is why the Honda P2 <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=172">walks with
locked arms</a>.</li>
<li><em>Assumption 3:</em> the center of mass keeps a constant height. This is why the
Honda P2 <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=26">walks with bent knees</a>.</li>
</ul>
<p>These two assumptions are used to derive linearized dynamics as follows.</p>
</div>
</div>
<div class="section" id="id1">
<h2>Equations of motion</h2>
<p>Let us consider the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point (ZMP)</a> of the contact wrench. It is a
point <span class="math">\(Z\)</span> where the moment of contact forces is vertical:</p>
<div class="math">
\begin{equation*}
\bfe_Z \times \bftau_Z = \boldsymbol{0}
\end{equation*}
</div>
<p>with <span class="math">\(\bfe_Z\)</span> the unit upward vertical vector of the inertial frame. This
quantity defines an axis in general: to make <span class="math">\(Z\)</span> a unique point, let us
take it on the ground with <span class="math">\(z_Z = z_G - h\)</span>, where <span class="math">\(h\)</span> is the
constant height of the CoM. The moment <span class="math">\(\bftau_Z\)</span> of the contact wrench
at this ZMP is related to the moment at the CoM by:</p>
<div class="math">
\begin{equation*}
\bftau_Z = \bftau_G + (\bfp_G - \bfp_Z) \times \bff
\end{equation*}
</div>
<p>Since <span class="math">\(\bftau_G = \boldsymbol{0}\)</span> (Assumption 2), we have:</p>
<div class="math">
\begin{equation*}
\bfe_Z \times ((\bfp_G - \bfp_Z) \times \bff) = \boldsymbol{0}
\end{equation*}
</div>
<p>Applying the <a class="reference external" href="https://en.wikipedia.org/wiki/Triple_product#Vector_triple_product">vector triple product</a> formula,
we get:</p>
<div class="math">
\begin{equation*}
f_z (\bfp_G - \bfp_Z) - h \bff = \boldsymbol{0}
\end{equation*}
</div>
<p>From Newton's equation, <span class="math">\(\bff = m (\bfpdd_G - \bfg)\)</span> and we can rewrite
the equation above as:</p>
<div class="math">
\begin{equation*}
h (\bfpdd_G - \bfg) = (\zdd_G + g) (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>Since <span class="math">\(\zdd_G = 0\)</span> (Assumption 3), this equation is a trivial identity in
the vertical direction while its horizontal coordinates are:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \omega^2 (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>where <span class="math">\(\omega^2 = g / h\)</span>, <span class="math">\(g\)</span> is the gravity constant and <span class="math">\(h
= z_G - z_Z\)</span> is the constant height of the center of mass. The constant
<span class="math">\(\omega\)</span> is called <em>natural frequency</em> of the linear inverted pendulum.</p>
<img alt="Humanoid robot walking in the linear inverted pendulum mode" class="center max-height-450px" src="https://scaron.info/figures/lipm.png" />
<p>In this model, the robot can be seen as a point-mass concentrated at <span class="math">\(G\)</span>
resting on a mass-less leg in contact with the ground at <span class="math">\(Z\)</span>.
Intuitively, the ZMP is the point where the robot applies its weight. As a
consequence, this point needs to lie inside the contact surface <span class="math">\(\cal S\)</span>.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The linear inverted pendulum <em>mode</em> (not model) was introduced in <a class="reference external" href="https://doi.org/10.1109/IROS.2001.973365">Kajita et
al. (2001)</a> for walking pattern
generation. The rationale for calling it a <em>mode</em> of motion is that it relies
solely on planar linear momentum, which is one among many ways to affect the
acceleration of the center of mass and thus locomote (others include <a class="reference external" href="/publications/tro-2019.html">height
variations</a> and angular momentum variations that
separate the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">ZMP</a> and <a class="reference external" href="http://ijr.sagepub.com/cgi/doi/10.1177/0278364905058363">centroidal
moment pivot</a>).</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Balance of Humanoid Robots in a Mix of Fixed and Sliding Multi-Contact Scenarios2019-09-16T00:00:00+02:002019-09-16T00:00:00+02:00Stéphane Carontag:scaron.info,2019-09-16:/publications/icra-2020-samadi.html<p class="authors"><strong>Saeid Samadi</strong>, <strong>Stéphane Caron</strong>, <strong>Arnaud Tanguy</strong> and <strong>Abderrahmane
Kheddar</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>This study deals with the balance of humanoid or multi-legged robots in a
multi-contact setting where a chosen subset of contacts is undergoing desired
sliding-task motions. One method to keep balance is to hold the center-of-mass …</p></div><p class="authors"><strong>Saeid Samadi</strong>, <strong>Stéphane Caron</strong>, <strong>Arnaud Tanguy</strong> and <strong>Abderrahmane
Kheddar</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>This study deals with the balance of humanoid or multi-legged robots in a
multi-contact setting where a chosen subset of contacts is undergoing desired
sliding-task motions. One method to keep balance is to hold the center-of-mass
(CoM) within an admissible convex area. This area is calculated based on the
contact positions and forces. We introduce a methodology to compute this CoM
support area (CSA) for multiple fixed and intentionally sliding contacts. To
select the most appropriate CoM position within CSA, we account for (i)
constraints of multiple fixed and sliding contacts, (ii) desired wrench
distribution for contacts, and (iii) desired CoM position (eventually dictated
by other tasks). These are formulated as a quadratic programming (QP)
optimization problems. We illustrate our approach with pushing against a wall
and wiping, and conducted experiments using the HRP-4 humanoid robot.</p>
</div>
<div class="section" id="video">
<h2>Video</h2>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/Wai-Lp4e5FE" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02297879/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA40945.2020.9197253">10.1109/ICRA40945.2020.9197253</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">samadi2020icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Balance of Humanoid Robots in a Mix of Fixed and Sliding Multi-Contact Scenarios}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Samadi, Saeid and Caron, St{\'e}phane and Tanguy, Arnaud and Kheddar, Abderrahmane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-02297879}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA40945.2020.9197253}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Parametric recurrence quantification analysis of autoregressive processes for pattern recognition in multichannel electroencephalographic data2019-07-05T00:00:00+02:002019-07-05T00:00:00+02:00Stéphane Carontag:scaron.info,2019-07-05:/publications/patcog-2020.html<p class="authors"><strong>Sofiane Ramdani</strong>, <strong>Anthony Boyer</strong>, <strong>Stéphane Caron</strong>, <strong>François Bonnetblanc</strong>, <strong>Frédéric Bouchara</strong>, <strong>HuguesDuffau</strong> and <strong>Annick Lesne</strong>. Pattern Recognition. Submitted July 2019. Accepted August 2020. To appear in January 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Recurrence quantification analysis (RQA) is an acknowledged method for the
characterization of experimental time series. We propose a parametric version
of RQA …</p></div><p class="authors"><strong>Sofiane Ramdani</strong>, <strong>Anthony Boyer</strong>, <strong>Stéphane Caron</strong>, <strong>François Bonnetblanc</strong>, <strong>Frédéric Bouchara</strong>, <strong>HuguesDuffau</strong> and <strong>Annick Lesne</strong>. Pattern Recognition. Submitted July 2019. Accepted August 2020. To appear in January 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Recurrence quantification analysis (RQA) is an acknowledged method for the
characterization of experimental time series. We propose a parametric version
of RQA, pRQA, allowing a fast processing of spatial arrays of time series, once
each is modeled by an autoregressive stochastic process. This method relies on
the analytical derivation of asymptotic expressions for five current RQA
measures as a function of the model parameters. By avoiding the construction of
the recurrence plot of the time series, pRQA is computationally efficient. As a
proof of principle, we apply pRQA to pattern recognition in multichannel
electroencephalographic (EEG) data from a patient with a brain tumor.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1016/J.PATCOG.2020.107572">10.1016/J.PATCOG.2020.107572</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">ramdani2021patcog</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Parametric recurrence quantification analysis of autoregressive processes for pattern recognition in multichannel electroencephalographic data}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Ramdani, Sofiane and Boyer, Anthony and Caron, St{\'e}phane and Bonnetblanc, Fran{\c{c}}ois and Bouchara, Frederic and Duffau, Hugues and Lesne, Annick}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{Pattern Recognition}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{107572}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2021}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">jan</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{109}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{Elsevier}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1016/j.patcog.2020.107572}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Walking and stair climbing controller for locomotion in an aircraft factory2019-06-05T00:00:00+02:002019-06-05T00:00:00+02:00Stéphane Carontag:scaron.info,2019-06-05:/talks/jpl-2019.html<p class="authors">Talk given at the NASA-Caltech <a class="reference external" href="https://www.jpl.nasa.gov/">Jet Propulsion Laboratory</a> on 5 June 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The task was initially phrased for humans: go upstairs, walk into the aircraft,
pick up the object and place it on the fuselage. Yet, repetition and the
disposition of target locations turned out to cause chronic back …</p></div><p class="authors">Talk given at the NASA-Caltech <a class="reference external" href="https://www.jpl.nasa.gov/">Jet Propulsion Laboratory</a> on 5 June 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The task was initially phrased for humans: go upstairs, walk into the aircraft,
pick up the object and place it on the fuselage. Yet, repetition and the
disposition of target locations turned out to cause chronic back pain, and the
question of automation was raised: can a robot do that? In this talk, we will
see how the HRP-4 humanoid can do that, and discuss the components we developed
for the <a class="reference external" href="https://cordis.europa.eu/article/id/386867-humanoids-may-soon-conquer-airplane-assembly">final demonstrator of the COMANOID project</a>
that ran at the Airbus Saint-Nazaire factory on 21 February 2019: self
localization, locomotion, object localization and manipulation. We will focus
on walking and stair climbing control. The audience is encouraged to bring
plastic bottles.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/jpl-2019.pdf">Slides</a></td>
</tr>
<tr><td><img alt="youtube" class="icon" src="https://scaron.info/images/icons/youtube.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=vFCFKAunsYM&t=22">Stair climbing experiment</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Stair climbing stabilization of the HRP-4 humanoid robot using whole-body admittance control</a></td>
</tr>
</tbody>
</table>
</div>
Stair Climbing Stabilization of the HRP-4 Humanoid Robot using Whole-body Admittance Control2019-05-20T00:00:00+02:002021-10-08T00:00:00+02:00Stéphane Carontag:scaron.info,2019-05-20:/publications/icra-2019.html<p class="authors"><strong>Stéphane Caron</strong>, <strong>Abderrahmane Kheddar</strong> and <strong>Olivier Tempier</strong>. ICRA
2019, Montreal, Canada, May 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We consider dynamic stair climbing with the HRP-4 humanoid robot as part of an
Airbus manufacturing use-case demonstrator. We share experimental knowledge
gathered so as to achieve this task, which HRP-4 had never been challenged to …</p></div><p class="authors"><strong>Stéphane Caron</strong>, <strong>Abderrahmane Kheddar</strong> and <strong>Olivier Tempier</strong>. ICRA
2019, Montreal, Canada, May 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We consider dynamic stair climbing with the HRP-4 humanoid robot as part of an
Airbus manufacturing use-case demonstrator. We share experimental knowledge
gathered so as to achieve this task, which HRP-4 had never been challenged to
before. In particular, we extend walking stabilization based on <a class="reference external" href="https://doi.org/10.1109/IROS.2010.5651082">linear
inverted pendulum tracking</a> by
quadratic programming-based wrench distribution and a whole-body admittance
controller that applies both end-effector and CoM strategies. While existing
stabilizers tend to use either one or the other, our experience suggests that
the combination of these two approaches improves tracking performance. We
demonstrate this solution in an on-site experiment where HRP-4 climbs an
industrial staircase with 18.5 cm high steps, and release <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">our walking
controller</a> as
open source software.</p>
</div>
<div class="section" id="videos">
<h2>Videos</h2>
<div class="section" id="climbing-stairs">
<h3>Climbing stairs</h3>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/vFCFKAunsYM?start=22" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="standing-on-mobile-ground">
<h3>Standing on mobile ground</h3>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/MxSJtEKkZE0?start=7" allowfullscreen seamless frameBorder="0"></iframe></div></div>
</div>
<div class="section" id="content">
<h2>Content</h2>
<p>The paper on HAL is a <em>post-print</em> I have kept updating after the conference to
keep up with the later additions and fixes implemented in the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">walking
controller</a>. See
the release notes on GitHub for an overview of the major changes since the
conference version (v1.1).</p>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Post-print</a> paper</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/icra-2019/poster.pdf">Poster</a> presented at ICRA 2019</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="/slides/jpl-2019.pdf">Presentation given at the Jet Propulsion Laboratory</a> on 5 June 2019</td>
</tr>
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://ieeexplore.ieee.org/document/8794348/media">Stair climbing at the Airbus Saint-Nazaire factory</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">Walking controller (C++)</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA.2019.8794348">10.1109/ICRA.2019.8794348</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="supplementary-material">
<h2>Supplementary material</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="/doc/lipm_walking_controller/">Controller documentation</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/robot-locomotion/floating-base-estimation.html">Floating base observer</a> used in this controller</td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">Overview</a> of the controller's principles</td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html">Python prototype</a> of the controller's walking pattern generator</td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">caron2019icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Stair Climbing Stabilization of the {HRP}-4 Humanoid Robot using Whole-body Admittance Control}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane and Kheddar, Abderrahmane and Tempier, Olivier}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01875387}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2019}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA.2019.8794348}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
<div class="section" id="discussion">
<h2>Discussion</h2>
<p>See also the list of <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/wiki">frequently asked questions</a> about the
open source controller.</p>
<p><strong>Ankle angular velocities are clamped in the foot damping controller. How
significant is this? Does the pipeline rely on this?</strong></p>
<blockquote>
We clamped angular velocities at 0.2 rad/s for the ankles. In the nominal
behavior (<em>a.k.a.</em> "if everything goes well"™) no clamping should happen,
so the control pipeline does not rely on this part. This clamping is rather
a safety to avoid some states going from bad to worse: if the ZMP hits the
boundary of its support area and the foot starts to tilt around its edge,
force/torque sensors may measure an obscenely-far ZMP. We don't want to
forward this value unclamped to a direct velocity controller ;-)</blockquote>
<p><strong>In the MPC trajectory generator, what ZMP constraints are enforced during
double support?</strong></p>
<blockquote>
To compute open-loop walking trajectories (<em>i.e.</em> before sensor readings
and DCM feedback, which are the task of the stabilizer), we solve linear
model predictive control (MPC) quadratic programs. These programs follow
the formulation from Wieber <em>et al.</em>, see <em>e.g.</em> the writeup in <a class="reference external" href="https://hal.inria.fr/hal-01418355/document">Brasseur
et al. (2015)</a>. In
particular, problems from this line of work use CoM jerk as control
inputs/optimization variables, and don't have inequality constraints during
double support phases, owing to the fact that these phases are no more than
one or two timesteps. See the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/wiki/What-ZMP-inequality-constraints-are-used-in-the-MPC-during-stair-climbing%3F">ZMP inequality constraint FAQ</a>
of the controller for more technical details.</blockquote>
<p><strong>Why isn't there an angular momentum task in whole body control?</strong></p>
<blockquote>
Although the linear inverted pendulum model assumes constant angular
momentum around the CoM, this controller does not try to explicitly control
angular momentum. Rather, it relies on <em>chest posture control</em> for the
upper body, and treats angular momentum variations as unmodeled
disturbances handled after the fact by DCM feedback. Chest posture control
is synchronized with foot damping control to adapt to terrain variations:
see the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/wiki/Why-isn%27t-there-an-angular-momentum-task-in-whole-body-control%3F">angular momentum task FAQ</a>
of the controller for more technical details.</blockquote>
<p><strong>What are the pros and cons of CoM admittance control?</strong></p>
<blockquote>
While we used CoM admittance control in 2019 stair climbing experiments,
such as the one you see in the video above, we disabled it by default when
distributing the open source controller. This choice is detailed in the
<a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/wiki/What-are-the-pros-and-cons-of-CoM-admittance-control%3F">CoM admittance control FAQ</a>
of the controller. In short: it improves disturbance rejection when contact
force constraints are saturated (like we had when stepping up), but on
stiff position-controlled robots like HRP-4 it has a tendency to pick up
resonance vibrations.</blockquote>
<p><strong>How is the divergent component of motion (DCM) estimated?</strong></p>
<blockquote>
As described in Appendix B, we compute the translation and orientation of
the floating base using the classical <a class="reference external" href="https://scaron.info/teaching/floating-base-estimation.html">IMU with anchor frame</a> estimate. We
then derive the CoM position by combining it with joint encoder
measurements, and the CoM velocity by first-order <a class="reference external" href="https://en.wikipedia.org/wiki/Low-pass_filter">low-pass filtering</a> of this position.</blockquote>
<p><strong>Why use damping control at the foot contact frame rather than, say, an acceleration-based force control law?</strong></p>
<blockquote>
<p>The answer can be found in Equation (2) of <a class="reference external" href="https://doi.org/10.1109/ROBOT.2001.933139">Kajita et al. (2001)</a>. Robots from the HRP series have inherited from their Honda elders the choice of a mechanical flexibility (rubber bushes and linear dampers) added between the ankle and foot links. Stiffness <span class="math">\(K\)</span> dominates damping <span class="math">\(B\)</span> in this flexibility, that is, in practice <span class="math">\(K \Delta \theta \gg B \Delta \dot{\theta}\)</span> where <span class="math">\(\theta\)</span> is a contact frame angle. We then use as a ground reaction torque model:</p>
<blockquote>
<div class="math">
\begin{equation*}
\tau = K (\theta - \theta_{\mathit{ground}})
\end{equation*}
</div>
</blockquote>
<p>Usually the environment is static, so that the time-derivative of this expression is:</p>
<blockquote>
<div class="math">
\begin{equation*}
\dot{\tau} = K \dot{\theta}
\end{equation*}
</div>
</blockquote>
<p>Foot damping control corresponds to the following angular velocity:</p>
<blockquote>
<div class="math">
\begin{equation*}
\dot{\theta} = A (\tau_{\mathit{desired}} - \tau)
\end{equation*}
</div>
</blockquote>
<p>In closed loop, this yields:</p>
<blockquote>
<div class="math">
\begin{equation*}
\dot{\tau} = K A (\tau_{\mathit{desired}} - \tau)
\end{equation*}
</div>
</blockquote>
<p>This ensures that <span class="math">\(\tau \to \tau_{\mathit{desired}}\)</span> as <span class="math">\(t \to \infty\)</span>.</p>
</blockquote>
<p><strong>OK, but why in practice use damping control rather than an acceleration-based formulation?</strong></p>
<blockquote>
Large stiffness and low damping were rather a design choice that came with
the robots we worked with. The second-order model with both stiffness
<span class="math">\(K\)</span> and damping <span class="math">\(B\)</span> coefficients has more parameters to tune,
but if <span class="math">\(K \Delta \theta \gg B \Delta \dot{\theta}\)</span> this more complex
model will anyway behave like a first-order model. Let us apply Occam's
razor then: using the first-order model, we get similar performance with
less parameters to tune and simpler code to maintain ;-)</blockquote>
<p><strong>Shall we try moving away from such high-stiffness low-damping at the contact interface?</strong></p>
<blockquote>
<p>Absolutely, moving away from high stiffness and low damping at contact
makes sense in my opinion. We tried moving away from this setting when
<a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01576516/document">walking on gravel with soft soles</a>: the soles had a
Young's modulus of 0.32 MPa (versus 5 MPa in previous works and
I-don't-know-how-much for the default hard robot soles) meaning we reduced
<span class="math">\(K\)</span> rather than increase <span class="math">\(B\)</span>. We still walked on hard floor
though. There were two takeaway observations from this work:</p>
<p>First, the interaction with the ground through soft soles had more (ankle
displacement to reaction force change) delay, <em>i.e.</em> lower force control
bandwidth. This is not great for balance control where, when the stabilizer
outputs a desired contact wrench, we want this wrench to be applied to the
environment as fast as possible. That's one thing to look at: what
combinations of <span class="math">\(K\)</span> and <span class="math">\(B\)</span> maximize the force control
bandwidth?</p>
<p>Second, foot impacts at touchdown with soft soles were better mitigated
mechanically. Walking controllers typically touch down earlier than
expected at every step, which incurs undesired CoM velocity disturbances
(foot impacts after they propagate across the kinematic chain). But with
the soft soles these impacts were lower. If we imagine a function
<span class="math">\(\mathrm{impact}(K, B)\)</span> at touchdown, it is likely decreasing in
<span class="math">\(B\)</span> yet increasing in <span class="math">\(K\)</span>. This leads us to another question:
for a given <span class="math">\(K\)</span> (and some model of early-touchdown impact), what
values of <span class="math">\(B\)</span> minimize <span class="math">\(\mathrm{impact}(K, B)\)</span>?</p>
</blockquote>
<p><strong>What is the formula for the matrix</strong> <span class="math">\(\bfU\)</span> <strong>in Equation (10)?</strong></p>
<blockquote>
<p>This matrix is given by:</p>
<blockquote>
<div class="math">
\begin{equation*}
\bfU = \begin{bmatrix}
-1 & 0 & -\mu & 0 & 0 & 0 \\
+1 & 0 & -\mu & 0 & 0 & 0 \\
0 & -1 & -\mu & 0 & 0 & 0 \\
0 & +1 & -\mu & 0 & 0 & 0 \\
0 & 0 & -Y & -1 & 0 & 0 \\
0 & 0 & -Y & +1 & 0 & 0 \\
0 & 0 & -X & 0 & -1 & 0 \\
0 & 0 & -X & 0 & +1 & 0 \\
-Y & -X & -(X + Y) \mu & +\mu & +\mu & -1 \\
-Y & +X & -(X + Y) \mu & +\mu & -\mu & -1 \\
+Y & -X & -(X + Y) \mu & -\mu & +\mu & -1 \\
+Y & +X & -(X + Y) \mu & -\mu & -\mu & -1 \\
+Y & +X & -(X + Y) \mu & +\mu & +\mu & +1 \\
+Y & -X & -(X + Y) \mu & +\mu & -\mu & +1 \\
-Y & +X & -(X + Y) \mu & -\mu & +\mu & +1 \\
-Y & -X & -(X + Y) \mu & -\mu & -\mu & +1
\end{bmatrix}
\end{equation*}
</div>
</blockquote>
<p>with <span class="math">\(\mu\)</span> the friction coefficient, <span class="math">\(X\)</span> the half-length and
<span class="math">\(Y\)</span> the half-width of the rectangular contact area. Check out <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02108449/document">this
paper</a> for the
derivation of this formula, which corresponds to its equations (15) to
(20). In code, it is implemented in the functions:</p>
<ul class="simple">
<li><a class="reference external" href="https://scaron.info/doc/pymanoid/contact-stability.html#pymanoid.contact.Contact.wrench_inequalities">Contact.wrench_inequalities()</a> of <a class="reference external" href="https://github.com/stephane-caron/pymanoid/">pymanoid</a> (Python)</li>
<li><a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/blob/1faa0b7adec71acf8e2af9de934695ab16296fc3/include/lipm_walking/Stabilizer.h#L215">Stabilizer::wrenchFaceMatrix()</a> of the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">lipm_walking_controller</a> (C++)</li>
</ul>
</blockquote>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Capturability-based Pattern Generation for Walking with Variable Height2019-05-14T00:00:00+02:002019-05-14T00:00:00+02:00Stéphane Carontag:scaron.info,2019-05-14:/publications/tro-2019.html<p class="authors"><strong>Stéphane Caron</strong>, <a class="reference external" href="https://sites.google.com/site/adrienescandehomepage/">Adrien Escande</a>, <a class="reference external" href="http://www.diag.uniroma1.it/~lanari/">Leonardo Lanari</a> and <a class="reference external" href="http://www.math.univ-paris13.fr/~mallein/">Bastien Mallein</a>. IEEE Transactions on Robotics. Submitted January 2018. Accepted May 2019. Published April 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Capturability analysis of the linear inverted pendulum (LIP) model enabled walking with constrained height based on the <em>capture point</em>. We generalize this analysis to the variable-height inverted …</p></div><p class="authors"><strong>Stéphane Caron</strong>, <a class="reference external" href="https://sites.google.com/site/adrienescandehomepage/">Adrien Escande</a>, <a class="reference external" href="http://www.diag.uniroma1.it/~lanari/">Leonardo Lanari</a> and <a class="reference external" href="http://www.math.univ-paris13.fr/~mallein/">Bastien Mallein</a>. IEEE Transactions on Robotics. Submitted January 2018. Accepted May 2019. Published April 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Capturability analysis of the linear inverted pendulum (LIP) model enabled walking with constrained height based on the <em>capture point</em>. We generalize this analysis to the variable-height inverted pendulum (VHIP) and show how it enables 3D walking over uneven terrains based on <em>capture inputs</em>. Thanks to a tailored optimization scheme, we can compute these inputs fast enough for real-time model predictive control. We implement this approach as open-source software and demonstrate it in dynamic simulations.</p>
</div>
<div class="section" id="video">
<h2>Video</h2>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/tro-2019.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">Paper</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="/slides/ntu-2018.pdf">Presentation given at NTU</a> on 14 May 2018</td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/jrl-umi3218/CaptureProblemSolver">Capture Problem Solver (C++)</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/capture_walking_controller">Walking controller (C++)</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/capture-walkgen">Walking pattern generator (Python)</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td>Walking pattern generator manual: <a class="reference external" href="https://scaron.info/doc/capture-walkgen/">HTML</a> or <a class="reference external" href="https://scaron.info/doc/capture-walkgen/capture-walkgen.pdf">PDF</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/TRO.2019.2923971">10.1109/TRO.2019.2923971</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">caron2019tro</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Capturability-based Pattern Generation for Walking with Variable Height}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane and Escande, Adrien and Lanari, Leonardo and Mallein, Bastien}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Transactions on Robotics}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">apr</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{36}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{2}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{517--536}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01689331}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{IEEE}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/TRO.2019.2923971}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Prototyping a walking pattern generator2019-04-30T00:00:00+02:002019-04-30T00:00:00+02:00Stéphane Carontag:scaron.info,2019-04-30:/robot-locomotion/prototyping-a-walking-pattern-generator.html<p>Walking pattern generation is the problem of computing an ideal locomotion
trajectory, called the <em>walking pattern</em>. It is the first sub-problem in the
<a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">traditional walking control scheme</a>
for legged robots. A walking pattern is dynamically consistent (it respects the
laws of physics) and unperturbed (it assumes all controls are perfectly …</p><p>Walking pattern generation is the problem of computing an ideal locomotion
trajectory, called the <em>walking pattern</em>. It is the first sub-problem in the
<a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">traditional walking control scheme</a>
for legged robots. A walking pattern is dynamically consistent (it respects the
laws of physics) and unperturbed (it assumes all controls are perfectly
executed by the robot). In this post, we will see how to design a complete
walking pattern generator in <a class="reference external" href="https://github.com/stephane-caron/pymanoid/">pymanoid</a> using polynomial interpolation
for the swing leg and model predictive control for the center of mass.</p>
<div class="section" id="footstep-planning">
<h2>Footstep planning</h2>
<p>The goal of our walking pattern generator will be to make the robot walk
through a given sequence of contacts (in Python, a list of <a class="reference external" href="https://scaron.info/doc/pymanoid/contact-stability.html#pymanoid.contact.Contact">pymanoid.Contact</a>
objects). For walking forward, we can generate this sequence simply by placing
footsteps to the left and right of a line segment:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">pymanoid</span>
<span class="k">def</span> <span class="nf">generate_footsteps</span><span class="p">(</span><span class="n">distance</span><span class="p">,</span> <span class="n">step_length</span><span class="p">,</span> <span class="n">foot_spread</span><span class="p">):</span>
<span class="n">contacts</span> <span class="o">=</span> <span class="p">[]</span>
<span class="k">def</span> <span class="nf">append_contact</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
<span class="n">contacts</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">pymanoid</span><span class="o">.</span><span class="n">Contact</span><span class="p">(</span>
<span class="n">shape</span><span class="o">=</span><span class="p">(</span><span class="mf">0.11</span><span class="p">,</span> <span class="mf">0.05</span><span class="p">),</span> <span class="n">pos</span><span class="o">=</span><span class="p">[</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="mf">0.</span><span class="p">],</span> <span class="n">friction</span><span class="o">=</span><span class="mf">0.7</span><span class="p">))</span>
<span class="n">append_contact</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="o">+</span><span class="n">foot_spread</span><span class="p">)</span>
<span class="n">append_contact</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="o">-</span><span class="n">foot_spread</span><span class="p">)</span>
<span class="n">x</span> <span class="o">=</span> <span class="mf">0.</span>
<span class="n">y</span> <span class="o">=</span> <span class="n">foot_spread</span>
<span class="k">while</span> <span class="n">x</span> <span class="o"><</span> <span class="n">distance</span><span class="p">:</span>
<span class="k">if</span> <span class="n">distance</span> <span class="o">-</span> <span class="n">x</span> <span class="o"><=</span> <span class="n">step_length</span><span class="p">:</span>
<span class="n">x</span> <span class="o">+=</span> <span class="nb">min</span><span class="p">(</span><span class="n">distance</span> <span class="o">-</span> <span class="n">x</span><span class="p">,</span> <span class="mf">0.5</span> <span class="o">*</span> <span class="n">step_length</span><span class="p">)</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># still some way to go</span>
<span class="n">x</span> <span class="o">+=</span> <span class="n">step_length</span>
<span class="n">y</span> <span class="o">=</span> <span class="o">-</span><span class="n">y</span>
<span class="n">append_contact</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="n">append_contact</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="n">y</span><span class="p">)</span> <span class="c1"># now x == distance</span>
<span class="k">return</span> <span class="n">contacts</span>
</pre>
<p>The function depends on three parameters: <tt class="docutils literal">distance</tt>, the total distance in
meters to walk, <tt class="docutils literal">step_length</tt>, which is by definition the distance between
right and left heel in double support, and the lateral distance <tt class="docutils literal">foot_spread</tt>
between left and right foot centers. The first and last steps have half step
length so as to accelerate and decelerate more progressively. In what follows,
the robot will walk seven steps forward with a step length of 30 cm:</p>
<pre class="code python literal-block">
<span class="n">footsteps</span> <span class="o">=</span> <span class="n">generate_footsteps</span><span class="p">(</span>
<span class="n">distance</span><span class="o">=</span><span class="mf">2.1</span><span class="p">,</span>
<span class="n">step_length</span><span class="o">=</span><span class="mf">0.3</span><span class="p">,</span>
<span class="n">foot_spread</span><span class="o">=</span><span class="mf">0.1</span><span class="p">,</span>
<span class="n">friction</span><span class="o">=</span><span class="mf">0.7</span><span class="p">)</span>
</pre>
<p>The robot starts from a standing posture with its center of mass (COM) 85 cm
above ground, which is a comfortable height for the default JVRC-1 model. We
put the robot in this initial posture by solving the inverse kinematics (IK) of
our desired stance (a <a class="reference external" href="https://scaron.info/doc/pymanoid/inverse-kinematics.html#stance">pymanoid.Stance</a>):</p>
<pre class="code python literal-block">
<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s2">"__main__"</span><span class="p">:</span>
<span class="n">dt</span> <span class="o">=</span> <span class="mf">0.03</span> <span class="c1"># simulation time step, in [s]</span>
<span class="n">sim</span> <span class="o">=</span> <span class="n">pymanoid</span><span class="o">.</span><span class="n">Simulation</span><span class="p">(</span><span class="n">dt</span><span class="o">=</span><span class="n">dt</span><span class="p">)</span>
<span class="n">robot</span> <span class="o">=</span> <span class="n">pymanoid</span><span class="o">.</span><span class="n">robots</span><span class="o">.</span><span class="n">JVRC1</span><span class="p">(</span><span class="n">download_if_needed</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">set_viewer</span><span class="p">()</span>
<span class="n">stance</span> <span class="o">=</span> <span class="n">pymanoid</span><span class="o">.</span><span class="n">Stance</span><span class="p">(</span>
<span class="n">com</span><span class="o">=</span><span class="n">pymanoid</span><span class="o">.</span><span class="n">PointMass</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mf">0.85</span><span class="p">],</span> <span class="n">robot</span><span class="o">.</span><span class="n">mass</span><span class="p">),</span>
<span class="n">left_foot</span><span class="o">=</span><span class="n">footsteps</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>
<span class="n">right_foot</span><span class="o">=</span><span class="n">footsteps</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
<span class="n">stance</span><span class="o">.</span><span class="n">bind</span><span class="p">(</span><span class="n">robot</span><span class="p">)</span>
<span class="n">robot</span><span class="o">.</span><span class="n">ik</span><span class="o">.</span><span class="n">solve</span><span class="p">(</span><span class="n">max_it</span><span class="o">=</span><span class="mi">42</span><span class="p">)</span>
</pre>
<p>Our robot is now ready to walk:</p>
<img alt="Humanoid model JVRC-1 is now ready to walk" class="center max-height-400px" src="https://scaron.info/figures/walkgen-ready.png" />
</div>
<div class="section" id="walking-state-machine">
<h2>Walking state machine</h2>
<p>Walking alternates <em>double support phases</em> (DSP), where both feet are in
contact with the ground, and <em>single support phases</em> (SSP), where one foot (the
<em>swing foot</em>) is in the air while the robot supports its weight on the other
one:</p>
<img alt="Single and double support phases while walking" class="center" src="https://scaron.info/figures/walkgen-phases.png" />
<p>We can represent these phases in a <a class="reference external" href="https://en.wikipedia.org/wiki/Finite-state_machine">finite-state machine (FSM)</a> with three states,
standing, double support and single support, implemented as a <a class="reference external" href="https://scaron.info/doc/pymanoid/simulation-environment.html#processes">pymanoid.Process</a>:</p>
<pre class="code python literal-block">
<span class="k">class</span> <span class="nc">WalkingFSM</span><span class="p">(</span><span class="n">pymanoid</span><span class="o">.</span><span class="n">Process</span><span class="p">):</span>
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ssp_duration</span><span class="p">,</span> <span class="n">dsp_duration</span><span class="p">):</span>
<span class="nb">super</span><span class="p">(</span><span class="n">WalkingFSM</span><span class="p">,</span> <span class="bp">self</span><span class="p">)</span><span class="o">.</span><span class="fm">__init__</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span> <span class="o">=</span> <span class="n">dsp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">mpc_timestep</span> <span class="o">=</span> <span class="mi">3</span> <span class="o">*</span> <span class="n">dt</span> <span class="c1"># update MPC every 90 [ms] (see below)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">=</span> <span class="mi">2</span>
<span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span> <span class="o">=</span> <span class="n">ssp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="kc">None</span>
<span class="c1">#</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_standing</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">on_tick</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">sim</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"Standing"</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_standing</span><span class="p">()</span>
<span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"DoubleSupport"</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_double_support</span><span class="p">()</span>
<span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"SingleSupport"</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_single_support</span><span class="p">()</span>
<span class="k">raise</span> <span class="ne">Exception</span><span class="p">(</span><span class="s2">"Unknown state: "</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
</pre>
<p>The state machine will switch between single and double support phases based on
the pre-defined phase timings <tt class="docutils literal">ssp_duration</tt> and <tt class="docutils literal">dsp_duration</tt>.</p>
<div class="section" id="standing-state">
<h3>Standing state</h3>
<p>For each state, we define a "start" and "run" function. The standing state
simply waits for the user to set the <tt class="docutils literal">start_walking</tt> boolean attribute to
<tt class="docutils literal">True</tt>:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_standing</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_walking</span> <span class="o">=</span> <span class="kc">False</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="s2">"Standing"</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_standing</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">run_standing</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_walking</span><span class="p">:</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_walking</span> <span class="o">=</span> <span class="kc">False</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o"><</span> <span class="nb">len</span><span class="p">(</span><span class="n">footsteps</span><span class="p">):</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_double_support</span><span class="p">()</span>
</pre>
</div>
<div class="section" id="double-support-state">
<h3>Double support state</h3>
<p>For starters, the double support state simply waits for its phase duration, then
switches to single support:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_double_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">%</span> <span class="mi">2</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span> <span class="c1"># left foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">right_foot</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># right foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">left_foot</span>
<span class="n">dsp_duration</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">==</span> <span class="mi">2</span> <span class="ow">or</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">==</span> <span class="nb">len</span><span class="p">(</span><span class="n">footsteps</span><span class="p">)</span> <span class="o">-</span> <span class="mi">1</span><span class="p">:</span>
<span class="c1"># double support is a bit longer for the first and last steps</span>
<span class="n">dsp_duration</span> <span class="o">=</span> <span class="mi">4</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span> <span class="o">=</span> <span class="n">footsteps</span><span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span><span class="p">]</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">=</span> <span class="n">dsp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="s2">"DoubleSupport"</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_com_mpc_dsp</span><span class="p">()</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_double_support</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">run_double_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o"><=</span> <span class="mf">0.</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_single_support</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_com_mpc</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">-=</span> <span class="n">dt</span>
</pre>
<p>We use the start function of the state to save two important contact locations,
namely the stance foot and swing target of the next SSP. The reason for saving
them here rather than in the start function of the SSP itself will become clear
later on, when we implement the <tt class="docutils literal">start_com_mpc_dsp()</tt> and <tt class="docutils literal">run_com_mpc()</tt>
functions. For now, let us complete them with a dummy linear interpolation to
debug the development of our ongoing FSM:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_com_mpc_dsp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">pass</span> <span class="c1"># to be implemented later</span>
<span class="k">def</span> <span class="nf">run_com_mpc</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">set_x</span><span class="p">(</span><span class="mf">0.5</span> <span class="o">*</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span><span class="o">.</span><span class="n">x</span><span class="p">))</span>
</pre>
</div>
<div class="section" id="single-support-state">
<h3>Single support state</h3>
<p>The single support phase interpolates both COM and swing foot trajectories, and
updates stance targets accordingly:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_single_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">%</span> <span class="mi">2</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span> <span class="c1"># left foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">free_contact</span><span class="p">(</span><span class="s1">'left_foot'</span><span class="p">)</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># right foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">free_contact</span><span class="p">(</span><span class="s1">'right_foot'</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">+=</span> <span class="mi">1</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="s2">"SingleSupport"</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_swing_foot</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_com_mpc_ssp</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_single_support</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">run_single_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o"><=</span> <span class="mf">0.</span><span class="p">:</span>
<span class="n">stance</span><span class="o">.</span><span class="n">set_contact</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="p">)</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o"><</span> <span class="nb">len</span><span class="p">(</span><span class="n">footsteps</span><span class="p">):</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_double_support</span><span class="p">()</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># footstep sequence is over</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_standing</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_swing_foot</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_com_mpc</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">-=</span> <span class="n">dt</span>
</pre>
<p>For now, we apply dummy linear interpolations for both the COM and swing foot
trajectories:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_start</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">pose</span>
<span class="k">def</span> <span class="nf">start_com_mpc_ssp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">pass</span> <span class="c1"># to be implemented later</span>
<span class="k">def</span> <span class="nf">run_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="kn">from</span> <span class="nn">pymanoid.interp</span> <span class="kn">import</span> <span class="n">interpolate_pose_linear</span>
<span class="n">progress</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="mf">1.</span><span class="p">,</span> <span class="nb">max</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">/</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span><span class="p">))</span>
<span class="n">new_pose</span> <span class="o">=</span> <span class="n">interpolate_pose_linear</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_start</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">pose</span><span class="p">,</span> <span class="n">progress</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">set_pose</span><span class="p">(</span><span class="n">new_pose</span><span class="p">)</span>
</pre>
<p>When the phase time reaches the desired SSP duration, the state machine
switches back to double support and the process repeats until the robot has
traversed all footsteps in the sequence, finally switching back to the standing
state.</p>
</div>
<div class="section" id="scheduling-simulation-processes">
<h3>Scheduling simulation processes</h3>
<p>Although we haven't implemented neither swing foot nor COM trajectories yet, we
can already validate this code in simulation. Let us create an FSM process with
phase durations roughly 0.7/0.1 seconds for single and double support
respectively:</p>
<pre class="code python literal-block">
<span class="n">ssp_duration</span> <span class="o">=</span> <span class="mi">24</span> <span class="o">*</span> <span class="n">dt</span> <span class="c1"># 720 [ms]</span>
<span class="n">dsp_duration</span> <span class="o">=</span> <span class="mi">3</span> <span class="o">*</span> <span class="n">dt</span> <span class="c1"># 90 [ms]</span>
<span class="n">fsm</span> <span class="o">=</span> <span class="n">WalkingFSM</span><span class="p">(</span><span class="n">ssp_duration</span><span class="p">,</span> <span class="n">dsp_duration</span><span class="p">)</span>
</pre>
<p>As the FSM updates targets, we schedule it before inverse kinematics in the
simulation:</p>
<pre class="code python literal-block">
<span class="n">sim</span><span class="o">.</span><span class="n">schedule</span><span class="p">(</span><span class="n">fsm</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">ik</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">start</span><span class="p">()</span>
</pre>
<p>Let us throw in some extra drawer processes to keep track of the COM and swing
foot position targets while the robot moves:</p>
<pre class="code python literal-block">
<span class="n">com_traj_drawer</span> <span class="o">=</span> <span class="n">TrajectoryDrawer</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="p">,</span> <span class="s1">'b-'</span><span class="p">)</span>
<span class="n">lf_traj_drawer</span> <span class="o">=</span> <span class="n">TrajectoryDrawer</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">left_foot</span><span class="p">,</span> <span class="s1">'g-'</span><span class="p">)</span>
<span class="n">rf_traj_drawer</span> <span class="o">=</span> <span class="n">TrajectoryDrawer</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">right_foot</span><span class="p">,</span> <span class="s1">'r-'</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">com_traj_drawer</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">lf_traj_drawer</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">rf_traj_drawer</span><span class="p">)</span>
</pre>
<p>Now, the only thing left to do to start walking is:</p>
<pre class="code python literal-block">
<span class="n">In</span> <span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">fsm</span><span class="o">.</span><span class="n">start_walking</span> <span class="o">=</span> <span class="kc">True</span>
</pre>
<img alt="This simple finite-state machine makes the legged robot shuffle its feet" class="center max-height-400px" src="https://scaron.info/figures/walkgen-shuffle.png" />
<p>The robot shuffles its feet in a motion that is kinematically but not
dynamically consistent. We can see that by scheduling an extra wrench drawer to
check the dynamics of the simulation:</p>
<pre class="code python literal-block">
<span class="n">wrench_drawer</span> <span class="o">=</span> <span class="n">PointMassWrenchDrawer</span><span class="p">(</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="p">,</span> <span class="n">stance</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">wrench_drawer</span><span class="p">)</span>
</pre>
<p>This process draws feasible contact forces when it finds some, otherwise it
colors the background in red to show that the motion is not dynamically
feasible. This is what happens most of the time with our controller at this
point.</p>
</div>
</div>
<div class="section" id="swing-foot-interpolation">
<h2>Swing foot interpolation</h2>
<p>Our fixed footstep sequence provides initial and target configurations of the
swing foot for each SSP. For this first prototype, let us implement a "flat
foot" walk where the swing foot stays parallel to the ground. This strategy is
easier to implement, but on real robots it has a tendency to yield early
contacts and larger impacts than the heel-strike toe-off walk commonly applied
by humans.</p>
<p>The orientation of the swing foot being given, the main parameter to tune is
the vertical clearance of the trajectory. The default <a class="reference external" href="https://scaron.info/doc/pymanoid/walking-pattern-generation.html#pymanoid.swing_foot.SwingFoot">pymanoid.SwingFoot</a>
interpolates a <a class="reference external" href="https://en.wikipedia.org/wiki/Cubic_Hermite_spline">cubic Hermite spline</a> parameterized by a
takeoff clearance height <span class="math">\(a\)</span> and a landing clearance height <span class="math">\(b\)</span>:</p>
<img alt="Swing foot trajectory interpolation" class="center max-height-200px" src="https://scaron.info/images/swing-foot.png" />
<p>For us the ground contact normal <span class="math">\(\bfn\)</span> coincides with the world vertical
<span class="math">\(\bfe_z\)</span> in the figure above. The swing foot interpolator computes a
polynomial <span class="math">\(P(t)\)</span> such that:</p>
<ul class="simple">
<li><span class="math">\(P(0)\)</span> is the initial foot position,</li>
<li><span class="math">\(P(T_\textit{SSP})\)</span> is the target foot position, with <span class="math">\(T_\textit{SSP}\)</span> the duration of the single support phase,</li>
<li><span class="math">\(P(\frac{1}{4} T_\textit{SSP})\)</span> is at height greater than <span class="math">\(a\)</span> from the initial contact, and</li>
<li><span class="math">\(P(\frac{3}{4} T_\textit{SSP})\)</span> is at height greater than <span class="math">\(b\)</span> from the target contact.</li>
</ul>
<p>We can choose <span class="math">\(a = b = 5~\text{cm}\)</span> so that the foot detaches neatly from
the ground without causing too much vertical height variations of the COM.
First, let us instantiate the interpolator by:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_start</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">pose</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_interp</span> <span class="o">=</span> <span class="n">SwingFoot</span><span class="p">(</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="p">,</span> <span class="n">ssp_duration</span><span class="p">,</span>
<span class="n">takeoff_clearance</span><span class="o">=</span><span class="mf">0.05</span><span class="p">,</span> <span class="n">landing_clearance</span><span class="o">=</span><span class="mf">0.05</span><span class="p">)</span>
</pre>
<p>Second, we replace the dummy linear interpolation by the output from the swing
foot interpolator:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">run_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="n">new_pose</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_interp</span><span class="o">.</span><span class="n">integrate</span><span class="p">(</span><span class="n">dt</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">set_pose</span><span class="p">(</span><span class="n">new_pose</span><span class="p">)</span>
</pre>
<img alt="Now the legged robot swings its feet properly between footsteps" class="center max-height-400px" src="https://scaron.info/figures/walkgen-swing.png" />
<p>Now the robot properly lifts its feet in a motion that is kinematically but
still not dynamically consistent.</p>
</div>
<div class="section" id="center-of-mass-trajectory">
<h2>Center of mass trajectory</h2>
<p>The core problem in walking pattern generation is to make the motion
dynamically consistent, that is, to make sure that the <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a></p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \sum_{i=1}^N \calJ_{C_i}^{\top} \bfw_{C_i}
\end{equation*}
</div>
<p>have physically feasible joint torques <span class="math">\(\bftau\)</span> and contact wrenches
<span class="math">\(\bfw_{C_i} = [\bftau_{C_i}\ \bff_i]\)</span>.</p>
<div class="section" id="linear-inverted-pendulum-mode">
<h3>Linear inverted pendulum mode</h3>
<p>Assuming that the robot is powerful enough to execute the walking motion, we
can omit the lines that contain <span class="math">\(\bftau\)</span> and focus on the <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton-Euler
equations</a> that govern the
floating-base dynamics of the robot:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
m \bfpdd_G & = & m \bfg + \sum_i \bff_i \\
\dot{\bfL}_G & = & \sum_i (\bfp_{C_i} - \bfp_G) \times \bff_i + \bftau_{C_i}
\end{array}
\end{equation*}
</div>
<p>For this first walking pattern generator, we won't use rotations of the
upper-body and focus on producing the linear horizontal motion of the center of
mass. We rely for this purpose on the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum model</a>, keeping the COM at a
constant height <span class="math">\(h\)</span> as well as a constant angular momentum <span class="math">\(\bfL_G
= \boldsymbol{0}\)</span>. As a consequence, the Euler equation reduces to having the
resultant of contact forces going from the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point (ZMP)</a> to the COM, and the equation of
motion becomes:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \frac{g}{h} (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>where <span class="math">\(Z\)</span> denotes the ZMP.</p>
</div>
<div class="section" id="feasibility-of-contact-forces">
<h3>Feasibility of contact forces</h3>
<p>We want to make sure that, at every time instant, there exists feasible contact
wrenches <span class="math">\(\bfw_{C_i} = [\bftau_{C_i}\ \bff_i]\)</span> that lie in their <a class="reference external" href="https://scaron.info/robot-locomotion/wrench-friction-cones.html">wrench
friction cone</a>. In the linear inverted
pendulum mode, this is equivalent to having the ZMP inside its <a class="reference external" href="/publications/tro-2016.html">ZMP support
area</a>. This area is a convex polytope projection
in general, fortunately when walking on a flat floor with large friction it
reduces to the convex hull <span class="math">\(\cal C\)</span> of ground contact points:</p>
<div class="math">
\begin{equation*}
\bfp_Z = \bfp_G - \frac{h}{g} \bfpdd_G \in {\cal C}
\end{equation*}
</div>
</div>
<div class="section" id="linear-model-predictive-control">
<h3>Linear model predictive control</h3>
<p>Walking involves falling forward during single support phases, which is
instantaneously unstable and only becomes "stable" when considering the future
of the system (for instance, the next double support phase where the biped may
slow down to a stop if needed). As such, walking pattern generation is commonly
solved by <em>model predictive control</em> (MPC) where the COM trajectory is computed
over a <em>preview horizon</em> of one or two steps. This approach was initially
known as <a class="reference external" href="https://doai.io/10.1109/ROBOT.2003.1241826">preview control</a> and
reformulated with inequality constraints as <a class="reference external" href="https://doai.io/10.1109/ICHR.2006.321375">linear model predictive control</a>.</p>
<p>Let us focus on lateral COM motions (sagittal motions can be derived
similarly), and define the <em>state</em> of our system as <span class="math">\(\bfx = [y_G\
\dot{y}_G\ \ddot{y}_G]\)</span>. The MPC problem is then to generate a trajectory
<span class="math">\(y_G(t)\)</span> such that:</p>
<ul class="simple">
<li>at the current time <span class="math">\(t_0\)</span>, <span class="math">\(\bfx(t_0)\)</span> is equal to the current
state of the COM <span class="math">\([y_G(t_0)\ \dot{y}_G(t_0)\ \ddot{y}_G(t_0)]\)</span>;</li>
<li>at all times, the ZMP <span class="math">\(y_\textit{min}(t) \leq y_Z(\bfx(t)) \leq
y_\textit{max}(t)\)</span> where <span class="math">\(y_\textit{min}(t)\)</span> and
<span class="math">\(y_\textit{max}(t)\)</span> define the edges of the support area at time
<span class="math">\(t\)</span>;</li>
<li>at the end of the preview horizon <span class="math">\(t_f\)</span>, <span class="math">\(\bfx(t_f)\)</span> is equal to
a desired terminal state, for instance <span class="math">\([y_f\ 0\ 0]\)</span> where the robot
has stopped walking over the last footstep.</li>
</ul>
<p>We define <em>control</em> input as the COM jerk <span class="math">\(u = \dddot{y}_G\)</span>, and
discretize our system as:</p>
<div class="math">
\begin{equation*}
\bfx_{k+1} = \bfA \bfx_k + \bfB u_k
\end{equation*}
</div>
<p>where the state matrix <span class="math">\(\bfA\)</span> and control matrix <span class="math">\(\bfB\)</span> are defined
by:</p>
<div class="math">
\begin{equation*}
\bfA = \begin{bmatrix}
1 & T & \frac{T^2}{2} \\
0 & 1 & T \\
0 & 0 & 1
\end{bmatrix}
\quad
\bfB = \begin{bmatrix}
\frac{T^3}{6} \\
\frac{T^2}{2} \\
T
\end{bmatrix}
\end{equation*}
</div>
<p>The ZMP is obtained from the state as:</p>
<div class="math">
\begin{equation*}
y_Z(x_k) = \begin{bmatrix} 1 & 0 & -\frac{h}{g} \end{bmatrix} \bfx_k
\end{equation*}
</div>
<p>At every discretized time <span class="math">\(t_k\)</span>, we want the ZMP <span class="math">\(y_Z\)</span> to lie
between <span class="math">\(y_{\textit{min},k}\)</span> and <span class="math">\(y_{\textit{max},k}\)</span>. This can be
written as a linear matrix inequality on the state:</p>
<div class="math">
\begin{equation*}
\bfC \bfx_k \leq \bfe_k
\end{equation*}
</div>
<p>where:</p>
<div class="math">
\begin{equation*}
\bfC = \begin{bmatrix}
+1 & 0 & -\frac{h}{g} \\
-1 & 0 & +\frac{h}{g}
\end{bmatrix}
\quad
\bfe_k = \begin{bmatrix}
+y_{\textit{max},k} \\
-y_{\textit{min},k}
\end{bmatrix}
\end{equation*}
</div>
<p>We now have all the ingredients to build a linear model predictive control
problem. The discretized time step of our problem is <span class="math">\(T = 90\)</span> ms, and our
preview window will have 16 such steps:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">update_mpc</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">dsp_duration</span><span class="p">,</span> <span class="n">ssp_duration</span><span class="p">):</span>
<span class="n">nb_preview_steps</span> <span class="o">=</span> <span class="mi">16</span>
<span class="n">T</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">mpc_timestep</span>
<span class="n">nb_init_dsp_steps</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="nb">round</span><span class="p">(</span><span class="n">dsp_duration</span> <span class="o">/</span> <span class="n">T</span><span class="p">))</span>
<span class="n">nb_init_ssp_steps</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="nb">round</span><span class="p">(</span><span class="n">ssp_duration</span> <span class="o">/</span> <span class="n">T</span><span class="p">))</span>
<span class="n">nb_dsp_steps</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="nb">round</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span> <span class="o">/</span> <span class="n">T</span><span class="p">))</span>
</pre>
<p>Our preview consists of an initial DSP of duration <tt class="docutils literal">dsp_duration</tt> (can be
zero), followed by an SSP of duration <tt class="docutils literal">ssp_duration</tt>, followed by a DSP of
regular duration <tt class="docutils literal">self.dsp_duration</tt>, followed by a second SSP lasting until
the preview horizon. We build the matrices of the problem following the
derivation above:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="c1"># def update_mpc(self, dsp_duration, ssp_duration):</span>
<span class="n">A</span> <span class="o">=</span> <span class="n">array</span><span class="p">([[</span><span class="mf">1.</span><span class="p">,</span> <span class="n">T</span><span class="p">,</span> <span class="n">T</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">/</span> <span class="mf">2.</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">,</span> <span class="n">T</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">]])</span>
<span class="n">B</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="n">T</span> <span class="o">**</span> <span class="mi">3</span> <span class="o">/</span> <span class="mf">6.</span><span class="p">,</span> <span class="n">T</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">/</span> <span class="mf">2.</span><span class="p">,</span> <span class="n">T</span><span class="p">])</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">1</span><span class="p">))</span>
<span class="n">h</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">z</span>
<span class="n">g</span> <span class="o">=</span> <span class="o">-</span><span class="n">sim</span><span class="o">.</span><span class="n">gravity</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span>
<span class="n">zmp_from_state</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="o">-</span><span class="n">h</span> <span class="o">/</span> <span class="n">g</span><span class="p">])</span>
<span class="n">C</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="n">zmp_from_state</span><span class="p">,</span> <span class="o">-</span><span class="n">zmp_from_state</span><span class="p">])</span>
<span class="n">D</span> <span class="o">=</span> <span class="kc">None</span>
<span class="n">e</span> <span class="o">=</span> <span class="p">[[],</span> <span class="p">[]]</span>
<span class="n">cur_vertices</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span><span class="o">.</span><span class="n">get_scaled_contact_area</span><span class="p">(</span><span class="mf">0.8</span><span class="p">)</span>
<span class="n">next_vertices</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">get_scaled_contact_area</span><span class="p">(</span><span class="mf">0.8</span><span class="p">)</span>
<span class="k">for</span> <span class="n">coord</span> <span class="ow">in</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]:</span>
<span class="n">cur_max</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">cur_vertices</span><span class="p">)</span>
<span class="n">cur_min</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">cur_vertices</span><span class="p">)</span>
<span class="n">next_max</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">next_vertices</span><span class="p">)</span>
<span class="n">next_min</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">next_vertices</span><span class="p">)</span>
<span class="n">e</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="o">=</span> <span class="p">[</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="mf">1000.</span><span class="p">,</span> <span class="o">+</span><span class="mf">1000.</span><span class="p">])</span> <span class="k">if</span> <span class="n">i</span> <span class="o"><</span> <span class="n">nb_init_dsp_steps</span> <span class="k">else</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="n">cur_max</span><span class="p">,</span> <span class="o">-</span><span class="n">cur_min</span><span class="p">])</span> <span class="k">if</span> <span class="n">i</span> <span class="o">-</span> <span class="n">nb_init_dsp_steps</span> <span class="o"><=</span> <span class="n">nb_init_ssp_steps</span> <span class="k">else</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="mf">1000.</span><span class="p">,</span> <span class="o">+</span><span class="mf">1000.</span><span class="p">])</span> <span class="k">if</span> <span class="n">i</span> <span class="o">-</span> <span class="n">nb_init_dsp_steps</span> <span class="o">-</span> <span class="n">nb_init_ssp_steps</span> <span class="o"><</span> <span class="n">nb_dsp_steps</span> <span class="k">else</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="n">next_max</span><span class="p">,</span> <span class="o">-</span><span class="n">next_min</span><span class="p">])</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">nb_preview_steps</span><span class="p">)]</span>
</pre>
<p>Here, we construct two lists of vectors <span class="math">\(\bfe = [\bfe_0\ \bfe_1\ \ldots]\)</span>
in parallel: <tt class="docutils literal">e[0]</tt> for the sagittal direction and <tt class="docutils literal">e[1]</tt> for the lateral
direction. We put constraints on the ZMP during single support phases based on
the vertices of the support area (the area is scaled by <tt class="docutils literal">0.9</tt> to make sure
that the ZMP stays well inside). Finally, we build and solve MPC problems for
both directions using the <a class="reference external" href="https://scaron.info/doc/pymanoid/walking-pattern-generation.html#pymanoid.mpc.LinearPredictiveControl">pymanoid.LinearPredictiveControl class</a>:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="c1"># def update_mpc(self, dsp_duration, ssp_duration):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">x_mpc</span> <span class="o">=</span> <span class="n">LinearPredictiveControl</span><span class="p">(</span>
<span class="n">A</span><span class="p">,</span> <span class="n">B</span><span class="p">,</span> <span class="n">C</span><span class="p">,</span> <span class="n">D</span><span class="p">,</span> <span class="n">e</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>
<span class="n">x_init</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">xd</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">xdd</span><span class="p">]),</span>
<span class="n">x_goal</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">]),</span>
<span class="n">nb_steps</span><span class="o">=</span><span class="n">nb_preview_steps</span><span class="p">,</span>
<span class="n">wxt</span><span class="o">=</span><span class="mf">1.</span><span class="p">,</span> <span class="n">wu</span><span class="o">=</span><span class="mf">0.01</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">y_mpc</span> <span class="o">=</span> <span class="n">LinearPredictiveControl</span><span class="p">(</span>
<span class="n">A</span><span class="p">,</span> <span class="n">B</span><span class="p">,</span> <span class="n">C</span><span class="p">,</span> <span class="n">D</span><span class="p">,</span> <span class="n">e</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
<span class="n">x_init</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">yd</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">ydd</span><span class="p">]),</span>
<span class="n">x_goal</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">]),</span>
<span class="n">nb_steps</span><span class="o">=</span><span class="n">nb_preview_steps</span><span class="p">,</span>
<span class="n">wxt</span><span class="o">=</span><span class="mf">1.</span><span class="p">,</span> <span class="n">wu</span><span class="o">=</span><span class="mf">0.01</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">x_mpc</span><span class="o">.</span><span class="n">solve</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">y_mpc</span><span class="o">.</span><span class="n">solve</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">preview_time</span> <span class="o">=</span> <span class="mf">0.</span>
</pre>
<p>We use this <tt class="docutils literal">update_mpc()</tt> function to initialize these problems at the
beginning of double and single support phases:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_com_mpc_dsp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span><span class="p">)</span>
<span class="k">def</span> <span class="nf">start_com_mpc_ssp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">)</span>
</pre>
<p>Finally, in the <tt class="docutils literal">run_com_mpc()</tt> function we update the COM by integrating
jerk outputs from model predictive control:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">run_com_mpc</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">preview_time</span> <span class="o">>=</span> <span class="bp">self</span><span class="o">.</span><span class="n">mpc_timestep</span><span class="p">:</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"DoubleSupport"</span><span class="p">:</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span><span class="p">)</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># self.state == "SingleSupport":</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">)</span>
<span class="n">com_jerk</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">x_mpc</span><span class="o">.</span><span class="n">U</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">y_mpc</span><span class="o">.</span><span class="n">U</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="mf">0.</span><span class="p">])</span>
<span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">integrate_constant_jerk</span><span class="p">(</span><span class="n">com_jerk</span><span class="p">,</span> <span class="n">dt</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">preview_time</span> <span class="o">+=</span> <span class="n">dt</span>
</pre>
<p>The <tt class="docutils literal">preview_time</tt> variable is used to compute new predictive solutions after
we are done integrating their first timestep. This is one of the key concepts
in MPC: we only execute the first control of the output trajectory, and
recompute that trajectory when this is done.</p>
<p>Dynamic constraints on the ZMP naturally produce a center of mass motion that
sways laterally:</p>
<img alt="Humanoid model JVRC-1 walks dynamically" class="center max-height-400px" src="https://scaron.info/figures/walkgen-com.png" />
<p>Now our robot model is properly walking with a motion that is both
kinematically and dynamically consistent. You can check out the complete
walking controller in the <a class="reference external" href="https://github.com/stephane-caron/pymanoid/blob/18b75807d679630752ea038536226fd46d6e8691/examples/horizontal_walking.py">horizontal_walking.py</a>
example in pymanoid.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The concepts we have seen in this introduction are used in a <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">full-fledged
walking controller</a> developed for
walking and stair climbing with the HRP-4 humanoid.</p>
<p>Our model predictive control problem is reduced to the strict minimum: dynamic
constraints on the ZMP and a terminal condition on the COM state. We also
simplified things a bit by decoupling the x-axis and y-axis MPC, which is only
valid when footstep edges are aligned with the axes of the inertial frame. You
can find a more advanced implementation of this pattern generation method in
the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/blob/58c08d1cb36697cc487de620beb6efe59d40857f/src/ModelPredictiveControl.cpp">ModelPredictiveControl.cpp</a>
source of the controller: it adds general footstep orientations, velocity and
jerk terms to the cost function, as well as a (strict) terminal condition on a
<a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent component of motion</a>. Those are
implemented using the <a class="reference external" href="https://github.com/vsamy/copra/">Copra</a> linear MPC
library.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Floating base estimation2019-04-13T00:00:00+02:002019-04-13T00:00:00+02:00Stéphane Carontag:scaron.info,2019-04-13:/robot-locomotion/floating-base-estimation.html<p>For a legged system, locomotion is about controlling the <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#case-of-mobile-robots">unactuated floating
base</a> to a desired
location in the world. Before a control action can be applied, the first thing
is to estimate the position and orientation of this floating base. In what
follows, we will see the simple <em>anchor point …</em></p><p>For a legged system, locomotion is about controlling the <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#case-of-mobile-robots">unactuated floating
base</a> to a desired
location in the world. Before a control action can be applied, the first thing
is to estimate the position and orientation of this floating base. In what
follows, we will see the simple <em>anchor point</em> solution, which is a good way to
get started and works well in practice. For example, it is the one used in
<a class="reference external" href="https://www.youtube.com/watch?v=vFCFKAunsYM&t=22">this stair climbing experiment</a>.</p>
<div class="section" id="orientation-of-the-floating-base">
<h2>Orientation of the floating base</h2>
<p>We assume that our legged robot is equipped with an inertial measurement unit
(IMU) in its pelvis link, and attach the base frame <span class="math">\(B\)</span> of our floating
base to the same link. The IMU contains gyroscopes, providing angular velocity
measurements <span class="math">\({}^B\bfomega_B\)</span>, and accelerometers, providing linear
acceleration measurements <span class="math">\({}^B\bfa\)</span> that include both gravity and linear
accelerations caused by robot motions. These measurements can be used to
estimate the orientation <span class="math">\({}^W\bfR_B\)</span> of the floating base with respect
to an inertial world frame <span class="math">\(W\)</span>, but only partially. Let us outline how
this process works.</p>
<div class="section" id="integration-of-angular-velocities">
<h3>Integration of angular velocities</h3>
<p>Define the world frame as the initial floating base of the system, so that
<span class="math">\(\bfR^i_0 := {}^W\bfR^i_B = \bfI_3\)</span> with <span class="math">\(\bfI_3\)</span> the <span class="math">\(3
\times 3\)</span> identity matrix. At discrete time <span class="math">\(t_k\)</span>, gyroscopes measure the
angular velocity <span class="math">\({}^B \bfomega_B(t_k)\)</span> of the floating base <span class="math">\(B\)</span>
relative to the inertial frame, expressed in <span class="math">\(B\)</span>. The discrete series
<span class="math">\(\bfomega_k := {}^B \bfomega_B(t_k)\)</span> of angular velocity measurements can
be integrated to get:</p>
<div class="math">
\begin{equation*}
\bfR^i_{k+1} = \bfR^i_{k} \exp([\bfomega_k \times] \Delta t)
\end{equation*}
</div>
<p>with <span class="math">\(\Delta t = t_k - t_{k-1}\)</span> the duration between two measurements and
<span class="math">\([\bfomega_k \times]\)</span> the skew-symmetric matrix formed from the angular
velocity vector. In practice the matrix exponential can be expanded by
<a class="reference external" href="https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula">Rodrigues' rotation formula</a>:</p>
<div class="math">
\begin{equation*}
\bfR^i_{k+1} = \bfR^i_{k} \left(\bfI_3 + \frac{\sin \theta_k}{\theta_k} [\bfomega_k \times] + \frac{1 - \cos \theta_k}{\theta_k^2} [\bfomega_k \times]^2 \right)
\end{equation*}
</div>
<p>with <span class="math">\(\theta_k = \| \omega_k \| \Delta t\)</span>. This integration scheme gives
us a first estimate of the orientation of the floating base over time, however
it quickly drifts in practice because gyroscopes have a (time-varying)
measurement bias.</p>
</div>
<div class="section" id="accelerometer-based-estimation">
<h3>Accelerometer-based estimation</h3>
<p>Accelerometers can be used to provide an alternative estimate of the floating
base orientation. When the robot is not moving, what these accelerometers see
on average is the gravity vector <span class="math">\({}^B\bfg\)</span> expressed in <span class="math">\(B\)</span>. As
the gravity vector <span class="math">\({}^W\bfg \approx [0 \ 0 \ {-9.81}]\)</span> in the inertial
frame is known, we can estimate <span class="math">\(\bfR^g =
{}^W \bfR_B\)</span> as <a class="reference external" href="https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/476311#476311">the rotation matrix that sends</a>
<span class="math">\({}^B \bfg\)</span> onto <span class="math">\({}^W \bfg\)</span>. Let us denote by <span class="math">\({}^B \bfu\)</span> and
<span class="math">\({}^W \bfu\)</span> the normalized vectors from <span class="math">\({}^B \bfg\)</span> and <span class="math">\({}^W
\bfg\)</span> respectively:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfv & = & {}^B \bfu \times {}^W \bfu \\
c & = & {}^B \bfu \cdot {}^W \bfu \\
\bfR^g & = & \bfI_3 + [\bfv \times] + \frac{1}{1 + c} [\bfv \times]^2
\end{array}
\end{equation*}
</div>
<p>This formula performs well to estimate the <em>roll</em> and <em>pitch</em> angles of the
rotation <span class="math">\({}^W \bfR_B\)</span> of the floating base, however it cannot estimate
the third <em>yaw</em> component because the gravity vector is invariant by rotation
around the yaw axis. Basically, at our levels of perception, gravity is felt
the same whether you face north, east or south.</p>
</div>
<div class="section" id="drift-compensation-of-the-gyroscope-bias">
<h3>Drift compensation of the gyroscope bias</h3>
<p>The drawbacks of these two estimates fortunately appear at different
frequencies: angular-velocity integration drifts in the long run,
accelerometer-based estimation is perturbed in the short run as the robot
moves. A common solution is then to rely mostly on the former, compensating its
drift by low-frequency feedback of the latter. On the HRP-4 humanoid, this
solution is implemented in an extended Kalman filter that runs on the robot's
embedded computer. It can also be realized by a complementary filter such as
<a class="reference external" href="https://forums.parallax.com/uploads/attachments/41167/106661.pdf">(Madgwick, 2010)</a>, which
provides remarkable C source code in its Appendix.</p>
</div>
</div>
<div class="section" id="translation-of-the-floating-base">
<h2>Translation of the floating base</h2>
<p>Now that we have estimated the orientation <span class="math">\({}^W\bfR_B\)</span> of the floating
base with respect to the world frame, let us estimate its translation
<span class="math">\({}^W \bfp_B\)</span> using the anchor point strategy. Its idea is summed up in
the following figure:</p>
<img alt="Illustration of anchor-point floating-base translation estimation" class="align-center max-height-500px" src="https://scaron.info/figures/anchor-point.png" />
<p>We select an anchor point <span class="math">\(A\)</span> at a fixed location on the foot sole, for
instance the point closest to the ankle frame (a pertinent choice for Honda and
HRP robots to limit deflection errors caused by their rubber foot flexibility),
and assume that this point is in contact at the position <span class="math">\({}^W \bfp_A\)</span> on
the ground where it is planned to be in the world frame. In the floating-base
frame, the position <span class="math">\({}^B \bfp_A\)</span> of this point is known by forward
kinematics. From the anchor-point assumption, we can then compute the
floating-base translation by:</p>
<div class="math">
\begin{equation*}
{}^W \bfp_A = {}^W \bfp_B + {}^W \bfR_B {}^B \bfp_A \ \Longrightarrow
\ {}^W \bfp_B = {}^W \bfp_A - {}^W \bfR_B {}^B \bfp_A
\end{equation*}
</div>
<p>This estimator is simple to implement and does not require additional
parameters. While walking, the anchor point can be chosen (1) closest to the
ankle frame during single-support phases and (2) continuously varying from one
ankle frame to the other during double-support phases. Continuous variations
help avoid discontinuities, which are generally undesirable in a controller.</p>
<p>While simple to implement, this estimator performs quite well in practice. Here
is for example a comparison to more precise VICON measurements ran while HRP-4
was stepping in place:</p>
<img alt="Precision of the observer compared to more precise VICON measurements" class="align-center max-height-450px" src="https://scaron.info/figures/floating-base-vicon.png" />
<p>In this example, the estimation error in the lateral direction was at most 0.8
cm.</p>
</div>
<div class="section" id="divergent-component-of-motion">
<h2>Divergent component of motion</h2>
<p>For <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html#walking-stabilization">walking stabilization</a>, the
controller may need to estimate not only the floating base position but also
the divergent component of motion (DCM) <span class="math">\(\bfxi\)</span>, which depends on the
center of mass (CoM) position <span class="math">\(\bfc\)</span> and velocity <span class="math">\(\bfcd\)</span> by:</p>
<div class="math">
\begin{equation*}
\bfxi = \bfc + \frac{\bfcd}{\omega}
\end{equation*}
</div>
<p>where <span class="math">\(\omega = \sqrt{g / h}\)</span> is a known constant. For a straightforward
solution, the CoM position can be derived from the floating base and joint
encoder measurements by forward kinematics. Its velocity is then obtained via a
first-order low-pass filter with cutoff period <span class="math">\(T_\textit{cutoff} =
10~\text{ms}\)</span>:</p>
<div class="math">
\begin{equation*}
\alpha = \frac{\Delta t}{T_\textit{cutoff}}
\ \Longrightarrow \
\bfcd_{k+1} = \alpha \frac{\bfc_{k+1} - \bfc_k}{\Delta t} + (1 - \alpha) \bfcd_k
\end{equation*}
</div>
<p>where <span class="math">\(\Delta t\)</span> is the duration of a control cycle. The DCM is finally
computed from the CoM position <span class="math">\(\bfc_k\)</span> and velocity <span class="math">\(\bfcd_k\)</span>.
While simple to implement, this scheme is the one we applied in a <a class="reference external" href="/publications/icra-2019.html">walking and
stair climbing controller</a> for the HRP-4
humanoid.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Estimating the floating base orientation from IMU measurements is a key
component in the above process, of which we only gave the outline. To go
further, you can take a look at Sections III and V of <a class="reference external" href="http://rpg.ifi.uzh.ch/docs/TRO16_forster.pdf">(Forster et al., 2016)</a>.</p>
<div class="section" id="kalman-and-complementary-filters">
<h3>Kalman and complementary filters</h3>
<p>There is significant literature on the topic of floating base estimation, and
more generally <em>state estimation</em> for legged robots. One way is to design a
Kalman or complementary filter. The following open source libraries follow this
approach:</p>
<ul class="simple">
<li><a class="reference external" href="https://github.com/ori-drs/pronto-distro">Pronto</a>: an extended Kalman
filter for floating base estimation, used by the MIT DRC team during the
DARPA Robotics Challenge.</li>
<li><a class="reference external" href="https://github.com/mrsp/serow">SEROW</a>: a Kalman filter for floating base
and CoM position and velocity estimation.</li>
<li><a class="reference external" href="https://github.com/mehdi-benallegue/state-observation">state-observation</a>:
a collection of Kalman filters based on different contact assumptions.</li>
</ul>
</div>
<div class="section" id="numerical-optimization">
<h3>Numerical optimization</h3>
<p>Another approach relies on <a class="reference external" href="https://doi.org/10.1109/IROS.2014.6942679">quadratic programming to minimize model and
measurement errors</a>. Quadratic
programming can also be used to <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01574819/document">fuse simple estimators with time-varying
weights</a>. The
<a class="reference external" href="https://github.com/jrl-umi3218/SpringEstimator">SpringEstimator</a> library
follows this approach. It applies a nonlinear hierarchical optimization that
can estimate multiple compliant links in contact. Technical details are
provided <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01276931/document">around Fig. 8 of this paper</a>.</p>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>How do biped robots walk?2019-03-27T00:00:00+01:002019-03-27T00:00:00+01:00Stéphane Carontag:scaron.info,2019-03-27:/talks/willow-2019.html<p class="authors">Talk given at the <a class="reference external" href="https://www.di.ens.fr/willow/">WILLOW team seminar</a>,
Computer Science Department of the École Normale Supérieure de Paris (DI ENS),
on 26 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this introductory talk, we will see the general methods used nowadays to
make humanoid robots walk or climb stairs. Predictive and feedback controls
will be …</p></div><p class="authors">Talk given at the <a class="reference external" href="https://www.di.ens.fr/willow/">WILLOW team seminar</a>,
Computer Science Department of the École Normale Supérieure de Paris (DI ENS),
on 26 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this introductory talk, we will see the general methods used nowadays to
make humanoid robots walk or climb stairs. Predictive and feedback controls
will be presented from a high-level perspective, with more videos than
equations. At its end, our bird-eye trip will leave us with open questions, for
instance: what is missing in our robots' brains today? The audience is
encouraged to bring lightweight plastic bottles.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/wandercraft-2019.pdf">Slides</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="/teaching/how-do-biped-robots-walk.html">Introduction to bipedal walking control</a></td>
</tr>
<tr><td><img alt="youtube" class="icon" src="https://scaron.info/images/icons/youtube.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=T7SEyvPtWFY">Documentary from the NHK on ASIMO</a></td>
</tr>
</tbody>
</table>
</div>
Walking and stair climbing stabilization for position-controlled biped robots2019-03-19T00:00:00+01:002019-03-19T00:00:00+01:00Stéphane Carontag:scaron.info,2019-03-19:/talks/wandercraft-2019.html<p class="authors">Talk given at <a class="reference external" href="https://www.wandercraft.eu/">Wandercraft</a> on 19 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will see the walking and stair climbing stabilizer implemented
on the HRP-4 humanoid robot as part of the final demonstrator for an Airbus
manufacturing use case. Our solution is based on DCM feedback control,
quadratic-programming wrench distribution …</p></div><p class="authors">Talk given at <a class="reference external" href="https://www.wandercraft.eu/">Wandercraft</a> on 19 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will see the walking and stair climbing stabilizer implemented
on the HRP-4 humanoid robot as part of the final demonstrator for an Airbus
manufacturing use case. Our solution is based on DCM feedback control,
quadratic-programming wrench distribution and a whole-body admittance
controller that combines three kinds of tasks: single-effector CoP control at
each foot, multi-effector foot pressure control during double-support phases,
and CoM admittance control.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/wandercraft-2019.pdf">Slides</a></td>
</tr>
<tr><td><img alt="youtube" class="icon" src="https://scaron.info/images/icons/youtube.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=vFCFKAunsYM">Stair climbing experiment</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Stair climbing stabilization of the HRP-4 humanoid robot using whole-body admittance control</a></td>
</tr>
</tbody>
</table>
</div>
Stair Climbing Stabilization of the HRP-4 Humanoid Robot2018-12-11T00:00:00+01:002018-12-11T00:00:00+01:00Stéphane Carontag:scaron.info,2018-12-11:/talks/jrl-2018.html<p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 11 December 2018 and at the <strong>University of Tokyo</strong> on 14 December 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will discuss stair climbing experiments carried out with the
HRP-4 humanoid as part of an Airbus manufacturing use case. Our solution is …</p></div><p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 11 December 2018 and at the <strong>University of Tokyo</strong> on 14 December 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will discuss stair climbing experiments carried out with the
HRP-4 humanoid as part of an Airbus manufacturing use case. Our solution is
based on linear inverted pendulum tracking, quadratic-programming wrench
distribution and a whole-body admittance controller that relies on both
end-effector and CoM strategies. After presenting these components, we will
open up the discussion on the interplay between end-effector and CoM
strategies, as well as their potential applications in multi-contact.</p>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/vFCFKAunsYM?start=22" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/jrl-2018.pdf">Slides</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Stair climbing experiments with HRP-4</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">Walking pattern generation with height variations</a></td>
</tr>
</tbody>
</table>
</div>
Converting robot models to OpenRAVE2018-11-03T00:00:00+01:002018-11-03T00:00:00+01:00Stéphane Carontag:scaron.info,2018-11-03:/robot-locomotion/converting-robot-models-to-openrave.html<p>OpenRAVE robot models can be written in <a class="reference external" href="http://openrave.programmingvision.com/wiki/index.php/Format:XML">OpenRAVE Custom XML Format</a>, if you
write them from scratch, but are most of the time written in the <a class="reference external" href="http://openrave.org/docs/latest_stable/collada_robot_extensions/">COLLADA 1.5
Robot Extensions</a>. This
format is not so widely used and converting to it can be tricky. In this post,
we will …</p><p>OpenRAVE robot models can be written in <a class="reference external" href="http://openrave.programmingvision.com/wiki/index.php/Format:XML">OpenRAVE Custom XML Format</a>, if you
write them from scratch, but are most of the time written in the <a class="reference external" href="http://openrave.org/docs/latest_stable/collada_robot_extensions/">COLLADA 1.5
Robot Extensions</a>. This
format is not so widely used and converting to it can be tricky. In this post,
we will review the conversion paths I've found to generate the humanoid robot
models in the <a class="reference external" href="https://github.com/stephane-caron/openrave_models">openrave_models</a> repository.</p>
<div class="section" id="from-vrml-to-collada-1-5">
<h2>From VRML to COLLADA 1.5</h2>
<p>The <a class="reference external" href="https://github.com/jvrc/model">JVRC-1 model</a>, similarly to robot models
from the HRP series, is provided in VRML format. To convert it to COLLADA, you
can use the <tt class="docutils literal"><span class="pre">openhrp-export-collada</span></tt> script provided in <a class="reference external" href="https://github.com/fkanehiro/openhrp3">OpenHRP 3</a>. After the regular CMake build
process, you will find it in the <tt class="docutils literal">bin</tt> folder of your build directory:</p>
<pre class="code bash literal-block">
<span class="nb">cd</span> <openhrp3-folder>
./build/bin/openhrp-export-collada -i main.wrl -o JVRC-1.dae
</pre>
<p>Textures are not converted in this process. To palliate this, you can open the
COLLADA file in a text editor (COLLADA is still XML so it can be read and
edited by human beings) and change link colors by hand. You can check out
<a class="reference external" href="https://github.com/stephane-caron/openrave_models/blob/master/JVRC-1/JVRC-1.dae">JVRC-1.dae</a>
to see how this is done.</p>
<img alt="JVRC-1 humanoid model in OpenRAVE" class="noborder gui align-center" src="https://scaron.info/images/openrave/jvrc-1.png" />
<p>More recently, the authors of OpenHRP have also developed <a class="reference external" href="https://github.com/fkanehiro/simtrans">simtrans</a> which is a more general set of
conversion scripts.</p>
</div>
<div class="section" id="from-urdf-to-collada-1-5">
<h2>From URDF to COLLADA 1.5</h2>
<p>Many robot models are provided in another XML dialect called Unified Robot
Description Format (URDF). This was the case for instance with the <a class="reference external" href="https://github.com/ros-aldebaran/romeo_robot">Romeo</a> model. To convert it to
COLLADA, you can use the <tt class="docutils literal">urdf_to_collada</tt> tool provided in <a class="reference external" href="http://wiki.ros.org/collada_urdf">collada_urdf</a>. After the package is installed in your
ROS distribution, run:</p>
<pre class="code bash literal-block">
rosrun collada_urdf urdf_to_collada romeo.urdf Romeo.dae
</pre>
<p>Textures are not converted in this process. To palliate this, you can open the
COLLADA file in a text editor (COLLADA is still XML so it can be read and
edited by human beings) and change link colors by hand. You can check out
<a class="reference external" href="https://github.com/stephane-caron/openrave_models/blob/master/Romeo/Romeo.dae">Romeo.dae</a>
to see how this is done.</p>
<img alt="Romeo humanoid model in OpenRAVE" class="noborder gui align-center" src="https://scaron.info/images/openrave/romeo.png" />
<p>For a more recent tool, the <a class="reference external" href="https://github.com/fkanehiro/simtrans">simtrans</a>
conversion scripts provide a less burdensome alternative</p>
</div>
How do biped robots walk?2018-10-26T00:00:00+02:002018-10-26T00:00:00+02:00Stéphane Carontag:scaron.info,2018-10-26:/robot-locomotion/how-do-biped-robots-walk.html<p>Walking has been realized on biped robots since the 1970s and 1980s, but a
major stride in the field came in 1996 when Honda unveiled its <a class="reference external" href="https://en.wikipedia.org/wiki/Honda_P_series">P2 humanoid
robot</a> which would later become
ASIMO. It was already capable of <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=26">walking</a>, <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=119">pushing a cart</a> and <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=172">climb stairs</a>. A key point in …</p><p>Walking has been realized on biped robots since the 1970s and 1980s, but a
major stride in the field came in 1996 when Honda unveiled its <a class="reference external" href="https://en.wikipedia.org/wiki/Honda_P_series">P2 humanoid
robot</a> which would later become
ASIMO. It was already capable of <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=26">walking</a>, <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=119">pushing a cart</a> and <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=172">climb stairs</a>. A key point in the design of P2 was its
walking control based on feedback of the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point (ZMP)</a>. Let us look at the working
assumptions and components behind it.</p>
<p>If this is your first time reading this page, I warmly advise you watch this
<a class="reference external" href="https://www.youtube.com/watch?v=5cuVsTf_P8U">excellent documentary from the NHK on ASIMO</a>. It does a very good job at
explaining some of the key concepts that we are going to define more formally
below.</p>
<div class="section" id="linear-inverted-pendulum-model">
<h2>Linear inverted pendulum model</h2>
<p>The common model for (fixed or mobile) robots consists of multiple rigid bodies
connected by actuated joints. The general <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equation of motion</a> for such a system are high-dimensional,
but they can be reduced using three working assumptions:</p>
<ul class="simple">
<li><em>Assumption 1:</em> the robot has enough joint torques to realize its motions.</li>
<li><em>Assumption 2:</em> there is no angular momentum around the center of mass (CoM).</li>
<li><em>Assumption 3:</em> the center of mass keeps a constant height.</li>
</ul>
<p>Assumptions 2 and 3 explain why you see the Honda P2 <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=172">walk with locked arms and
bent knees</a>. Under these three
assumptions, the equations of motion of the walking biped are <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">reduced to a
linear model</a>, the <em>linear
inverted pendulum</em>:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \omega^2 (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>where <span class="math">\(\omega^2 = g / h\)</span>, <span class="math">\(g\)</span> is the gravity constant, <span class="math">\(h\)</span> is
the CoM height and <span class="math">\(\bfp_Z\)</span> is the position of the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment
point (ZMP)</a>. The constant
<span class="math">\(\omega\)</span> is called the <em>natural frequency</em> of the linear inverted
pendulum. In this model, the robot can be seen as a point-mass concentrated at
<span class="math">\(G\)</span> resting on a massless leg in contact with the ground at <span class="math">\(Z\)</span>.
Intuitively, the ZMP is the point where the robot applies its weight. As a
consequence, this point needs to lie inside the contact surface <span class="math">\(\cal S\)</span>.</p>
<img alt="Humanoid robot walking in the linear inverted pendulum mode" class="center max-height-350px" src="https://scaron.info/figures/lipm.png" />
<p>To walk, the robot shifts its ZMP backward, which makes its CoM accelerate
forward from the above equation (intuitively, walking starts by falling
forward). Meanwhile, it swings its free leg to make a new step. After the swing
foot touches down on the ground, the robot shifts its ZMP to the new foothold
(intuitively, it transfers its weight there), which decelerates the CoM from
the equation above. Then the process repeats.</p>
<p>Now that we have a model, let us turn to the questions of planning and control.
Walking is commonly decomposed into two sub-tasks:</p>
<ul class="simple">
<li><em>Walking pattern generation:</em> generate a reference CoM-ZMP trajectory, assuming
no disturbance and a perfect model.</li>
<li><em>Walking stabilization:</em> track at best this reference trajectory, using
feedback control to reject disturbances and model errors.</li>
</ul>
</div>
<div class="section" id="walking-pattern-generation">
<h2>Walking pattern generation</h2>
<p>The goal of walking pattern generation is to generate a CoM trajectory
<span class="math">\(\bfp_G(t)\)</span> whose corresponding ZMP, derived by:</p>
<div class="math">
\begin{equation*}
\bfp_Z = \bfp_G - \frac{\bfpdd_G}{\omega^2}
\end{equation*}
</div>
<p>lies at all times within the contact area <span class="math">\(\cal S\)</span> between the biped and
its environment. If the robot is in <em>single support</em> (<em>i.e.</em> on one foot), this
area corresponds to the contact surface below the sole. If the robot is in
<em>double support</em> (two feet in contact) and a flat floor, it corresponds to the
convex hull of all ground contact points. (If the ground is uneven or the robot
makes other contacts (for instance leaning somewhere with its hands), the
<a class="reference external" href="https://scaron.info/publications/tro-2016.html">multi-contact ZMP area</a> can
be defined, but its construction is a bit more complex.)</p>
<div class="section" id="linear-model-predictive-control">
<h3>Linear Model Predictive Control</h3>
<p>There are different methods to generate walking patterns. One of the most
prominent ones is to formulate the problem as a numerical optimization, an
approach introduced as <a class="reference external" href="https://doi.org/10.1109/ROBOT.2003.1241826">preview control</a> in 2003 by Kajita <em>et al.</em> and
that has since then be extended to <a class="reference external" href="https://doi.org/10.1109/ICHR.2006.321375">linear model predictive control (MPC)</a> by Wieber <em>et al.</em> (also with
<a class="reference external" href="https://doi.org/10.1109/IROS.2010.5654429">footstep adaption</a> and <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2015.7363423">CoM
height variations</a>). This
approach powers walking pattern generation for robots of the HRP series like
<a class="reference external" href="https://www.youtube.com/watch?v=IWG3dt9HELw">HRP-2</a> and <a class="reference external" href="https://www.youtube.com/watch?v=tcDyZ89TRAA">HRP-4</a>.</p>
</div>
<div class="section" id="dcm-trajectory-generation">
<h3>DCM Trajectory Generation</h3>
<p>Another method (actually not incompatible with the latter) is to decompose the
second-order dynamics of the LIPM into two first-order systems. Define
<span class="math">\(\bfxi\)</span> as follows:</p>
<div class="math">
\begin{equation*}
\bfxi = \bfp_G + \frac{\bfpd_G}{\omega}
\end{equation*}
</div>
<p>The dynamics of the LIPM can then be re-written as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\dot{\bfxi} & = & \omega (\bfxi - \bfp_Z) \\
\bfpd_G & = & \omega(\bfxi - \bfp_G)
\end{array}
\end{equation*}
</div>
<p>The interesting thing here is that the second equation is a stable system: it
has a negative feedback gain <span class="math">\(-\omega\)</span> on <span class="math">\(\bfp_G\)</span>, or to say it
otherwise, if the forcing term <span class="math">\(\bfxi\)</span> becomes constant then
<span class="math">\(\bfp_G\)</span> will naturally converge to it. The point <span class="math">\(\bfxi\)</span> is known
as the <em>instantaneous capture point</em> (ICP). The other equation remains unstable:
the capture point <span class="math">\(\bfxi\)</span> always diverges away from the ZMP
<span class="math">\(\bfp_Z\)</span>, which is why <span class="math">\(\bfxi\)</span> is also called the <em>divergent
component of motion</em> (DCM). The name <em>instantaneous capture point</em> comes from
the fact that, if the robot would instantaneously step on this point
<span class="math">\(\forall t \geq t_0, \bfp_Z(t) = \bfxi\)</span>, its CoM would naturally come to
a stop (be "captured") with <span class="math">\(\bfp_G(t) \to \bfxi\)</span> as <span class="math">\(t \to
\infty\)</span>.</p>
<p>As the CoM always converges to the DCM, there is no need to take care of the
second equation in the dynamic decomposition above. Walking controllers become
more efficient when they focus on controlling the DCM rather than both the CoM
position and velocity: informally, no unnecessary control is "spent" to control
the stable dynamics. Formally, controlling the DCM <a class="reference external" href="https://doi.org/10.1109/ROBOT.2009.5152284">maximizes the basin of
attraction</a> of linear feedback
controllers. Walking pattern generation can then focus on producing a
trajectory <span class="math">\(\bfxi(t)\)</span> rather than <span class="math">\(\bfp_G(t)\)</span>. Since the equation
<span class="math">\(\dot{\bfxi} = \omega (\bfxi - \bfp_Z)\)</span> is linear, this can be done using
<a class="reference external" href="https://doi.org/10.1109/IROS.2011.6094435">geometric</a> or <a class="reference external" href="https://doi.org/10.1109/IROS.2009.5354662">analytic solutions</a>. These DCM trajectory generation
methods power walking pattern generation for <a class="reference external" href="https://www.youtube.com/watch?v=T7SEyvPtWFY">ASIMO</a>, IHMC's <a class="reference external" href="https://www.youtube.com/watch?v=f2Pyu2HxVUQ">Atlas</a> or <a class="reference external" href="https://alexanderwerner.net/publications/Comanoid2016_compressed_1000k.mp4">TORO</a>
humanoid robots.</p>
<p>Now that we have a reference walking pattern, we want to make the real robot
execute it. Simple open-loop playback won't work here, as we saw that the
dynamics of walking are naturally diverging (walking is a "controlled fall").
We will therefore add feedback to it.</p>
</div>
</div>
<div class="section" id="walking-stabilization">
<h2>Walking stabilization</h2>
<p>In 1996, the Honda P2 introduced two key developments: on the hardware side, a
rubber bush added between the ankle and the foot sole to absorb impacts and
enable compliant control of the ground reaction force, and on the software
side, feedback control of the ZMP. Using the terminology from <a class="reference external" href="https://doi.org/10.1109/IROS.2009.5354522">ASIMO's balance
control report</a>, this feedback law
can be expressed using the DCM</p>
<div class="math">
\begin{equation*}
\dot{\bfxi} = \dot{\bfxi}^d + k_\xi (\bfxi^d - \bfxi)
\end{equation*}
</div>
<p>where <span class="math">\(\bfxi^d\)</span> is the desired DCM from the walking pattern. Given the
equation above where <span class="math">\(\dot{\bfxi} = \omega (\bfxi - \bfp_Z)\)</span>, this
feedback law can be rewritten equivalently in terms of the ZMP:</p>
<div class="math">
\begin{equation*}
\bfp_Z = \bfp_Z^d + k_z (\bfxi - \bfxi^d)
\end{equation*}
</div>
<p>where <span class="math">\(k_Z = 1 + k_\xi / \omega\)</span>, <span class="math">\(\bfp_Z^d\)</span> is the desired ZMP
from the walking pattern and <span class="math">\(\bfp_Z\)</span> is the ZMP controlled by the robot
using foot force control. For position-controlled robots such as HRP-2 or
HRP-4, foot force control can be realized by damping control of ankle joints,
see for instance Section III.D of the <a class="reference external" href="https://doi.org/10.1109/IROS.2010.5651082">reference report on HRP-4C's walking
stabilizer</a>. This report is in
itself an excellent read and I warmly encourage you to read it if you want to
learn more about walking stabilization: every section of it is meaningful.</p>
<img alt="Effect of DCM feedback on a biped's balance" class="right max-height-350px" src="https://scaron.info/figures/dcm-feedback.png" />
<p>So, what happens with this control law? Imagine for instance that, while
playing back the walking pattern, the robot starts tilting to the right for
some reason (umodeled dynamics, tilted ground, ...) As a consequence, the
lateral coordinate <span class="math">\(\xi_y\)</span> of the DCM will become lower than
<span class="math">\(\xi^d\)</span>. As a consequence of the feedback law above, the ZMP will then
shift toward <span class="math">\(y_Z < y_Z^d\)</span>, generating a positive velocity</p>
<div class="math">
\begin{equation*}
\dot{\xi}_y = \omega (\xi_y - y_Z) = \dot{\xi}^d_y + k_\xi
(\xi_y^d - \xi_y)
\end{equation*}
</div>
<p>on the DCM (red arrow on the figure to the right) that brings it closer to the
desired one. Hand-wavingly, the robot is tilting its right foot to the right in
order to push itself back to its left.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Is that it? Well, yes, at least for a global overview. Follow the links inlined
in the discussion above for specifics on each part. The main point I didn't
mention above is called <em>state observation</em>: in this instance, how to <a class="reference external" href="https://scaron.info/robot-locomotion/floating-base-estimation.html">estimate
the CoM position and velocity</a> from
sensory measurements. Here are some other pointers on walking control itself:</p>
<ul class="simple">
<li><a class="reference external" href="https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html">Prototyping a walking pattern generator</a>: a step-by-step
tutorial on implementing a walking pattern generator in Python.</li>
<li>Source code of the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller">lipm_walking_controller</a> used on the
HRP-4 humanoid, which follows the steps we have described.</li>
<li><a class="reference external" href="https://www.youtube.com/watch?v=qT9qzwCJjAk">Lecture on Walking Motion Control</a>: lecture by Pierre-Brice
Wieber given at the Humanoid Soccer School 2013 in Bonn.</li>
<li><a class="reference external" href="https://github.com/Leph/PhD/blob/master/build/TheseMinimized.pdf">PhD thesis of Quentin Rouxel</a> (in
French), which describes nicely the problems encountered by walking bipeds in
practice and common solutions to them.</li>
</ul>
<p>There are other families of walking control methods that do not (at least
explicitly) rely on ZMP feedback, notably <em>passive walkers</em> or the <em>hybrid zero
dynamics</em> that power the <a class="reference external" href="https://www.youtube.com/watch?v=1fC7b2LjVW4">DURUS</a>
biped.</p>
<p>Roboticist and YouTuber <tt class="docutils literal">Dr.Guero</tt> showed great <a class="reference external" href="https://www.youtube.com/watch?v=aVJs_x2a9z4">human-like walking</a> performance on his PRIMER-V7
hobby humanoid robot. The underlying method, called Upper Body Vertical Control
(UVC), relies on upper-body rather than ankle motions, and is not based on ZMP
feedback. It is described in <a class="reference external" href="https://ai2001.ifdef.jp/uvc/uvc_Eng.html">this writeup</a> (see also the <a class="reference external" href="https://ai2001.ifdef.jp/uvc/code2_Eng.html">source code</a> and <a class="reference external" href="https://ai2001.ifdef.jp/uvc/QandA_Eng.html">Q&A</a>).</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>3D Bipedal Walking including COM height variations2018-05-29T00:00:00+02:002018-05-29T00:00:00+02:00Stéphane Carontag:scaron.info,2018-05-29:/talks/qut-2018.html<p class="authors">Talk given at <a class="reference external" href="http://www.ntu.edu.sg/home/cuong/seminars.html">Nanyang Technological University</a> on 14 May 2018 and at
<strong>Queensland University of Technology</strong> on 29 May 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Real robots that walk in the field today rely on the Linear Inverted Pendulum
Mode (LIPM) for walking control. Rigorously, the LIPM requires the robot's
center-of-mass to lie in …</p></div><p class="authors">Talk given at <a class="reference external" href="http://www.ntu.edu.sg/home/cuong/seminars.html">Nanyang Technological University</a> on 14 May 2018 and at
<strong>Queensland University of Technology</strong> on 29 May 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Real robots that walk in the field today rely on the Linear Inverted Pendulum
Mode (LIPM) for walking control. Rigorously, the LIPM requires the robot's
center-of-mass to lie in a plane, which is valid for walking on flat surfaces
but becomes inexact over more general terrains. In this talk, we will see how
to extend the LIPM to 3D walking, opening up old but refreshed questions on the
analysis and control of bipeds. Technically, we will encounter a nonlinear
control problem that we address by model predictive control of a quasi-convex
optimization problem. We will see how the resulting controller works on the
HRP-4 humanoid robot.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/ntu-2018.pdf">Slides of NTU version</a>, more detailed</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/qut-2018.pdf">Slides of QUT version</a>, more high-level</td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01590509/document">Balance problem with height variations</a> (ICRA 2018)</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">Walking pattern generation with height variations</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="discussion">
<h2>Discussion</h2>
<p>Thanks to all the people who attended the presentations and asked meaningful
questions about it (including those I forgot to write down below!). Feel free
to write me directly if you have any other question related to this talk.</p>
<p><strong>What is your vector</strong> <span class="math">\(\boldsymbol{\varphi}\)</span> <strong>of design variables?</strong></p>
<blockquote>
Sorry for this gap in the talk, as I didn't want to step too much into the
details of the derivation (these are available in <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01590509/document">condensed</a> and <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">unabridged</a> versions). The
function is actually <span class="math">\(\varphi(s) = (s \omega)^2\)</span>. It is not obvious
from the equations we saw but this quantity appears quite frequently in the
structure of the problem. This draws another connection with TOPP as
<a class="reference external" href="https://arxiv.org/abs/1707.07239">TOPP-RA</a> defines the same quantity.</blockquote>
<p><strong>Do you interpolate a CoM path as in TOPP?</strong></p>
<blockquote>
No, the CoM path will be an output of the optimization (derived by forward
integration from <span class="math">\(\lambda(t)\)</span> and <span class="math">\(r(t)\)</span>). The key trick we
adapted from TOPP is the change of variable <span class="math">\(s(t)\)</span> and the idea to
define solutions as functions of <span class="math">\(s\)</span> rather than <span class="math">\(t\)</span>. (If you
think of TOPP, an expression like <span class="math">\(\dot{s}(s)\)</span> expands to
<span class="math">\(\frac{{\rm d} s}{{\rm d}t}(s(t))\)</span> which looks like a snake biting
its own tail, but is actually well-defined.) Path tracking was not
necessary here to formulate our problem.</blockquote>
<p><strong>What makes your "capture problems" faster to solve than generic nonlinear
problems?</strong></p>
<blockquote>
Mainly two things: (1) the structure from <em>linear</em> inequalities that allows
for taylored QR decompositions in the least-square steps of the SQP solver,
and (2) the fact that the nonlinear equality does not "disrupt" the
improvements thus obtained on the "QP part" of the optimization. You will
need to take a look at Section IV of the <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">the paper</a> for more precise
statements.</blockquote>
<p><strong>Edit (21/05/2018):</strong> a better answer from <a class="reference external" href="https://sites.google.com/site/adrienescandehomepage/">Adrien Escande</a>:</p>
<blockquote>
<p>There are mainly four factors behind this speedup:</p>
<ul class="simple">
<li>There is only <em>one</em> nonlinear constraint, so that using a quadratic
penalty with fixed gain works well.</li>
<li>Linear inequality constraints have a fixed and known structure that we
can leverage during the least-square steps.</li>
<li>The latter make it easy to find an initial feasible point.</li>
<li>The problem is well-conditioned: most standard SQP refinements and tricks
are not needed, so that we can get away with a simple "straight out of
the textbook" implementation. Even line search or other globalization
methods may not be necessary.</li>
</ul>
</blockquote>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Balance control using both ZMP and COM height variations: A convex boundedness approach2018-05-22T00:00:00+02:002018-05-22T00:00:00+02:00Stéphane Carontag:scaron.info,2018-05-22:/publications/icra-2018.html<p class="authors"><strong>Stéphane Caron</strong> and <a class="reference external" href="http://www.math.univ-paris13.fr/~mallein/">Bastien Mallein</a>. ICRA 2018, Brisbane, Australia,
May 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Developments for 3D control of the center of mass (CoM) of biped robots are
currently located in two local minima: on the one hand, methods that allow CoM
height variations but only work in the 2D sagittal plane …</p></div><p class="authors"><strong>Stéphane Caron</strong> and <a class="reference external" href="http://www.math.univ-paris13.fr/~mallein/">Bastien Mallein</a>. ICRA 2018, Brisbane, Australia,
May 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Developments for 3D control of the center of mass (CoM) of biped robots are
currently located in two local minima: on the one hand, methods that allow CoM
height variations but only work in the 2D sagittal plane; on the other hand,
nonconvex direct transcriptions of centroidal dynamics that are delicate to
handle. This paper presents an alternative that controls the CoM in 3D via an
indirect transcription that is both low-dimensional and solvable fast enough
for real-time control. The key to this development is the notion of boundedness
condition, which quantifies the capturability of 3D CoM trajectories.</p>
</div>
<div class="section" id="video">
<h2>Video</h2>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/icra-2018.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01590509/document">Paper</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="/slides/ntu-2018.pdf">Presentation given at NTU</a> on 14 May 2018</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td>Posters: <a class="reference external" href="https://scaron.info/files/icra-2018/poster-left.pdf">first part</a> and <a class="reference external" href="https://scaron.info/files/icra-2018/poster-right.pdf">second part</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/3d-balance">Source code</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA.2018.8460942">10.1109/ICRA.2018.8460942</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">caron2018icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{{Balance control using both ZMP and COM height variations: A convex boundedness approach}}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane and Mallein, Bastien}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01590509}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2018}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA.2018.8460942}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
<div class="section" id="discussion">
<h2>Discussion</h2>
<p><strong>It seems that dt/ds goes to infinity as s goes to zero. Does this mean that
the integration accuracy of the discretization is also arbitrarily bad over
[s0, s1]?</strong></p>
<blockquote>
That's right. The user can control the accuracy of <em>spatial</em> integration
through the choice of <span class="math">\((s_1, \ldots, s_{N-1})\)</span>, but <em>time</em>
integration will by essence worsen close to <span class="math">\(s=0\)</span> (because the system
only converges for <span class="math">\(t \to \infty\)</span>). To say it the other way round,
the best accuracy is obtained close to the current robot state
<span class="math">\((t=0)\)</span>. This is a rather desired feature for model predictive
control, where precision is most important for the immediate output (the
only part of the solution actually used for control).</blockquote>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Robotics assignment 12018-05-18T00:00:00+02:002018-05-18T00:00:00+02:00Stéphane Carontag:scaron.info,2018-05-18:/robot-locomotion/assignment-1.html<p>All questions are formulated in Python with <a class="reference external" href="https://github.com/stephane-caron/pymanoid">pymanoid</a> but you can solve them using
any other robotics environment. Stars indicate questions that are significantly
harder than those before them.</p>
<div class="section" id="problem-1-interpolation">
<h2>Problem 1: interpolation</h2>
<div class="section" id="question-1-1">
<h3>Question 1.1</h3>
<p>Consider two points <span class="math">\(A\)</span> and <span class="math">\(B\)</span> in the 3D Euclidean space, as well
as a …</p></div></div><script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script><p>All questions are formulated in Python with <a class="reference external" href="https://github.com/stephane-caron/pymanoid">pymanoid</a> but you can solve them using
any other robotics environment. Stars indicate questions that are significantly
harder than those before them.</p>
<div class="section" id="problem-1-interpolation">
<h2>Problem 1: interpolation</h2>
<div class="section" id="question-1-1">
<h3>Question 1.1</h3>
<p>Consider two points <span class="math">\(A\)</span> and <span class="math">\(B\)</span> in the 3D Euclidean space, as well
as a duration <span class="math">\(T\)</span> in seconds. Derive the coefficients of a polynomial
<span class="math">\(P(t)\)</span> of time <span class="math">\(t\)</span> such that:</p>
<ul class="simple">
<li><span class="math">\(P(0) = A\)</span></li>
<li><span class="math">\(P(T) = B\)</span></li>
<li><span class="math">\(P'(0) = 0\)</span></li>
<li><span class="math">\(P'(T) = 0\)</span></li>
</ul>
</div>
<div class="section" id="question-1-2">
<h3>Question 1.2</h3>
<p>Here are two transformations in the Euclidean space:</p>
<pre class="code python literal-block">
<span class="n">pose0</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span> <span class="mf">0.65775439</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.10466086</span><span class="p">,</span> <span class="mf">0.05145381</span><span class="p">,</span> <span class="mf">0.74414903</span><span class="p">,</span> <span class="mf">1.4</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">1.4</span><span class="p">])</span>
<span class="n">pose1</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span> <span class="mf">0.47447667</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.13290111</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.05144997</span><span class="p">,</span> <span class="mf">0.86865533</span><span class="p">,</span> <span class="mf">1.22861559</span><span class="p">,</span> <span class="mf">0.67119575</span><span class="p">,</span> <span class="mf">1.68765532</span><span class="p">])</span>
</pre>
<p>Each <em>pose</em> consists of a quaternion <tt class="docutils literal">[qw qx qy qz]</tt> followed by the
three position coordinates <tt class="docutils literal">[x y z]</tt>. We will use poses to describe
the position and orientation of robot frames (for instance, the heel
frame under a foot link) with respect to the intertial frame.</p>
<p>Find a <a class="reference external" href="https://en.wikipedia.org/wiki/Slerp">Slerp</a> interpolation
function for the language you are using, and use it to implement a
function <tt class="docutils literal">pose(t)</tt> such that:</p>
<ul class="simple">
<li><tt class="docutils literal">pose(0) = pose0</tt></li>
<li><tt class="docutils literal">pose(1) = pose1</tt></li>
<li>The quaternion <tt class="docutils literal">pose(t)</tt> is the Slerp between <tt class="docutils literal"><span class="pre">pose0[:4]</span></tt> and <tt class="docutils literal"><span class="pre">pose1[:4]</span></tt></li>
<li>The linear velocity <tt class="docutils literal"><span class="pre">pose'(0)[4:]</span> = 0</tt></li>
<li>The linear velocity <tt class="docutils literal"><span class="pre">pose'(1)[4:]</span> = 0</tt></li>
</ul>
</div>
<div class="section" id="question-1-3">
<h3>Question 1.3</h3>
<p>In pymanoid, a <tt class="docutils literal">Contact</tt> is a surface where the robot can put its feet or
hands. A contact is defined by two things: its dimensions (width and length, in
meters), and the location of its frame in the Euclidean space, as follows:</p>
<pre class="code python literal-block">
<span class="p">(</span><span class="n">half_length</span><span class="p">,</span> <span class="n">half_width</span><span class="p">)</span> <span class="o">=</span> <span class="p">(</span><span class="mf">0.1</span><span class="p">,</span> <span class="mf">0.05</span><span class="p">)</span> <span class="c1"># contact dimensions in [m]</span>
<span class="n">c0</span> <span class="o">=</span> <span class="n">Contact</span><span class="p">((</span><span class="n">half_width</span><span class="p">,</span> <span class="n">half_length</span><span class="p">),</span> <span class="n">pose</span><span class="o">=</span><span class="p">[</span> <span class="mf">0.65775439</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.10466086</span><span class="p">,</span> <span class="mf">0.05145381</span><span class="p">,</span> <span class="mf">0.74414903</span><span class="p">,</span> <span class="mf">1.4</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">1.4</span><span class="p">])</span>
<span class="n">c1</span> <span class="o">=</span> <span class="n">Contact</span><span class="p">((</span><span class="n">half_width</span><span class="p">,</span> <span class="n">half_length</span><span class="p">),</span> <span class="n">pose</span><span class="o">=</span><span class="p">[</span> <span class="mf">0.47447667</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.13290111</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.05144997</span><span class="p">,</span> <span class="mf">0.86865533</span><span class="p">,</span> <span class="mf">1.22861559</span><span class="p">,</span> <span class="mf">0.67119575</span><span class="p">,</span> <span class="mf">1.68765532</span><span class="p">])</span>
</pre>
<p>Adapting your solution to Question 1, draw a trajectory between the two
contacts <tt class="docutils literal">c0</tt> and <tt class="docutils literal">c1</tt> above. You can use the <tt class="docutils literal">draw_line</tt> function.</p>
</div>
</div>
<div class="section" id="problem-2-dynamics">
<h2>Problem 2: dynamics</h2>
<p>The dynamics of a walking biped are summed up by the second-order differential
equation:</p>
<div class="math">
\begin{equation*}
\ddot{c}(t) = \omega^2 (c(t) - r(t))
\end{equation*}
</div>
<p>where <span class="math">\(\omega^2\)</span> is a constant, <span class="math">\(c(t)\)</span> is the position of the
<a class="reference external" href="https://en.wikipedia.org/wiki/Center_of_mass">center of mass</a> (CoM) of the
robot and <span class="math">\(r(t)\)</span> is the position of the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html#center-of-pressure">center of pressure</a> (CoP) at time
<span class="math">\(t\)</span>.</p>
<div class="section" id="question-2-1">
<h3>Question 2.1</h3>
<p>Let us take <span class="math">\(\omega = \sqrt{13} \ \text{s}^{-1}\)</span> for now. Assuming that
<span class="math">\(r(t) = r_f\)</span> is stationary, solve the differential equation above,
writing <span class="math">\(c(t)\)</span> as a function of <span class="math">\(t\)</span>, <span class="math">\(\omega^2\)</span> and the CoP
location <span class="math">\(r_f\)</span>.</p>
</div>
<div class="section" id="question-2-2">
<h3>Question 2.2</h3>
<p>Implement your solution as a function <tt class="docutils literal">integrate</tt> with the following
specification:</p>
<ul>
<li><p class="first"><em>Parameters:</em></p>
<blockquote>
<ul class="simple">
<li>Initial CoM position</li>
<li>Initial CoM velocity</li>
<li>CoP position</li>
<li>Duration <span class="math">\(T\)</span></li>
</ul>
</blockquote>
</li>
<li><p class="first"><em>Return values:</em></p>
<blockquote>
<ul class="simple">
<li>CoM position at time <span class="math">\(T\)</span></li>
<li>CoM velocity at time <span class="math">\(T\)</span></li>
</ul>
</blockquote>
</li>
</ul>
<p>Run it on the following example:</p>
<pre class="code python literal-block">
<span class="n">init_com</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.01</span><span class="p">,</span> <span class="mf">0.002</span><span class="p">,</span> <span class="mf">0.8</span><span class="p">]</span> <span class="c1"># initial CoM position in [m]</span>
<span class="n">init_comd</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.01</span><span class="p">,</span> <span class="mf">0.1</span><span class="p">]</span> <span class="c1"># initial CoM velocity in [m] / [s]</span>
<span class="n">cop</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.</span><span class="p">]</span> <span class="c1"># CoP position in [m]</span>
<span class="n">duration</span> <span class="o">=</span> <span class="mf">0.2</span> <span class="c1"># duration in [s]</span>
<span class="n">last_com</span><span class="p">,</span> <span class="n">last_comd</span> <span class="o">=</span> <span class="n">integrate</span><span class="p">(</span><span class="n">init_com</span><span class="p">,</span> <span class="n">init_comd</span><span class="p">,</span> <span class="n">cop</span><span class="p">,</span> <span class="n">duration</span><span class="p">)</span>
</pre>
<p>What are the values of <tt class="docutils literal">last_com</tt> and <tt class="docutils literal">last_comd</tt>?</p>
</div>
<div class="section" id="question-2-3">
<h3>Question 2.3</h3>
<p>Draw the corresponding trajectory from the initial state <tt class="docutils literal">(init_com,
init_comd)</tt> to the terminal state <tt class="docutils literal">(last_com, last_comd)</tt>.</p>
</div>
</div>
<div class="section" id="problem-3-planar-walking">
<h2>Problem 3: planar walking</h2>
<p>Here is a set of footsteps in the horizontal plane (<tt class="docutils literal">x</tt> and <tt class="docutils literal">y</tt> coordinates
only):</p>
<pre class="code python literal-block">
<span class="n">footsteps</span> <span class="o">=</span> <span class="p">[</span>
<span class="p">[</span><span class="mf">0.0</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.2</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.45</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.725</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.975</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">1.25</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">1.5</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">1.775</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">1.975</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">1.975</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">2.0</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.035</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.035</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.335</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.335</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.635</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.09</span><span class="p">],</span>
<span class="p">[</span><span class="mf">0.635</span><span class="p">,</span> <span class="mf">0.09</span><span class="p">]]</span>
</pre>
<p>Each footstep is a rectangle centered at coordinates <tt class="docutils literal">(x, y)</tt> from the list
above. As in Question 1.p, the width of each footstep rectangle (<em>i.e.</em> its
dimension along the y-axis) is 10 cm, while the length of each rectangle (its
dimension along the x-axis) is 20 cm.</p>
<div class="section" id="question-3-1">
<h3>Question 3.1</h3>
<p>Read the paper <a class="reference external" href="https://hal.inria.fr/file/index/docid/390462/filename/Preview.pdf">Trajectory Free Linear Model Predictive Control for Stable
Walking in the Presence of Strong Perturbations</a> by
Wieber (2006). Are there some parts you don’t understand? If so, pick one and
pinpoint what you don’t grasp. Are there some points you disagree with? If so,
detail why.</p>
</div>
<div class="section" id="question-3-2">
<h3>Question 3.2</h3>
<p>What is the CoM height <span class="math">\(h\)</span> corresponding to the value of <span class="math">\(\omega\)</span>
we used in Question 2.1?</p>
</div>
<div class="section" id="question-3-3">
<h3>Question 3.3</h3>
<p>Find a trajectory with piecewise-constant CoPs such that, starting from
<tt class="docutils literal"><span class="pre">init_com=[0.,</span> 0., 0.8]</tt>, the CoM ends above the fourth footstep of the
sequence above, <em>i.e.</em> the 20 cm x 10 cm rectangle centered on <tt class="docutils literal">[0.45,
0.09]</tt>. You can describe your trajectory as a list of (CoP, duration) pairs
that can be played in Python as:</p>
<pre class="code python literal-block">
<span class="n">com</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.8</span><span class="p">])</span>
<span class="n">comd</span> <span class="o">=</span> <span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
<span class="k">for</span> <span class="p">(</span><span class="n">cop</span><span class="p">,</span> <span class="n">duration</span><span class="p">)</span> <span class="ow">in</span> <span class="n">trajectory</span><span class="p">:</span>
<span class="n">com</span><span class="p">,</span> <span class="n">comd</span> <span class="o">=</span> <span class="n">integrate</span><span class="p">(</span><span class="n">com</span><span class="p">,</span> <span class="n">comd</span><span class="p">,</span> <span class="n">cop</span><span class="p">,</span> <span class="n">duration</span><span class="p">)</span>
<span class="k">assert</span><span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">com</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="mf">0.45</span><span class="p">)</span> <span class="o"><</span> <span class="mf">0.1</span> <span class="ow">and</span> <span class="nb">abs</span><span class="p">(</span><span class="n">com</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="mf">0.09</span><span class="p">)</span> <span class="o"><</span> <span class="mf">0.05</span><span class="p">)</span>
</pre>
</div>
<div class="section" id="question-3-4">
<h3>Question 3.4 ★</h3>
<p>Compute a CoM-CoP trajectory that traverses these footsteps using the method
from the paper above. You can use the <tt class="docutils literal">solve_qp</tt> function from <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a> in Python.</p>
</div>
<div class="section" id="question-3-5">
<h3>Question 3.5</h3>
<p>Combine your answers to the questions so far to display your walking trajectory
in pymanoid, showing all of the following:</p>
<ul class="simple">
<li>Left foot trajectory in green</li>
<li>Right foot trajectory in magenta</li>
<li>CoP trajectory in red</li>
<li>CoM trajectory in blue</li>
</ul>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Reviewing a scientific paper2018-03-20T00:00:00+01:002018-03-20T00:00:00+01:00Stéphane Carontag:scaron.info,2018-03-20:/blog/reviewing-a-scientific-paper.html<p>For scientists, the review process is personal, almost intimate: everyone does
it (maybe first as tit for that when submitting your first papers, then for
other reasons discussed thereafter), but researchers don’t talk about it so
much. One probable cause for this is the anonymity principle that is at …</p><p>For scientists, the review process is personal, almost intimate: everyone does
it (maybe first as tit for that when submitting your first papers, then for
other reasons discussed thereafter), but researchers don’t talk about it so
much. One probable cause for this is the anonymity principle that is at the
core of the review process: authors are not told who their reviewers are, and
in double-blind submissions reviewers are not told either who the authors are.</p>
<div class="section" id="what-is-the-review-process">
<h2>What is the review process</h2>
<p>The goal of a review is to assess the contribution of a submitted manuscript,
<em>i.e.</em> what’s new compared to existing works. Peers of the authors are asked
for their evaluation, and on the way give their opinion and other comments to
help improve the manuscript. For authors and reviewers alike, the review
process is a way of interacting between peers.</p>
<div class="section" id="surviving-the-review-process">
<h3>Surviving the review process</h3>
<p>There is an excellent and short piece of writing by Seth Hutchinson, <a class="reference external" href="http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=5663683">Surviving
the review process</a>, that lifts
the curtain on a number of important points about the review process. If you
haven’t read it yet, you can forget about what’s written below and click this
link immediately ;)</p>
</div>
<div class="section" id="what-are-the-qualities-of-a-good-review">
<h3>What are the qualities of a good review?</h3>
<p>A good review is constructive in its criticism, and does not diminish the
authors’ efforts (even when the review has to be negative, or sometimes, very
negative). To make sure you achieve this, there is a simple strategy: try to be
as nice as possible; remember you are talking to one of your peers. When
pointing out that something is wrong, make sure you (1) explain why it is
wrong, and (2) point out how it can be improved, whenever possible.</p>
</div>
</div>
<div class="section" id="a-simple-review-method">
<h2>A simple review method</h2>
<p>Reading someone else’s work is an effort. Your own curiosity in the topic is
one way to balance it. Another way is to make your reading “interactive” as
follows:</p>
<ul class="simple">
<li>Read the paper linearly, taking notes in a text file as you go.</li>
<li>Complete your notes following <em>e.g.</em> the template below.</li>
<li>Double-check by reading the introduction or conclusion of the manuscript again.</li>
<li>Go to the review website and polish your review as you submit it.</li>
</ul>
<p>For me, notes usually start as a bullet list of oral comments: what I would
like to say to the authors in response to specific parts of the manuscript. As
I keep on reading, I usually shuffle this list, putting most important comments
first and minor ones at the end. This is the “low level” part of the review.
The “high level” part of the review (general comments and assessment of the
manuscript) usually comes as a set of paragraphs that open your review, before
going into detailed (bullet-list like) comments. You can use the following
structure.</p>
<div class="section" id="summary">
<h3>Summary</h3>
<p>Start with a quick summary, using your own words (not the authors’), briefly
describing the steps taken in the paper. The summary can simply list out what
the paper does without going into constructive criticism (yet). This step is
also useful for the editor (and authors when there is a rebuttal phase) to
check how you understood the paper.</p>
</div>
<div class="section" id="contribution">
<h3>Contribution</h3>
<p>Now is the time for constructive criticism: single out what’s new in the
manuscript compared to existing works, and discuss it. Is it meaningful?
Technically sound? Also, if the authors’ contribution statement is incorrect
(<em>e.g.</em> they listed as contribution something that was already published), it
is the time to gently point them to relevant references.</p>
<p>Here is for example the list of questions that the IEEE Robotics and Automation
Society suggests you evaluate at this stage of your review:</p>
<ul class="simple">
<li>Is the contribution of the paper relevant to its field?</li>
<li>Does it properly cite related existing works?</li>
<li>Does it provide all technical material needed to understand and reproduce the contribution?</li>
<li>How are its experimental results?</li>
<li>Is the writing clear and comprehensible?</li>
</ul>
<p>It is also common practice to group minor suggestions and typos in a short list
at the end of your review.</p>
</div>
<div class="section" id="strengths-and-weaknesses">
<h3>Strengths and weaknesses</h3>
<p>Discussing both pros and cons of the submission is usually a good strategy: if
you liked the paper, it reminds you to try to look for its downsides, and
conversely. Include suggestions for improvement as appropriate. Be thorough,
fair, and constructive.</p>
</div>
<div class="section" id="final-checks">
<h3>Final checks</h3>
<p>Finally, if you consider that the paper can be accepted for publication, you
can give a quick check to the authors’ webpage to verify that the work has not
been submitted/published elsewhere. (This can happen: when it does, the authors
should mention it explicitly in their manuscript, otherwise this is a
deontological red flag.)</p>
</div>
</div>
<div class="section" id="q-a">
<h2>Q & A</h2>
<p><strong>What is the point of the anonymity principle?</strong></p>
<p>It tries to remove bias. Of course it is not a panacea so don’t expect it to
eliminate all kinds of biases encountered in the scientific community, but
think of the following situation: a PhD student reviews a technically flawed
manuscript from the lab of a very famous professor in his field (one he may
<em>e.g.</em> like to join for post-doc…). Without anonymity, the student faces two
conflicting objectives: writing a truthful review, and avoiding potential harm
to his/her future career. Anonymity helps avoid such situations.</p>
<p><strong>What about an opposite situation: a famous professor reviews a paper
on a topic someone in his/her lab is currently working on?</strong></p>
<p>This is one of the conflicting situations that is not solved by <em>rules</em> of the
current system. (Be careful about people who want to put rules everywhere.) It
therefore becomes a question of <a class="reference external" href="https://en.wikipedia.org/wiki/Ethics">ethics</a>. You will encounter both ethical and
unethical tempers among people in your community. Rewarding and interacting
with ethical people while avoiding interactions with unethical ones is a simple
strategy that everyone can apply at one’s own level.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Here are a couple of references on this topic:</p>
<ul class="simple">
<li><a class="reference external" href="http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=5663683">Surviving the review process</a> by Seth
Hutchinson.</li>
<li><a class="reference external" href="https://agupubs.onlinelibrary.wiley.com/doi/pdf/10.1029/2011EO280001">A Quick Guide to Writing a Solid Peer Review</a>,
including a flow chart of the process.</li>
<li><a class="reference external" href="http://www.ieee-ras.org/publications/t-ro/information-for-reviewers">Information for Reviewers</a> from
the IEEE Transactions on Robotics.</li>
</ul>
<p>You can also check out the <em>Solicited Reviews</em> section of <a class="reference external" href="http://www.semantic-web-journal.net/content/dbpedia-large-scale-multilingual-knowledge-base-extracted-wikipedia">this page</a>
for examples of reviews written by senior researchers.</p>
</div>
What is a controller?2018-02-21T00:00:00+01:002021-06-10T00:00:00+02:00Stéphane Carontag:scaron.info,2018-02-21:/robot-locomotion/what-is-a-controller.html<p>Robots run many layers of software. The layer that decides what actions to take
based on the latest sensor readings is called the <em>controller</em>. In
reinforcement learning, it is known as the <em>agent</em>.</p>
<div class="section" id="feedback-loop">
<h2>Feedback loop</h2>
<p>In the general sense, robot components can be classified in three categories:
actuation (“the plant …</p></div><p>Robots run many layers of software. The layer that decides what actions to take
based on the latest sensor readings is called the <em>controller</em>. In
reinforcement learning, it is known as the <em>agent</em>.</p>
<div class="section" id="feedback-loop">
<h2>Feedback loop</h2>
<p>In the general sense, robot components can be classified in three categories:
actuation (“the plant”, sometimes simply called “the robot”), sensing and
control. A controller is, by definition,anything that turns robot outputs
(<em>a.k.a.</em> “states”, for instance joint positions and velocities) into new robot
inputs (<em>a.k.a.</em> “controls”, for instance joint accelerations or torques).
Robot outputs are not known perfectly but measured by sensors. Putting these
three components together yields the <em>feedback loop</em>:</p>
<img alt="Feedback loop" src="https://scaron.info/figures/feedback_loop.png" />
<p>If we take the simple example of a velocity-controlled point mass, the input of
your robot is a velocity and its output is a new position. A controller is then
any piece of software that takes positions as inputs and outputs velocities.
Under the hood, the controller may carry out various operations such as
trajectory planning or <a class="reference external" href="https://en.wikipedia.org/wiki/PID_controller">PID feedback</a>. For instance, a <a class="reference external" href="https://en.wikipedia.org/wiki/Model_predictive_control">model
predictive controller</a> computes a desired
future trajectory of the robot starting from its current (measured) state, then
extracts the first controls from it and sends it to the robot.</p>
<div class="section" id="example-of-hrp-4">
<h3>Example of HRP-4</h3>
<p>The HRP-4 humanoid robot from Kawada Industries is a position-controlled robot.</p>
<ul class="simple">
<li><strong>Inputs:</strong> desired joint positions</li>
<li><strong>Outputs:</strong> joint angle positions, as well as the position and orientation
of the robot with respect to the inertial frame (a.k.a. its <em>floating base</em>
transform)</li>
</ul>
<p>Being a mobile robot, it is <a class="reference external" href="https://en.wikipedia.org/wiki/Underactuation">underactuated</a>, meaning its state has a
higher dimension than its inputs. Measurements of the robot state are carried
out by <a class="reference external" href="https://en.wikipedia.org/wiki/Rotary_encoder">rotary encoders</a> for
joint angles and using an <a class="reference external" href="https://en.wikipedia.org/wiki/Inertial_measurement_unit">IMU</a> for the free-flyer
transform. A controller for HRP-4 therefore takes as inputs the robot’s state
(joint positions + position and orientation with respect to the inertial frame)
and outputs a new set of desired positions.</p>
</div>
</div>
<div class="section" id="terminology">
<h2>Terminology</h2>
<p>Control theory has a bit more terminology to describe the feedback loop above.
Here is the same loop with standard notations from this field:</p>
<img alt="Exogenous and control inputs" class="center" src="https://scaron.info/figures/exogenous_inputs.png" />
<p>The real state <span class="math">\(\bfx\)</span> of the system is an ideal quantity. Some components
of it may not be observable by sensors, for example the <a class="reference external" href="https://scaron.info/robot-locomotion/floating-base-estimation.html">yaw orientation of the
robot with respect to gravity</a>.</p>
<ul class="simple">
<li><span class="math">\(\bfy\)</span> is the <em>output</em>, the set of observable components in <span class="math">\(\bfx\)</span>.</li>
<li><span class="math">\(\bfz\)</span> is the <em>exogenous output</em>, the set of non-observable components
in <span class="math">\(\bfx\)</span>.</li>
<li><span class="math">\(\bfL\)</span> is the <em>state observer</em> that estimates <span class="math">\(\bfy\)</span> from
available sensor readings.</li>
<li><span class="math">\(\bfK\)</span> is the <em>controller</em> that computes the control input from the
measured state.</li>
<li><span class="math">\(\bfu\)</span> is the <em>control input</em> by which the controller can act on the plant.</li>
<li><span class="math">\(\bfP\)</span> is the <em>plant</em>, the external system being controlled, <em>a.k.a.</em>
the robot.</li>
<li><span class="math">\(\bfw\)</span> is the <em>exogenous input</em>, a set of external causes unseen by the
controller.</li>
</ul>
<p>The vocabulary of control theory puts the plant-observer at the center, which
is why <span class="math">\(\bfu\)</span> is called the control <em>input</em> (although it's an output of
the controller: because it's an input to the plant) and <span class="math">\(\bfy\)</span> is called
the <em>output</em> (it's an output of the observer and an input to the controller).</p>
<div class="section" id="exogenous-inputs">
<h3>Exogenous inputs</h3>
<p>Control theory precises the notion of <em>exogenous</em> inputs <span class="math">\(\bfw\)</span> to
represent external causes that the system cannot observe. For example, when a
humanoid robot <span class="math">\(\bfP\)</span> walking on unknown terrain is subjected to an
external push <span class="math">\(\bfw\)</span>, the robot cannot distinguish a terrain irregularity
(there is a difference between the expected and measured contact forces caused
by some unknown terrain shape) from the external push (the contact force
difference is caused by the push). The push is then an exogenous input,
required to integrate <span class="math">\(\bfP\)</span> forward in time but unknown to the
controller <span class="math">\(\bfK\)</span>.</p>
</div>
<div class="section" id="reinforcement-learning">
<h3>Reinforcement learning</h3>
<p>Reinforcement learning frames the same problem with a slightly different
terminology. In reinforcement learning, <span class="math">\(\bfy\)</span> is the <em>observation</em>,
<span class="math">\(\bfu\)</span> is the <em>action</em>, <span class="math">\(\bfP\)</span> is the <em>environment</em> and
<span class="math">\(\bfK\)</span> is the <em>agent</em>. Most of the time, exogenous quantiies and the
state observer <span class="math">\(\bfL\)</span> are collectively included in <span class="math">\(\bfP\)</span>, as
reinforcement learning centers on the agent. The addition of reinforcement
learning to classical control theory is a second input <span class="math">\(r\)</span> to the
controller representing the <em>reward</em> obtained from executing action
<span class="math">\(\bfu\)</span> from state <span class="math">\(\bfx\)</span>.</p>
</div>
</div>
<div class="section" id="q-a">
<h2>Q & A</h2>
<p><strong>How to achieve velocity/acceleration control on a position-controlled
robot?</strong></p>
<blockquote>
Sending successive joint angles along a trajectory with the desired
velocities or accelerations, and relying and the robot’s stiff position
tracking.</blockquote>
<p><strong>How to control the underactuated position of the robot in the inertial
frame?</strong></p>
<blockquote>
Using contacts with the environment and force control. For instance,
position-controlled robots use <a class="reference external" href="https://en.wikipedia.org/wiki/Impedance_control">admittance control</a> to regulate forces at
their end-effectors.</blockquote>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>There are many references to pick from in control theory. For an introduction,
you can check out Quang-Cuong Pham’s <a class="reference external" href="https://github.com/quangounet/open-control-theory/raw/253a1c8e852b4717a719170c62e6d8e34f58b45e/lecture-notes/notes.pdf">lecture notes on control theory</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>CMake can use a local Boost2017-12-11T00:00:00+01:002017-12-11T00:00:00+01:00Stéphane Carontag:scaron.info,2017-12-11:/blog/cmake-can-use-a-local-boost.html<p>One of my current projects relies on <a class="reference external" href="http://www.boost.org/doc/libs/1_64_0/libs/python/doc/html/">Boost.Python</a>, which requires
a more recent version of Boost (1.64) than the one (1.54) provided by my Linux
distribution (Ubuntu 14.04). My first guess was to remove Boost from my package
manager and install its newer version from source …</p><p>One of my current projects relies on <a class="reference external" href="http://www.boost.org/doc/libs/1_64_0/libs/python/doc/html/">Boost.Python</a>, which requires
a more recent version of Boost (1.64) than the one (1.54) provided by my Linux
distribution (Ubuntu 14.04). My first guess was to remove Boost from my package
manager and install its newer version from source... with dramatic
consequences! At first, I only experienced a few glitches in <a class="reference external" href="http://www.ros.org/">ROS</a> and some libraries built with the old version of Boost,
but down the road I ran into serious runtime problems that forced me to revert
to the system's Boost 1.54.</p>
<p>My second guess was to use a local version of Boost 1.64, compiled but not
installed system-wide and only used by the project that needs it. I knew this
is possible because the <a class="reference external" href="http://pid.lirmm.net/pid-framework/">PID framework</a>
does this. Without switching to a software source distribution manager, let's
see how to change only the <tt class="docutils literal">CMakeCache.txt</tt> configuration to switch from a
system-wide to a local Boost install.</p>
<div class="section" id="local-changes-to-cmakecache-txt">
<h2>Local changes to CMakeCache.txt</h2>
<p>I'm assuming you already have your <tt class="docutils literal">CMakeLists.txt</tt> file written and
functional. For instance, you can follow these <a class="reference external" href="https://vsamy.github.io/en/blog/boost-python-cmake-build">instructions to use
Boost.Python with CMake</a>. We won't make any
change to the <tt class="docutils literal">CMakeLists.txt</tt>, so that your local Boost install will only
affect you and stay transparent for other project users.</p>
<p>To start with, go to your build directory and call <tt class="docutils literal">ccmake .</tt>, or
alternatively open <tt class="docutils literal">CMakeCache.txt</tt> in your text editor.</p>
<div class="section" id="boost-paths">
<h3>Boost paths</h3>
<p>If you use the <a class="reference external" href="https://github.com/jrl-umi3218/jrl-cmakemodules">JRL CMake modules</a> to find Boost
automatically, you will already have a number of <tt class="docutils literal">Boost_*</tt> CMake variables.
The first thing is to update them to your local path:</p>
<div class="highlight"><pre><span></span>Boost_INCLUDE_DIR:PATH<span class="o">=</span>/my/local/path/boost/1.64.0/include
Boost_LIBRARY_DIR:PATH<span class="o">=</span>/my/local/path/boost/1.64.0/lib
Boost_PYTHON_LIBRARY_DEBUG:FILEPATH<span class="o">=</span>/my/local/path/boost/1.64.0/lib/libboost_python.so
Boost_PYTHON_LIBRARY_RELEASE:FILEPATH<span class="o">=</span>/my/local/path/boost/1.64.0/lib/libboost_python.so
</pre></div>
<p>If compilation fails with Boost headers not found, add them manually by:</p>
<div class="highlight"><pre><span></span>CMAKE_CXX_FLAGS:STRING<span class="o">=</span>-I/my/local/path/boost/1.64.0/include
</pre></div>
<p>Possibly the <tt class="docutils literal">Boost_DIR</tt> variable will revert to <tt class="docutils literal"><span class="pre">Boost_DIR-NOTFOUND</span></tt> after
you run CMake again. This is not a problem.</p>
</div>
<div class="section" id="update-linker-flags">
<h3>Update linker flags</h3>
<p>While Boost.Python will be linked with its full path, some libraries like
Boost.NumPy may still be linked by a mere <tt class="docutils literal"><span class="pre">-lboost_numpy</span></tt>, in which case the
linker will fail to find the local shared object (.so) file. Let's add a <tt class="docutils literal"><span class="pre">-L</span></tt>
linker flag to help it here:</p>
<div class="highlight"><pre><span></span>CMAKE_SHARED_LINKER_FLAGS:STRING<span class="o">=</span>-L/my/local/path/boost/1.64.0/lib
</pre></div>
<p>At this stage, your executable or library should compile and link. You can
check that the files produced in your <em>build</em> folder (let's assume a .so here)
links properly to your local Boost libraries:</p>
<div class="highlight"><pre><span></span>$ ldd ./build/mylib.so
linux-vdso.so.1 <span class="o">=</span>> <span class="o">(</span>0x0000...<span class="o">)</span>
libboost_numpy.so.1.64.0 <span class="o">=</span>> /my/local/path/boost/1.64.0/lib/libboost_numpy.so.1.64.0 <span class="o">(</span>0x0000...<span class="o">)</span>
libboost_python.so.1.64.0 <span class="o">=</span>> /my/local/path/boost/1.64.0/lib/libboost_python.so.1.64.0 <span class="o">(</span>0x0000...<span class="o">)</span>
</pre></div>
<p>However, you may notice that it is not the case with your <em>installed</em> files:</p>
<div class="highlight"><pre><span></span>$ ldd /install/path/mylib.so
linux-vdso.so.1 <span class="o">=</span>> <span class="o">(</span>0x00007ffc8f5c7000<span class="o">)</span>
libboost_numpy.so.1.64.0 <span class="o">=</span>> not found
libboost_python.so.1.64.0 <span class="o">=</span>> not found
...
</pre></div>
<p>This is caused by the different behaviors of CMake between build and install.</p>
</div>
<div class="section" id="a-tale-of-two-rpaths">
<h3>A Tale of two RPATHs</h3>
<p><tt class="docutils literal">RPATH</tt> is a list of directories that is directly written into your
executable or shared object and tells the linker where to look for libraries.
It has precedence over the <tt class="docutils literal">LD_LIBRARY_PATH</tt> system-wide variable, as
explained in this page on <a class="reference external" href="https://cmake.org/Wiki/CMake_RPATH_handling">CMake RPATH handling</a>.</p>
<p>A dirty solution to the "not found" problem would be to add the local Boost
1.64 libraries to <tt class="docutils literal">LD_LIBRARY_PATH</tt>. But then, they would also be visible in
other projects trying to link to Boost, which can be problematic. We will
rather set <tt class="docutils literal">RPATH</tt> to avoid this and keep things local.</p>
<p>The behavior of CMake with respect to RPATH is controlled by two parameters:</p>
<div class="highlight"><pre><span></span>// If set, runtime paths are not added when installing shared libraries,
// but are added when building.
CMAKE_SKIP_INSTALL_RPATH:BOOL<span class="o">=</span>OFF
// If set, runtime paths are not added when using shared libraries.
CMAKE_SKIP_RPATH:BOOL<span class="o">=</span>OFF
</pre></div>
<p>In our case, we need both of these values to be off. Plus, we want the linker
to behave for the <em>install</em> the way it does for the <em>build</em>, <em>i.e.</em> using full
paths to our local Boost libraries. This is specified by the following
parameter:</p>
<div class="highlight"><pre><span></span>// Use same RPATH at install and build
CMAKE_INSTALL_RPATH_USE_LINK_PATH:BOOL<span class="o">=</span>ON
</pre></div>
<p>Once this is set, run <tt class="docutils literal">make install</tt> again. You should see something like:</p>
<div class="highlight"><pre><span></span>$ ldd /install/path/mylib.so
linux-vdso.so.1 <span class="o">=</span>> <span class="o">(</span>0x0000...<span class="o">)</span>
libboost_numpy.so.1.64.0 <span class="o">=</span>> /my/local/path/boost/1.64.0/lib/libboost_numpy.so.1.64.0 <span class="o">(</span>0x0000...<span class="o">)</span>
libboost_python.so.1.64.0 <span class="o">=</span>> /my/local/path/boost/1.64.0/lib/libboost_python.so.1.64.0 <span class="o">(</span>0x0000...<span class="o">)</span>
</pre></div>
</div>
</div>
<div class="section" id="on-a-concluding-note">
<h2>On a concluding note</h2>
<p>These instructions provide an <em>ad hoc</em> way of linking with local libraries
without affecting the <tt class="docutils literal">CMakeLists.txt</tt> of your project (used by other users
who don't want to see your local paths on their side). While CMake is the <em>de
facto</em> standard today, we are feeling its limitations in such scenarios. A
better solution here would be to switch to full-fledged CMake-based package
manager.</p>
<div class="section" id="credits">
<h3>Credits</h3>
<p>Thanks to Kevin Chappellet for suggesting local changes to the
<tt class="docutils literal">CMakeCache.txt</tt> and to Robin Passama for walking me through the RPATH :)</p>
</div>
</div>
Walking on Gravel with Soft Soles using Linear Inverted Pendulum Tracking and Reaction Force Distribution2017-11-17T00:00:00+01:002017-11-17T00:00:00+01:00Stéphane Carontag:scaron.info,2017-11-17:/publications/humanoids-2017-pajon.html<p class="authors"><strong>Adrien Pajon</strong>, <strong>Stéphane Caron</strong>, <strong>Giovanni De Magistris</strong>, <a class="reference external" href="http://sylvain.miossec.free.fr/work/">Sylvain
Miossec</a> and <strong>Abderrahmane Kheddar</strong>.
<a class="reference external" href="http://humanoids2017.loria.fr/">Humanoids 2017</a>, Birmingham, United Kingdom,
November 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Soft soles absorb impacts and cast ground un-evenness during locomotion on
rough terrains. However, they introduce passive degrees of freedom
(deformations under the feet) that complexify the tasks of state …</p></div><p class="authors"><strong>Adrien Pajon</strong>, <strong>Stéphane Caron</strong>, <strong>Giovanni De Magistris</strong>, <a class="reference external" href="http://sylvain.miossec.free.fr/work/">Sylvain
Miossec</a> and <strong>Abderrahmane Kheddar</strong>.
<a class="reference external" href="http://humanoids2017.loria.fr/">Humanoids 2017</a>, Birmingham, United Kingdom,
November 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Soft soles absorb impacts and cast ground un-evenness during locomotion on
rough terrains. However, they introduce passive degrees of freedom
(deformations under the feet) that complexify the tasks of state estimation and
overall robot stabilization. We address this problem by developing a control
loop that stabilizes humanoid robots when walking with soft soles on flat and
uneven terrain. Our closed-loop controller minimizes the errors on the center
of mass (COM) and the zero moment point (ZMP) with an admittance control of the
feet based on a simple deformation estimator. We demonstrate its effectiveness
in real experiments on the HRP-4 humanoid.</p>
</div>
<div class="section" id="video">
<h2>Video</h2>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/Hb6HQbIqII8" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01576516/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2017.8246909">10.1109/HUMANOIDS.2017.8246909</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">pajon2017humanoids</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{{Walking on Gravel with Soft Soles using Linear Inverted Pendulum Tracking and Reaction Force Distribution}}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Pajon, Adrien and Caron, St{\'e}phane and De Magistris, Giovanni and Miossec, Sylvain and Kheddar, Abderrahmane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE-RAS International Conference on Humanoid Robots}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2017}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">nov</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01576516}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/HUMANOIDS.2017.8246909}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Post-Impact Adaptive Compliance for Humanoid Falls Using Predictive Control of a Reduced Model2017-11-16T00:00:00+01:002017-11-16T00:00:00+01:00Stéphane Carontag:scaron.info,2017-11-16:/publications/humanoids-2017-samy.html<p class="authors"><a class="reference external" href="https://vsamy.github.io/">Vincent Samy</a>, <strong>Stéphane Caron</strong>, <a class="reference external" href="https://members.loria.fr/kbouyarmane/">Karim
Bouyarmane</a> and <strong>Abderrahmane
Kheddar</strong>. <a class="reference external" href="http://humanoids2017.loria.fr/">Humanoids 2017</a>, Birmingham,
United Kingdom, November 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We consider control of a humanoid robot in active compliance just after the
impact consecutive to a fall. The goal of this post-impact braking is to absorb
undesired linear momentum accumulated during the …</p></div><p class="authors"><a class="reference external" href="https://vsamy.github.io/">Vincent Samy</a>, <strong>Stéphane Caron</strong>, <a class="reference external" href="https://members.loria.fr/kbouyarmane/">Karim
Bouyarmane</a> and <strong>Abderrahmane
Kheddar</strong>. <a class="reference external" href="http://humanoids2017.loria.fr/">Humanoids 2017</a>, Birmingham,
United Kingdom, November 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We consider control of a humanoid robot in active compliance just after the
impact consecutive to a fall. The goal of this post-impact braking is to absorb
undesired linear momentum accumulated during the fall, using a limited supply
of time and actuation power. The gist of our method is an optimal distribution
of undesired momentum between the robot's hand and foot contact points,
followed by the parallel resolution of Linear Model Predictive Control (LMPC)
at each contact. This distribution is made possible thanks to
emph{torque-limited friction polytopes}, an extension of friction cones that
takes actuation limits into account. Individual LMPC results are finally
combined back into a feasible CoM trajectory sent to the robot's whole-body
controller. We validate the solution in full-body dynamics simulation of an
HRP-4 humanoid falling on a wall.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01569819/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2017.8246942">10.1109/HUMANOIDS.2017.8246942</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">samy2017humanoids</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Post-Impact Adaptive Compliance for Humanoid Falls Using Predictive Control of a Reduced Model}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Samy, Vincent and Caron, St{\'e}phane and Bouyarmane, Karim and Kheddar, Abderrahmane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE-RAS International Conference on Humanoid Robots}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2017}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">nov</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01569819}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/HUMANOIDS.2017.8246942}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
When to make a step? Tackling the timing problem in multi-contact locomotion by TOPP-MPC2017-11-15T00:00:00+01:002017-11-15T00:00:00+01:00Stéphane Carontag:scaron.info,2017-11-15:/publications/humanoids-2017.html<p class="authors"><strong>Stéphane Caron</strong> and <a class="reference external" href="http://www.ntu.edu.sg/home/cuong/index.html">Quang-Cuong Pham</a>. <a class="reference external" href="http://humanoids2017.loria.fr/">Humanoids 2017</a>, Birmingham, United Kingdom, November 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We present a model predictive controller (MPC) for multi-contact locomotion
where predictive optimizations are realized by time-optimal path
parameterization (TOPP). A key feature of this solution is that, contrary to
existing planners where step timings are provided …</p></div><p class="authors"><strong>Stéphane Caron</strong> and <a class="reference external" href="http://www.ntu.edu.sg/home/cuong/index.html">Quang-Cuong Pham</a>. <a class="reference external" href="http://humanoids2017.loria.fr/">Humanoids 2017</a>, Birmingham, United Kingdom, November 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We present a model predictive controller (MPC) for multi-contact locomotion
where predictive optimizations are realized by time-optimal path
parameterization (TOPP). A key feature of this solution is that, contrary to
existing planners where step timings are provided as inputs, here the timing
between contact switches is computed as <em>output</em> of a fast nonlinear
optimization. This is appealing to multi-contact locomotion, where proper
timings depend on terrain topology and suitable heuristics are unknown. We show
how to formulate legged locomotion as a TOPP problem and demonstrate the
behavior of the resulting TOPP-MPC controller in simulations with a model of
the HRP-4 humanoid robot.</p>
</div>
<div class="section" id="video">
<h2>Video</h2>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/humanoids-2017.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01363757/document">Paper</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/topp-mpc">Source code</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2017.8246922">10.1109/HUMANOIDS.2017.8246922</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="presentation">
<h2>Presentation</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/humanoids-2017/mc-walking-lmpc.pdf">Poster 1 — Linear MPC for Multi-contact Walking</a> (recap of previous work)</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/humanoids-2017/topp-mpc.pdf">Poster 2 — MPC by Time-optimal Retiming</a> (this paper)</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/humanoids-2017/dynamic-walking-nmpc.pdf">Poster 3 — Nonlinear MPC for Dynamic Walking</a> (later work, better behavior)</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/humanoids-2017/boundedness-mpc.pdf">Poster 4 — Boundedness MPC in 3D</a> (latest works, still ongoing!)</td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">caron2017humanoids</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{When to make a step? Tackling the timing problem in multi-contact locomotion by TOPP-MPC}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane and Pham, Quang-Cuong}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE-RAS International Conference on Humanoid Robots}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2017}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">nov</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01363757}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/HUMANOIDS.2017.8246922}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>