Stéphane Caronhttps://scaron.info/2017-09-24T00:00:00+02:00Dynamic Walking over Rough Terrains by Nonlinear Predictive Control of the Floating-base Inverted Pendulum2017-09-24T00:00:00+02:00Stéphane Carontag:scaron.info,2017-09-24:research/iros-2017.html<p class="authors"><strong>Stéphane Caron</strong>, <strong>Abderrahmane Kheddar</strong>. To be presented at <a class="reference external" href="http://www.iros2017.org/">2017 IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS)</a>, September 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We present a real-time rough-terrain dynamic walking pattern generator. Our
method automatically finds step durations, which is a critical issue over rough
terrains where they depend on terrain topology. To achieve this level of
generality, we introduce the Floating-base Inverted Pendulum (FIP) model where
the center of mass can translate freely and the zero-tilting moment point is
allowed to leave the contact surface. We show that this model is equivalent to
the linear-inverted pendulum mode with variable center of mass height, aside
from the fact that its equations of motion remain linear. Our design then
follows three steps: (i) we characterize the FIP contact-stability condition;
(ii) we compute feedforward controls by solving a nonlinear optimization over
receding-horizon FIP trajectories. Despite running at 30 Hz in a
model-predictive fashion, simulations show that the latter is too slow to
stabilize dynamic motions. To remedy this, we (iii) linearize FIP feedback
control computations into a quadratic program, resulting in a constrained
linear-quadratic regulator that runs at 300 Hz. We finally demonstrate our
solution in simulations with a model of the HRP-4 humanoid robot, including
noise and delays over both state estimation and foot force control.</p>
<img alt="HRP-4 walking down an elliptic staircase using our dynamic walking pattern generator" class="noborder max-height-400px align-center" src="https://scaron.info/figures/dynamic-walking.png" />
</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-01481052/document">Pre-print on HAL</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/dynamic-walking">Source code</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/dynamic-walking.mp4">Video</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="cite">
<h2>Cite</h2>
<div class="highlight"><pre><span></span>@inproceedings{caron2017iros,
title = {Dynamic Walking over Rough Terrains by Nonlinear Predictive Control of the Floating-base Inverted Pendulum},
author = {Caron, St{\'e}phane and Kheddar, Abderrahmane},
booktitle = {Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on},
year = {2017},
month = {September},
note = {to be presented},
hal_id = {hal-01481052},
hal_version = {v1},
url = {https://hal.archives-ouvertes.fr/hal-01481052},
}
</pre></div>
</div>
<div class="section" id="q-a">
<h2>Q & A</h2>
<p>Feel free to write me directly about any question you have on this work.</p>
</div>
Pendular models for walking over rough terrains2017-06-20T00:00:00+02:00Stéphane Carontag:scaron.info,2017-06-20:research/jnrh-2017.html<p class="authors">Talk given at the <a class="reference external" href="https://jnrh2017.sciencesconf.org/">Journées Nationales de la Robotique Humanoïde (JNRH)</a>, June 2017.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The Newton-Euler equations that goven multi-body systems are not integrable in
general, so that forward integration in time requires approximations (such as
the Runge-Kutta method). However, in the pendular mode, obtained by enforcing
conservation of the angular momentum at the center of mass, these equations can
become integrable. This property was successfully showcased for walking over
horizontal floors (2D locomotion) by pattern generators based on the LIPM and
CART-table models. In this talk, we will see how to generalize these two models
to 3D locomotion while taking into account both friction and non-tilting
constraints, resulting into the FIP (3D version of LIPM) and COM-accel (3D
version of CART-table) models. We will demonstrate both approaches in live
simulations with the HRP-4 humanoid model.</p>
<img alt="HRP-4 descendant un escalier elliptique (motif généré avec un modèle pendulaire)" class="noborder max-height-400px align-center" src="https://scaron.info/figures/dynamic-walking.png" />
</div>
<div class="section" id="resume">
<h2>Résumé</h2>
<p>Dans cet exposé, nous revenons sur les modèles pendulaires pour la marche en
terrain accidenté. Ceux-ci présentent deux composantes (à vue de nez)
distinctes : l'équation différentielle qui régit la dynamique du système, et
les contraintes d'inégalité qui limitent son actuation. Deux angles d'attaque
pointent ici le bout de leur nez. Le premier procède directement des équations
de Newton-Euler(Z) : la dynamique du système est non-linéaire tandis que ses
contraintes sont linéaires. Une des difficultés est alors d'intégrer le système
entre deux points de collocation (par exemple avec la méthode de Runge-Kutta).
La seconde approche change de commande pour rendre la dynamique linéaire et
invariante, rendant l'intégration exacte au prix de nouvelles contraintes
non-linéaires. Fourrant notre nez dans cette direction, nous présenterons le
modèle du pendule inversé flottant (PIF) utilisé dans notre générateur de
marche tout-terrain. Nous lancerons enfin quelques simulations de l'humanoïde
HRP-4 traversant un escalier circulaire et accidenté, normalement sans se
casser le nez.</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/files/jnrh-2017/slides.pdf">Slides</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-01349880/document">Paper on the COM-accel model</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-01481052/document">Paper on the FIP model</a></td>
</tr>
</tbody>
</table>
</div>
Solving an Ordinary Differential Inequality2017-05-31T00:00:00+02:00Stéphane Carontag:scaron.info,2017-05-31:blog/solving-an-ordinary-differential-inequality.html<p>In his 2008 paper <a class="reference external" href="https://hal.archives-ouvertes.fr/inria-00390555/document">Viability and Predictive Control for Safe Locomotion</a>, Pierre-Brice
Wieber discusses what happens when the center-of-mass of a humanoid reaches the
boundary of its supporting contact area. Mathematically, this gives rise to the
Ordinary Differential Inequality (ODI):</p>
<div class="math">
\begin{equation*}
\frac{\ddot{x}}{\omega^2} \ \geq \ x + b
\end{equation*}
</div>
<p>Under the boundary condition:</p>
<div class="math">
\begin{equation*}
x(t_0) + b \ = \ 0
\end{equation*}
</div>
<p>The paper provides an analytical solution to the ODI as follows:</p>
<div class="math">
\begin{equation*}
x(t) \ \geq \ \frac{\xd(t_0)}{\omega} \sinh\left(\frac{t}{\omega}\right) - b
\end{equation*}
</div>
<p>Let us now detail how to get there.</p>
<div class="section" id="bounding-by-an-ode-solution">
<h2>Bounding by an ODE solution</h2>
<p>The method to derive a lower-bound on ODI solutions relies on a "parallel"
solution to the related Ordinary Differential Equation (ODE):</p>
<blockquote>
<strong>Theorem (Petrovitsch, 1901):</strong> let <span class="math">\(\dot{\gamma} \geq f(\gamma, t)\)</span>
denote an ODI, and let <span class="math">\(\eta\)</span> denote a solution to the ODE
<span class="math">\(\dot{\eta} = f(\eta, t)\)</span> subject to the boundary condition
<span class="math">\(\eta(t_0) = \gamma(t_0)\)</span>. Then,</blockquote>
<div class="math">
\begin{equation*}
\begin{array}{r}
\forall t < t_0, \ \gamma(t) \leq \eta(t) \\
\forall t > t_0, \ \gamma(t) \geq \eta(t)
\end{array}
\end{equation*}
</div>
<p>In our case, a solution to the ODE associated with our second-order ODI
satisfies:</p>
<div class="math">
\begin{equation*}
\frac{\ydd}{\omega^2} \ = \ y + b
\end{equation*}
</div>
<p>It is well known that the set of solutions to such second-order differential
equations, whose characteristic polynomial has real roots, are linear
combinations of <a class="reference external" href="https://en.wikipedia.org/wiki/Hyperbolic_function">hyperbolic functions</a>, plus the non-homogeneous
part of the solution:</p>
<div class="math">
\begin{equation*}
y = \alpha \cosh(\omega (t - t_0)) + \beta \sinh(\omega (t - t_0)) - b
\end{equation*}
</div>
<p>We now choose the boundary conditions such that <span class="math">\(y(t_0) = x(t_0) = -b\)</span> and
<span class="math">\(\yd(t_0) = \xd(t_0)\)</span>, resulting in:</p>
<div class="math">
\begin{equation*}
y \ = \ \frac{\xd(t_0)}{\omega} \sinh\left(\frac{t}{\omega}\right) - b
\end{equation*}
</div>
<p>We cannot apply Petrovitsch's theorem directly as our variable <span class="math">\(\gamma =
(x, \xd)\)</span> is two-dimensional, but the underlying idea is the same. Consider the
difference <span class="math">\(\delta\)</span> between <span class="math">\(x\)</span> (ODI solution) and <span class="math">\(y\)</span> (ODE
solution). Then:</p>
<div class="math">
\begin{equation*}
\begin{array}{r}
\ddot{\delta} \geq \delta \\
\delta(t_0) = 0 \\
\dot{\delta}(t_0) = 0
\end{array}
\end{equation*}
</div>
<p>Due to the initial condition, either <span class="math">\(\delta\)</span> is uniformly <span class="math">\(0\)</span> for
all <span class="math">\(t \geq t_0\)</span> (in which case the bound is tight), or <span class="math">\(\delta\)</span> is
increasing for <span class="math">\(t \geq t_0\)</span> and thus positive. In both cases, this
difference ends up being positive, so that <span class="math">\(\forall t \geq t_0, x(t) \geq
y(t)\)</span> and <span class="math">\(y\)</span> is indeed an analytical lower-bound to all solutions of the
ODI.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '//cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
mathjaxscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'AMS' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: 'center'," +
" displayIndent: '0em'," +
" showMathMenu: true," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }, linebreaks: { automatic: false, width: '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}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" 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]," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" pd: \"{\\\\dot{\\\\bfp}}\"," +
" pdd: \"{\\\\ddot{\\\\bfp}}\"," +
" 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}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); ";
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Quadratic Programming in Python2017-04-24T00:00:00+02:00Stéphane Carontag:scaron.info,2017-04-24:blog/quadratic-programming-in-python.html<p>Quadratic programs are a particular kind of mathematical optimization problems
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
(SVMs)</a>, in robotics to
solve <a class="reference external" href="/teaching/inverse-kinematics.html">inverse kinematics</a>, etc. They are
the first step beyond linear programming (LP) in convex optimization. We will
now see how to solve quadratic programs in Python using a number of available
solvers: CVXOPT, CVXPY, Gurobi, MOSEK, qpOASES and quadprog.</p>
<div class="section" id="standard-form-of-quadratic-programs">
<h2>Standard form of quadratic programs</h2>
<p>A quadratic program (QP) is written in standard form as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\mathrm{minimize} & (1/2) x^T P x + q^T x \\
\mathrm{subject\ to} & G x \leq h \\
& A x = b
\end{array}
\end{equation*}
</div>
<p>Here, <span class="math">\(x\)</span> is the vector of optimization variables <span class="math">\(x_1, \ldots,
x_n\)</span>. The matrix <span class="math">\(P\)</span> and vector <span class="math">\(q\)</span> are used to define any
<em>quadratic</em> objective function on these variables, while the matrix-vector
couples <span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span> are used to define inequality and
equality constraints, respectively. Vector inequalities apply coordinate by
coordinate.</p>
<p>This mathematical construction means the following: a QP finds the minimum of a
quadratic function over a linear set:</p>
<img alt="QP is about finding the minimum of a quadratic function (circles) over a linear set (polygon)" class="center" src="https://scaron.info/images/quadratic-programming.jpg" />
<p>In the 2D illustration above, the level sets of the quadratic function are
represented by ellipses, and the linear set is the blue polygon. Since the
global optimal of the objective function is outside of the polygon, the
solution <span class="math">\(x^*\)</span> of the QP lies at the boundary of the linear set. The set
of linear constraints that are <em>saturated</em> at <span class="math">\(x^*\)</span> is called the <a class="reference external" href="https://en.wikipedia.org/wiki/Active_set_method">active
set</a>, but that's a story for
another post...</p>
<p>Back to the standard form, notice that there is no constant term in the
objective function. Indeed, it would have no effect on the result of the
optimization, which is the location of the solution <span class="math">\(x^*\)</span>. Expressions
like <span class="math">\(\| A x - b \|^2\)</span> are written in standard form as <span class="math">\(x^T (A^T A)
x - 2 (A^T b)^T x\)</span>.</p>
<p>The standard form also assumes without loss of generality that the matrix
<span class="math">\(P\)</span> is symmetric. Any matrix <span class="math">\(M\)</span> can be decomposed as sum of its
symmetric part <span class="math">\(M^+\)</span> and antisymmetric part <span class="math">\(M^-\)</span>, and the latter
yields zero in <span class="math">\(x^T M^- x\)</span>. Note that some QP solvers assume that you
feed them a symmetric cost matrix: they won't check this, and will return wrong
results if you don't.</p>
</div>
<div class="section" id="setting-up-qp-solvers">
<h2>Setting up QP solvers</h2>
<p>The two readily-available QP solvers in Python are CVXOPT and quadprog. They
can be installed by:</p>
<pre class="code bash literal-block">
$ sudo <span class="nv">CVXOPT_BUILD_GLPK</span><span class="o">=</span><span class="m">1</span> pip install cvxopt
$ sudo pip install quadprog
</pre>
<p>CVXOPT uses its own matrix type, and it requires the matrix <span class="math">\(P\)</span> of the
objective function to be symmetric. To be on the safe side, you can wrap it as
follows:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">cvxopt_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="o">=</span><span class="bp">None</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="bp">None</span><span class="p">,</span> <span class="n">A</span><span class="o">=</span><span class="bp">None</span><span class="p">,</span> <span class="n">b</span><span class="o">=</span><span class="bp">None</span><span class="p">):</span>
<span class="n">P</span> <span class="o">=</span> <span class="o">.</span><span class="mi">5</span> <span class="o">*</span> <span class="p">(</span><span class="n">P</span> <span class="o">+</span> <span class="n">P</span><span class="o">.</span><span class="n">T</span><span class="p">)</span> <span class="c1"># make sure P is symmetric</span>
<span class="n">args</span> <span class="o">=</span> <span class="p">[</span><span class="n">matrix</span><span class="p">(</span><span class="n">P</span><span class="p">),</span> <span class="n">matrix</span><span class="p">(</span><span class="n">q</span><span class="p">)]</span>
<span class="k">if</span> <span class="n">G</span> <span class="ow">is</span> <span class="ow">not</span> <span class="bp">None</span><span class="p">:</span>
<span class="n">args</span><span class="o">.</span><span class="n">extend</span><span class="p">([</span><span class="n">matrix</span><span class="p">(</span><span class="n">G</span><span class="p">),</span> <span class="n">matrix</span><span class="p">(</span><span class="n">h</span><span class="p">)])</span>
<span class="k">if</span> <span class="n">A</span> <span class="ow">is</span> <span class="ow">not</span> <span class="bp">None</span><span class="p">:</span>
<span class="n">args</span><span class="o">.</span><span class="n">extend</span><span class="p">([</span><span class="n">matrix</span><span class="p">(</span><span class="n">A</span><span class="p">),</span> <span class="n">matrix</span><span class="p">(</span><span class="n">b</span><span class="p">)])</span>
<span class="n">sol</span> <span class="o">=</span> <span class="n">cvxopt</span><span class="o">.</span><span class="n">solvers</span><span class="o">.</span><span class="n">qp</span><span class="p">(</span><span class="o">*</span><span class="n">args</span><span class="p">)</span>
<span class="k">if</span> <span class="s1">'optimal'</span> <span class="ow">not</span> <span class="ow">in</span> <span class="n">sol</span><span class="p">[</span><span class="s1">'status'</span><span class="p">]:</span>
<span class="k">return</span> <span class="bp">None</span>
<span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">sol</span><span class="p">[</span><span class="s1">'x'</span><span class="p">])</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="n">P</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">],))</span>
</pre>
<p>The quadprog module works directly on NumPy arrays so there is no need for
type conversion. Its matrix representation is equivalent but with different
names:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">quadprog_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="o">=</span><span class="bp">None</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="bp">None</span><span class="p">,</span> <span class="n">A</span><span class="o">=</span><span class="bp">None</span><span class="p">,</span> <span class="n">b</span><span class="o">=</span><span class="bp">None</span><span class="p">):</span>
<span class="n">qp_G</span> <span class="o">=</span> <span class="o">.</span><span class="mi">5</span> <span class="o">*</span> <span class="p">(</span><span class="n">P</span> <span class="o">+</span> <span class="n">P</span><span class="o">.</span><span class="n">T</span><span class="p">)</span> <span class="c1"># make sure P is symmetric</span>
<span class="n">qp_a</span> <span class="o">=</span> <span class="o">-</span><span class="n">q</span>
<span class="k">if</span> <span class="n">A</span> <span class="ow">is</span> <span class="ow">not</span> <span class="bp">None</span><span class="p">:</span>
<span class="n">qp_C</span> <span class="o">=</span> <span class="o">-</span><span class="n">numpy</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">A</span><span class="p">,</span> <span class="n">G</span><span class="p">])</span><span class="o">.</span><span class="n">T</span>
<span class="n">qp_b</span> <span class="o">=</span> <span class="o">-</span><span class="n">numpy</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">b</span><span class="p">,</span> <span class="n">h</span><span class="p">])</span>
<span class="n">meq</span> <span class="o">=</span> <span class="n">A</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="k">else</span><span class="p">:</span> <span class="c1"># no equality constraint</span>
<span class="n">qp_C</span> <span class="o">=</span> <span class="o">-</span><span class="n">G</span><span class="o">.</span><span class="n">T</span>
<span class="n">qp_b</span> <span class="o">=</span> <span class="o">-</span><span class="n">h</span>
<span class="n">meq</span> <span class="o">=</span> <span class="mi">0</span>
<span class="k">return</span> <span class="n">quadprog</span><span class="o">.</span><span class="n">solve_qp</span><span class="p">(</span><span class="n">qp_G</span><span class="p">,</span> <span class="n">qp_a</span><span class="p">,</span> <span class="n">qp_C</span><span class="p">,</span> <span class="n">qp_b</span><span class="p">,</span> <span class="n">meq</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>
</pre>
<p>In case you want to try out other solvers, I implemented similar wrappers for
those I could get my hands on (like Gurobi or MOSEK) in the <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a> module.</p>
</div>
<div class="section" id="example">
<h2>Example</h2>
<p>For a small example, let us see how to solve the following QP:</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 QP matrices in proper format:</p>
<pre class="code python literal-block">
<span class="n">M</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">P</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</span><span class="n">M</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="n">M</span><span class="p">)</span>
<span class="n">q</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</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">M</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="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="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,))</span>
</pre>
<p>Finally, we compute the solution using one of the available QP solvers:</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">cvxopt_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">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="o">-</span><span class="mf">0.49025721</span><span class="p">,</span> <span class="o">-</span><span class="mf">1.57755278</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.66484775</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="n">quadprog_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">Out</span><span class="p">[</span><span class="mi">2</span><span class="p">]:</span> <span class="n">array</span><span class="p">([</span><span class="o">-</span><span class="mf">0.49025721</span><span class="p">,</span> <span class="o">-</span><span class="mf">1.57755261</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.66484801</span><span class="p">])</span>
</pre>
</div>
<div class="section" id="comparing-solver-performances">
<h2>Comparing solver performances</h2>
<p>In the following benchmark, I compared six different solvers. Three of them are
numerical, which is the approach we have seen so far:</p>
<ul class="simple">
<li><a class="reference external" href="http://cvxopt.org">CVXOPT</a></li>
<li><a class="reference external" href="https://projects.coin-or.org/qpOASES">qpOASES</a></li>
<li><a class="reference external" href="https://pypi.python.org/pypi/quadprog/">quadprog</a></li>
</ul>
<p>The three others are symbolic, meaning that if you dig into their API they
allow you to construct your problem formally (with variable names) rather than
using the matrix-vector representation. This is convenient for big sparse
problems, but slower and small problems such as the one we are looking at here.
The three symbolic solvers I tested are:</p>
<ul class="simple">
<li><a class="reference external" href="http://www.cvxpy.org/en/latest/">CVXPY</a></li>
<li><a class="reference external" href="https://www.gurobi.com/">Gurobi</a></li>
<li><a class="reference external" href="https://mosek.com/">MOSEK</a> (as wrapped by CVXOPT)</li>
</ul>
<p>Here is a sample of computation times on my machine:</p>
<ul class="simple">
<li>CVXOPT: 1000 loops, best of 3: 559 µs per loop</li>
<li>CVXPY: 100 loops, best of 3: 2.81 ms per loop</li>
<li>Gurobi: 1000 loops, best of 3: 865 µs per loop</li>
<li>MOSEK: 100 loops, best of 3: 7.24 ms per loop</li>
<li>qpOASES: 10000 loops, best of 3: 31.5 µs per loop</li>
<li>quadprog: 10000 loops, best of 3: 34.1 µs per loop</li>
</ul>
<p>For further investigation, let us generate random problems of arbitrary size as
follows:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">solve_random_qp</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="n">solver</span><span class="p">):</span>
<span class="n">M</span><span class="p">,</span> <span class="n">b</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">n</span><span class="p">,</span> <span class="n">n</span><span class="p">)),</span> <span class="n">random</span><span class="o">.</span><span class="n">random</span><span class="p">(</span><span class="n">n</span><span class="p">)</span>
<span class="n">P</span><span class="p">,</span> <span class="n">q</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</span><span class="n">M</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="n">M</span><span class="p">),</span> <span class="n">dot</span><span class="p">(</span><span class="n">b</span><span class="p">,</span> <span class="n">M</span><span class="p">)</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="n">n</span><span class="p">,))</span>
<span class="n">G</span> <span class="o">=</span> <span class="n">toeplitz</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="mf">0.</span><span class="p">]</span> <span class="o">+</span> <span class="p">[</span><span class="mf">0.</span><span class="p">]</span> <span class="o">*</span> <span class="p">(</span><span class="n">n</span> <span class="o">-</span> <span class="mi">3</span><span class="p">),</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">3.</span><span class="p">]</span> <span class="o">+</span> <span class="p">[</span><span class="mf">0.</span><span class="p">]</span> <span class="o">*</span> <span class="p">(</span><span class="n">n</span> <span class="o">-</span> <span class="mi">3</span><span class="p">))</span>
<span class="n">h</span> <span class="o">=</span> <span class="n">ones</span><span class="p">(</span><span class="n">n</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">solver</span><span class="o">=</span><span class="n">solver</span><span class="p">)</span>
</pre>
<p>The Toeplitz matrix used to generate inequalities is just an upper-tridiagonal
matrix with coefficients 1, 2, 3, all other coefficients being zero. This
matrix is sparse but represented by (dense) NumPy arrays here. Using the
function above, I generated a benchmark for problem sizes ranging from 10 to
2,000, averaging computation times over 10 runs for each point. Here are the
results:</p>
<img alt="Results of the benchmark of QP solvers in Python" class="noborder align-center" src="https://scaron.info/images/qp-benchmark.png" style="width: 750px;" />
<p>The bottom line of this small comparison is that <strong>quadprog</strong>, which implements
the Goldfarb-Idnani dual algorithm, <strong>simply rocks</strong>. More generally,
active-set solvers (quadprog and qpOASES) perform best on these dense problems.
To see the benefit of symbolic solvers (CVXPY or Gurobi), one would have to use
sparse matrix representation, which I didn't do here.</p>
<p>You can try for yourself on your own machine, all scripts and solver wrapping
functions are in the <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a>
repository.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '//cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
mathjaxscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'AMS' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: 'center'," +
" displayIndent: '0em'," +
" showMathMenu: true," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }, linebreaks: { automatic: false, width: '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}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" 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]," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" pd: \"{\\\\dot{\\\\bfp}}\"," +
" pdd: \"{\\\\ddot{\\\\bfp}}\"," +
" 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}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); ";
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Contact modes2017-03-25T19:42:00+01:00Stéphane Carontag:scaron.info,2017-03-25:teaching/contact-modes.html<p>Contacts are temporary degrees of constraints (DOCs) used by mobile robots to
control their motions. They stem from the <em>non-penetrability condition</em>: if
<span class="math">\(d_{ij}\)</span> is the distance between the closest pair of points, one belonging to a
body <span class="math">\(i\)</span> and the other to a body <span class="math">\(j \neq i\)</span>, then
</p>
<div class="math">$$
d_{ij} \geq 0.
$$</div>
<p>
When <span class="math">\(d_{ij} > 0\)</span>, there is no contact and the two bodies may move freely. But
as soon as <span class="math">\(d_{ij} = 0\)</span>, contact appears and constraints the respective motions
of the two bodies.</p>
<h2>Definition from degrees of constraints</h2>
<p>In rigid body dynamics, a body has six degrees of freedom, three for its
translation and three for its orientation. Therefore, a contact can create
between one and six degrees of constraints, depending on the number and
arrangement of contact points between the contacting bodies.</p>
<blockquote>
<p><strong>Definition (<a href="http://doai.io/10.1177%2F0278364902021012003">Balkcom and
Trinkle</a>):</strong>
the <em>mode</em> of a contact is the set of degrees of constraints that it
introduces between the two contacting bodies.</p>
</blockquote>
<p>Common contact modes include:</p>
<ul>
<li><em>Broken</em>: when there is no contact (DOC: 0)</li>
<li><em>Sliding</em>: when there is a relative translation between contact surfaces,
accompanied or not by a rotation along the contact normal (DOC: 3 or 4)</li>
<li><em>Rolling</em>: when one body in contact is rotating around a line on the other
body, accompanied or not by a translation along that line (DOC: 2 or 3)</li>
<li><em>Fixed</em>: when the contact is fully constrained (DOC: 6)</li>
</ul>
<h2>Complementarity in contact modes</h2>
<p>Owing to the complementary relationship between motion and force vectors in
<a href="/teaching/screw-theory.html">screw theory</a>, each degree of constraint on
motions springs a degree of freedom on forces, and <em>vice versa</em>. For instance,</p>
<ul>
<li>A <em>broken</em> contact allows for free motions in the space <span class="math">\(d_{ij} \geq 0\)</span>
delimited by the non-penetration condition, and fully constraints the contact
wrench to <span class="math">\(\boldsymbol{0}\)</span>.</li>
<li>A <em>fixed</em> contact fully constraints motion vectors (velocities,
accelerations, etc.) to <span class="math">\(\boldsymbol{0}\)</span>, and allows for free forces in the
space <span class="math">\(\bfF \bff \leq \boldsymbol{0}\)</span> delimited by the <a href="/teaching/friction-model.html">friction
model</a>.</li>
</ul>
<p>More generally, contact modes can be seen as the states of the discrete system
of contact dynamics. Transitions between modes, known as <a href="/teaching/contact-stability.html">contact
switches</a>, occur when certain guard
conditions are crossed. For instance, when the contact force reaches the
boundary of its friction cone, or when the <a href="/teaching/zero-tilting-moment-point.html">center of
pressure</a> hits the boundary of the
contact area. Here is an example of a subgraph with transitions between contact
modes:</p>
<p><img src="/figures/contact-modes.png" class="center"></p>
<p>(To view full screen: right-click on the image, <em>View Image</em>.) The complete
automaton of the discrete system is symmetric and dense.</p>
<h2>To go further</h2>
<p>The article by <a href="http://doai.io/10.1177/0278364902021012003">(Balkcom and Trinkle,
2002)</a> introduced several
important ideas, including contact modes and the representation of contact
conditions by polyhedral convex sets.</p>
<script type="text/javascript">if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '//cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
mathjaxscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'AMS' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: 'center'," +
" displayIndent: '0em'," +
" showMathMenu: true," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }, linebreaks: { automatic: false, width: '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}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" 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]," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" pd: \"{\\\\dot{\\\\bfp}}\"," +
" pdd: \"{\\\\ddot{\\\\bfp}}\"," +
" 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}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); ";
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Contact stability2017-03-24T19:42:00+01:00Stéphane Carontag:scaron.info,2017-03-24:teaching/contact-stability.html<p>Contact stability is a condition used to check the <em>feasibility</em> of robot
motions. It should not be confused with the notion of (Lyapunov) stability, a
notion from control theory used to formalize the performance of a controller.</p>
<h2>Feasibility of a contact wrench</h2>
<div style="clear: both; width: 250px" class="right">
<img src="/figures/coulomb-foot-wrench.png">
Contact is maintained as long as its wrench stays inside the 6D friction cone.
</div>
<p>Everything starts with a mobile robot, <em>i.e.</em>, a robot that is able to
establish and break temporary contacts with its environment. Think for instance
of a humanoid that alternatively plants its feet on the ground to walk. At each
contact, the sum of forces exerted by the environment on the robot is
represented by a <em>contact wrench</em>. As long as the contact is maintained, this
wrench will lie inside its <a href="/research/icra-2015.html">6D friction cones</a>. The
contact breaks are switches to a different <a href="/teaching/contact-modes.html">contact
mode</a> as soon as its wrench hits the boundaries
of this friction cone.</p>
<h2>Contact stability, modes and switches</h2>
<p>As the robot moves, its <a href="/teaching/newton-euler-equations.html">Newton-Euler equations of
motion</a> induce a coupling between the
motion's whole-body momentum (linear and angular) and all contact wrenches. If
there exists no set of wrenches that simulatenously lie in their friction cones
and sum up to the whole-body momentum, the motion is <em>infeasible</em>. When such
wrenches exist, the motion <em>may be</em> feasible. This latter condition is known as
weak contact stability:</p>
<blockquote>
<p><strong>Definition (<a href="http://doai.io/10.1002/1521-4001(200010)80:10<643::AID-ZAMM643>3.0.CO;2-E">Pang and
Trinkle</a>):</strong>
a motion of the robot is <em>weakly contact-stable</em> if and only if there exists
contact wrenches summing up to its whole-body momentum.</p>
</blockquote>
<p>Weak contact stability is the underlying criterion used in a variety of
humanoid motion planners and pattern generators. Note that, despite its use of
the term "stability", this notion is only a <em>feasibility</em> condition. The
"weakness" of its definition is relative to the more general definition of
contact stability in the context of <a href="/teaching/contact-modes.html">contact
modes</a>:</p>
<ul>
<li>A motion is <em>contact-stable</em> when all its contacts stay in the same contact
mode, <em>i.e.</em>, there is no <em>contact switch</em>.</li>
<li>A motion is <em>weakly</em> contact-stable if it <em>may</em> keep all its contacts in the
same contact mode, <em>i.e.</em>, there exists contact wrenches that support the
motion without changing contact mode.</li>
<li>A motion is <em>strongly</em> contact-stable if it <em>must</em> keep all its contacts in
the same contact mode, that is, all contact wrenches that support the motion
can only be realized without changing contact mode. (This condition is hard
to achieve and rarely used in practice.)</li>
</ul>
<p>The bottom line of these definitions is to avoid contact switches as best as
possible. For a concrete example, think of these switches as follows:</p>
<p><img src="/figures/contact-switching.png" class="center" style="width: 90%"></p>
<p>In practice, only a few contact modes are exploited by current humanoid
controllers. Roughly, only two contact modes: fixed (full) contacts, and no
contact. Detecting contact switches on real robot, or providing a controller
that would be general enough to deal with multiple contact modes, is still a
widely open question, as of 2016.</p>
<h3>Checking contact stability</h3>
<p>In practice, checking contact stability for a given robot motion amounts to
solving a Quadratic Program (QP). Let <span class="math">\((\pdd_G, \dot{\bfL}_G)\)</span> denote the
centroidal momentum of the motion, which is the whole-body momentum taken at
the center of mass <span class="math">\(G\)</span>. Then, feasible contact wrenches exist if and only if a
solution to the following QP can be found:
</p>
<div class="math">\begin{eqnarray}
\underset{\bfw_1, \bfw_2, \ldots}{\textrm{minimize }} & & \sum_i \|\bfw_{C_i}\|^2 \\
\textrm{subject to } & & \sum_i \bff_i = m \pdd_G \\
& & \sum_i (\bfp_{C_i} - \bfp_G) \times \bff_i + \bftau_{C_i} = \dot{\bfL}_G \\
& & \forall i, \bfF \bfR_i^T \bfw_{C_i} \leq 0
\end{eqnarray}</div>
<p>
where <span class="math">\(\bfw_{C_i} = (\bff_i, \bftau_{C_i})\)</span> is the vector of coordinates for
the <span class="math">\(i^\mathrm{th}\)</span> contact wrench taken at a contact point <span class="math">\(C_i\)</span> and in the
world frame. The matrix <span class="math">\(\bfF\)</span> describes a linearized <a href="/teaching/friction-model.html">friction cone
model</a> in the local frame, and is thus applied
after a rotation <span class="math">\(\bfR_i^T\)</span> of wrench coordinates from the world to the local
contact frame.</p>
<h3>Centroidal wrench cone</h3>
<p><img src="/figures/cwc.png" alt="Illustration of the Contact Wrench Cone in
multi-contact" style='width: 300px' class="right"></p>
<p>The <em>centroidal wrench cone</em> (CWC) is a polyhedral convex cone that
characterizes all feasible motions, without solving an optimization problem for
each of them. It is the 6D frictional wrench cone computed for the <em>net</em>
contact wrench, <em>i.e.</em> the sum of all contact wrenches, using <a href="/research/rss-2015.html">numerical
polyhedral projection algorithms</a>.</p>
<p>By construction, a solution can be found to the QP above if and only if the
centroidal momentum <span class="math">\((\pdd_G, \dot{\bfL}_G)\)</span> lies in the CWC. When there is a
large number of momenta to check, it is computationally more efficient to
compute the CWC as a matrix <span class="math">\(\bfU\)</span> first, and then perform each check as <span class="math">\(\bfU
[\pdd_G\ \dot{\bfL}_G] \leq \boldsymbol{0}\)</span> rather than solving a large number
of quadratic programs.</p>
<p>On an advanced note: in the figure above, the CWC is represented by a red force
cone and a green moment cone. This is a drawing convenience: in practice, the
CWC is a 6D cone where force and moment are not independent. If you choose a
resultant force in the red cone, it will affect the shape of the green one. And
conversely, fixing the resultant moment to a given value (e.g. zero, see below)
will affect the shape of the red cone.</p>
<h3>Contact-stability areas and volumes</h3>
<p>When additional constraints are imposed on the centroidal motion, the 6D
centroidal wrench cone reduces to lower-dimensional areas and volumes that can
be used for planning or control:</p>
<ul>
<li>
<p>When the robot is not moving, contact stability is characterized by the
<a href="https://github.com/stephane-caron/3d-com-mpc/tree/master/sep">static equilibrium
polygon</a>: the
configuration of the robot is feasible (sustainable) if and only if the
center of mass lies in a specific polygon, which can be computed efficiently.</p>
</li>
<li>
<p>When the robot moves in the <a href="http://doai.io/10.1109/IROS.2001.973365">Linear Inverted Pendulum
Mode</a> (LIPM, <em>i.e.</em> with conserved
angular momentum and keeping the COM in a plane), the CWC reduces to a
<a href="/research/tro-2016.html">ZMP support area</a>.</p>
</li>
<li>
<p>When the robot moves with a conserved angular momentum <span class="math">\((\Ld_G = 0)\)</span>, the CWC
reduces to a <a href="/research/humanoids-2016.html">3D cone over COM accelerations</a>
that can be used <em>e.g.</em> for multi-contact locomotion.</p>
</li>
</ul>
<h2>To go further</h2>
<p>Stability conditions are used extensively in motion generation. A description
of the computation of the CWC can be found in <a href="/research/rss-2015.html">this
paper</a>, while the algorithms themselves are
implemented in the <a href="https://github.com/stephane-caron/pymanoid/blob/master/pymanoid/contact_set.py">ContactSet class of the pymanoid
library</a>.
For a practical application, you can take a look at this <a href="/research/humanoids-2016.html">multi-contact
humanoid walking pattern generator</a>.</p>
<script type="text/javascript">if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '//cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
mathjaxscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'AMS' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: 'center'," +
" displayIndent: '0em'," +
" showMathMenu: true," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }, linebreaks: { automatic: false, width: '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}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" 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]," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" pd: \"{\\\\dot{\\\\bfp}}\"," +
" pdd: \"{\\\\ddot{\\\\bfp}}\"," +
" 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}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); ";
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Locomotion humanoïde : du sol plat au tout-terrain2017-03-08T00:00:00+01:00Stéphane Carontag:scaron.info,2017-03-08:research/softbank-2017.html<p class="authors">Talk given at <a class="reference external" href="https://www.ald.softbankrobotics.com">SoftBank Robotics Europe (SBRE)</a>, Paris, 8 March 2017.</p>
<div class="section" id="resume">
<h2>Résumé</h2>
<p>Cette présentation retrace les étapes importantes du développement des
contrôleurs de marche pour les robots humanoïdes, depuis la démonstration
publique du Honda P2 en 1997 jusqu'aux récents développements en marche
tout-terrain. Elle n'aborde que des résultats partagés dans le monde ouvert,
chaque slide étant associé à un article de recherche. La première partie
revisite les concepts majeurs qui ont permis de résoudre la locomotion sur sol
plat. La seconde décrit plusieurs développements récents en marche
tout-terrain.</p>
</div>
<div class="section" id="contenu">
<h2>Contenu</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/softbank-2017/slides.pdf">Slides</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>Références</h2>
<div class="section" id="marche-sur-sol-plat">
<h3>Marche sur sol plat</h3>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=d2BUO4HEhvM">Honda humanoid robot P2 (1997)</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="http://groups.csail.mit.edu/drl/journal_club/papers/Hirai98.pdf">The development of Honda humanoid robot (1998)</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="http://users.dimi.uniud.it/~antonio.dangelo/Robotica/dissertations/helper/3D_Linear_Inverted_Pendulum_Model.pdf">The 3D linear inverted pendulum mode: a simple modeling for a
biped walking pattern generation (2001)</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://pdfs.semanticscholar.org/acd2/1a074c22c7a9ccfe6136903024c66ccc5c6e.pdf">Biped walking pattern generation by using preview control of
zero-moment point (2003)</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="http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.138.8014&rep=rep1&type=pdf">Forces acting on a biped robot. center of pressure-zero moment
point (2004)</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/docs/00/39/04/62/PDF/Preview.pdf">Trajectory free linear model predictive control for stable
walking in the presence of strong perturbations (2006)</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td>Bipedal walking control based on capture point dynamics (2011)</td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="marche-tout-terrain">
<h3>Marche tout-terrain</h3>
<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-01256511/document">Model preview control in multi-contact motion—Application to a humanoid robot (2014)</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td>Three-dimensional bipedal walking control based on divergent component of motion (2015)</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-01349880/document">Multi-contact walking pattern generation based on model preview control of 3D COM accelerations (2016)</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-01481052">Dynamic walking over rough terrains by nonlinear predictive control of the floating-base inverted pendulum (2017)</a></td>
</tr>
</tbody>
</table>
</div>
</div>
ZMP Support Areas for Multi-contact Mobility Under Frictional Constraints2016-12-14T00:00:00+01:00Stéphane Carontag:scaron.info,2016-12-14:research/tro-2016.html<p class="authors"><strong>Stéphane Caron</strong>, <a class="reference external" href="http://www.normalesup.org/~pham/">Quang-Cuong Pham</a>,
<strong>Yoshihiko Nakamura</strong>. IEEE Transactions on Robotics (TRO). Submitted 12
October 2015. Accepted 6 September 2016. Published 14 December 2016.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We propose a method for checking and enforcing multi-contact stability based on
the Zero-tilting Moment Point (ZMP). The key to our development is the
generalization of ZMP <em>support areas</em> to take into account (a) frictional
constraints and (b) multiple non-coplanar contacts. We introduce and
investigate two kinds of ZMP support areas. First, we characterize and provide
a fast geometric construction for the support area generated by valid contact
forces, with no other constraint on the robot motion. We call this set the
<em>full support area</em>. Next, we consider the control of humanoid robots using the
Linear Pendulum Mode (LPM). We observe that the constraints stemming from the
LPM induce a shrinking of the support area, even for walking on horizontal
floors. We propose an algorithm to compute the new area, which we call
<em>pendular support area</em>. We show that, in the LPM, having the ZMP in the
pendular support area is a necessary <em>and sufficient</em> condition for contact
stability. Based on these developments, we implement a whole-body controller
and generate feasible multi-contact motions where an HRP-4 humanoid locomotes
in challenging multi-contact scenarios.</p>
<img alt="Two kinds of ZMP support area" class="noborder padtop align-center" src="https://scaron.info/images/two-areas.png" style="width: 80%;" />
</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/papers/journal/caron-tro-2016.pdf">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://scaron.info/videos/tro-2016.mp4">Video</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/multi-contact-zmp">Source code</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a href="/slides/jnrh-2016/index.html" target="_blank">Slides</a> (opens in new window/tab for online reading)</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.2016.2623338">10.1109/TRO.2016.2623338</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="cite">
<h2>Cite</h2>
<div class="highlight"><pre><span></span>@article{caron2016tro,
title = {ZMP Support Areas for Multi-contact Mobility Under Frictional Constraints},
author = {Caron, St{\'e}phane and Pham, Quang-Cuong and Nakamura, Yoshihiko},
journal = {IEEE Transactions on Robotics},
year={2017},
volume={33},
number={1},
pages={67-80},
month={Feb},
publisher = {IEEE},
doi = {10.1109/TRO.2016.2623338}
}
</pre></div>
</div>
<div class="section" id="q-a">
<h2>Q & A</h2>
<p>Feel free to write me directly about any question you have on this work.</p>
<p><strong>In Equation (5), the wrench coordinates</strong> <span class="math">\(\boldsymbol{w}^c_{O}\)</span> <strong>are
taken with respect to the origin of the world frame. Why don't you rather take
this wrench at the COM, as is usually done?</strong></p>
<blockquote>
<p>Wrench coordinates are indeed taken at the origin of the world frame (or
any other fixed point, for what matters). Taking screw coordinates at the
origin of the world frame is typical of <a class="reference external" href="http://royfeatherstone.org/spatial/">spatial vector algebra</a>, which was formalized by Roy
Featherstone.</p>
<p>The main reason for working with the CWC at the origin is that it only
depends on the stance (set of contacts), while the CWC taken at the COM
would also depend on COM coordinates. Actually, once you have the former,
it is straightforward to compute the latter using a simple dual
transformation formula, as we described in Section III of the <a class="reference external" href="/research/humanoids-2016.html">following
paper</a>.</p>
</blockquote>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '//cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
mathjaxscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'AMS' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: 'center'," +
" displayIndent: '0em'," +
" showMathMenu: true," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }, linebreaks: { automatic: false, width: '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}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" 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]," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" pd: \"{\\\\dot{\\\\bfp}}\"," +
" pdd: \"{\\\\ddot{\\\\bfp}}\"," +
" 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}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); ";
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Whole-Body Contact Force Sensing From Motion Capture2016-12-13T00:00:00+01:00Stéphane Carontag:scaron.info,2016-12-13:research/sii-2016.html<p class="authors"><a class="reference external" href="https://hoa.pm/">Tu-Hoa Pham</a>, <strong>Adrien Bufort</strong>, <strong>Stéphane Caron</strong>,
<strong>Abderrahmane Kheddar</strong>. The 2016 IEEE/SICE International Symposium on System
Integration (SII), Sapporo, Japan, December 2016. <em>Best Paper Award</em>.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this paper, we challenge the estimation of contact forces backed with
ground-truth sensing in human whole-body interaction with the environment, from
motion capture only. Our novel method makes it possible to get rid of
cumbersome force sensors in monitoring multi-contact motion together with force
data. This problem is very challenging. Indeed, while a given force
distribution uniquely determines the resulting kinematics, the converse is
generally not true in multi-contact. In such scenarios, physics-based
optimization alone may only capture force distributions that are physically
compatible with a given motion rather than the actual forces being applied. We
address this indeterminacy by collecting a large-scale dataset on whole-body
motion and contact forces humans apply in multi-contact scenarios. We then
train recurrent neural networks on real human force distribution patterns and
complement them with a second-order cone program ensuring the physical validity
of the predictions. Extensive validation on challenging dynamic and
multi-contact scenarios shows that the method we propose can outperform
physical force sensing both in terms of accuracy and usability.</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-01372531/document">Pre-print</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/SII.2016.7843975">10.1109/SII.2016.7843975</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="cite">
<h2>Cite</h2>
<div class="highlight"><pre><span></span>@inproceedings{pham2016sii,
title = {Whole-Body Contact Force Sensing From Motion Capture},
author = {Pham, Tu-Hoa and Bufort, Adrien and Caron, St{\'e}phane and Kheddar, Abderrahmane},
booktitle = {System Integration (SII), 2016 IEEE/SICE International Symposium on},
year = {2016},
month = {Dec},
pages = {58-63},
organization = {IEEE/SICE},
doi = {10.1109/SII.2016.7843975},
}
</pre></div>
</div>
Completeness of Randomized Kinodynamic Planners with State-based Steering2016-12-11T00:00:00+01:00Stéphane Carontag:scaron.info,2016-12-11:research/ras-2016.html<p class="authors"><strong>Stéphane Caron</strong>, <strong>Quang-Cuong Pham</strong>, <strong>Yoshihiko Nakamura</strong>. Robotics and
Autonomous Systems (RAS). Submitted 17 November 2015. Accepted 11 December
2016. Published 19 December 2016.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Probabilistic completeness is an important property in motion planning.
Although it has been established with clear assumptions for geometric planners,
the panorama of completeness results for kinodynamic planners is still
incomplete, as most existing proofs rely on strong assumptions that are
difficult, if not impossible, to verify on practical systems. In this paper, we
focus on an important class of kinodynamic planners, namely those that
interpolate trajectories in the state space. We provide a proof of
probabilistic completeness for these planners under assumptions that can be
readily verified from the system’s equations of motion and the user-defined
interpolation function. Our proof relies crucially on a property of
interpolated trajectories, termed second-order continuity (SOC), which we
show is tightly related to the ability of a planner to benefit from denser
sampling. We analyze the impact of this property in simulations on a low-torque
pendulum. Our results show that a simple RRT using a second-order continuous
interpolation swiftly finds solution, while it is impossible for the same
planner using standard Bezier curves (which are not SOC) to find any solution</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/1511.05259v2.pdf">Pre-print on the arXiv</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/rrt-completeness">Source code</a></td>
</tr>
<tr><td><img alt="tar" class="icon" src="https://scaron.info/images/icons/tar.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/pendulum-benchmark.tar.bz2">Benchmark data</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.1016/j.robot.2016.12.002">10.1016/j.robot.2016.12.002</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="cite">
<h2>Cite</h2>
<div class="highlight"><pre><span></span>@article{caron2016ras,
title = {Completeness of Randomized Kinodynamic Planners with State-based Steering},
author = {Caron, St{\'e}phane and Pham, Quang-Cuong and Nakamura, Yoshihiko},
journal = {Robotics and Autonomous Systems},
pages={85--94},
publisher = {Elsevier},
volume={89},
year={2017},
url = {https://arxiv.org/pdf/1511.05259v2.pdf},
doi = {10.1016/j.robot.2016.12.002}
}
</pre></div>
</div>