Stéphane Caronhttps://scaron.info/2021-10-16T00:00:00+02:00Simple 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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/robotics/least-squares.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 &amp; \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">&quot;quadprog&quot;</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">&quot;quadprog&quot;</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&amp;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">&quot;quadprog&quot;</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">&quot;simple_linear_regression_sklearn(x, y)&quot;</span><span class="p">,</span> <span class="s1">'simple_linear_regression_ls(x, y, solver=&quot;quadprog&quot;)'</span><span class="p">,</span> <span class="s2">&quot;simple_linear_regression_cramer(x, y)&quot;</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">&quot;timeit </span><span class="si">{</span><span class="n">instruction</span><span class="si">}</span><span class="s2">&quot;</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">&gt;</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 &quot;summing the coordinates of <span class="math">$$\bfv_k$$</span>&quot;, 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">&gt;</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 &quot;capture&quot; 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">&quot;cpu&quot;</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">&quot;gpu&quot;</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">&quot;cramer_jax_cpu(jx, jy)&quot;</span><span class="p">,</span> <span class="s2">&quot;cramer_jax_gpu(jx, jy)&quot;</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">&quot;timeit </span><span class="si">{</span><span class="n">instruction</span><span class="si">}</span><span class="s2">&quot;</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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 &quot;polished&quot; 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 &quot;polished&quot; 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/robotics/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 not 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/robotics/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 derives 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).appendChild(mathjaxscript); } </script>Inverse dynamics2021-06-03T00:00:00+02:002021-06-09T00:00:00+02:00Stéphane Carontag:scaron.info,2021-06-03:/robotics/inverse-dynamics.html<p>Inverse dynamics (ID) 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>, inverse dynamics computes the joint torques <span class="math">$$\bftau$$</span> and contact forces <span class="math">$$\bff$$</span> such that the <a class="reference external" href="https://scaron.info/robotics/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p> <div class="math"> \begin{equation*} \begin{array}{rcl} \bfM(\bfq) \qdd …</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).appendChild(mathjaxscript); } </script><p>Inverse dynamics (ID) 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>, inverse dynamics computes the joint torques <span class="math">$$\bftau$$</span> and contact forces <span class="math">$$\bff$$</span> such that the <a class="reference external" href="https://scaron.info/robotics/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 &amp; = &amp; \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \\ \bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd &amp; = &amp; \boldsymbol{0} \end{array} \end{equation*} </div> <p>Mathematically, we can write inverse dynamics as a function <span class="math">$$\mathrm{ID}$$</span> such that:</p> <div class="math"> \begin{equation*} (\bftau, \bff) = \mathrm{ID}(\bfq, \qd, \qdd) \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{ID}$$</span> functions in a given configuration <span class="math">$$\bfq$$</span>.</p> <div class="section" id="recursive-newton-euler-algorithm"> <h2>Recursive Newton-Euler algorithm</h2> <p>The recursive Newton-Euler algorithm (RENA) computes the joint side of inverse dynamics, <em>i.e.</em> it computes joint torques assuming contact forces are known:</p> <div class="math"> \begin{equation*} \bftau = \mathrm{RNEA}(\bfq, \qd, \qdd, \bff) \end{equation*} </div> <p>This algorithm works in two passes.</p> <div class="section" id="forward-pass"> <h3>Forward pass</h3> <p>This first pass computes body velocities <span class="math">$$\bfv_i$$</span> and accelerations <span class="math">$$\bfa_i$$</span>. Starting from the root of the kinematic tree, the motion (velocity or acceleration, let's do velocities) <span class="math">$$\bfv_{i}$$</span> of body <span class="math">$$i$$</span> is computed from the motion <span class="math">$$\bfv_{\lambda(i)}$$</span> of its parent body <span class="math">$$\lambda(i)$$</span> plus the component caused by the velocity <span class="math">$$\dot{q}_i = \bfS_i \qd$$</span> of the joint between them:</p> <div class="math"> \begin{equation*} \bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd \end{equation*} </div> <p>We won't go into the details of this equation here, but to look it up in the references below, <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>. This forward pass is a second-order forward kinematics that also computes inertial forces (of the form <span class="math">$$\bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i$$</span>) along the way.</p> </div> <div class="section" id="backward-pass"> <h3>Backward pass</h3> <p>This second pass compute 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 computed from the force <span class="math">$$\bff_{i+1}$$</span> of its successor body <span class="math">$$i+1$$</span> plus the inertial forces <span class="math">$$\bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i$$</span> computed from motions in the forward pass. The latter correspond to the forces to apply to track the desired joint motions <span class="math">$$\qd, \qdd$$</span> from the current configuration <span class="math">$$\bfq$$</span>.</p> <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">$$\tau_i$$</span> by projecting this 6D vector along the joint axis. This component corresponds to the actuation torque that the joint servo will have to provide. 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> <div class="section" id="to-go-further"> <h2>To go further</h2> <p>RNEA was proposed by <a class="reference external" href="https://doi.org/10.1115/1.3139699">Walker and Orin (1982)</a>. There is an excellent introduction to it in this <a class="reference external" href="https://www.youtube.com/watch?v=ZASVKAlegfQ">video supplement to Modern Robotics</a>. RNEA itself has been implemented in rigid body dynamics libraries, such as <a class="reference external" href="https://www.openrave.org/">OpenRAVE</a> or <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 described 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> <p>Dually to forward kinematics being the &quot;easy&quot; problem relative to <a class="reference external" href="https://scaron.info/robotics/inverse-kinematics.html">inverse kinematics</a>, inverse dynamics is &quot;easy&quot; relative to <a class="reference external" href="https://scaron.info/robotics/forward-dynamics.html">forward dynamics</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).appendChild(mathjaxscript); } </script>Kinematics jargon2021-05-01T00:00:00+02:002021-06-03T00:00:00+02:00Stéphane Carontag:scaron.info,2021-05-01:/robotics/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 &quot;the transform of a rigid body&quot;, although more precise expressions are &quot;the pose of a rigid body&quot; and &quot;the transform from frame A to frame B&quot;.</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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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}} &amp; \frac{1}{2} \| R x - s \|^2_W = \frac{1}{2} (R x - s)^T W (R x - s) \\ \mathrm{subject\ to} &amp; G x \leq h \\ &amp; 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 &quot;close&quot; 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}} &amp; \frac12 x^T P x + q^T x \\ \mathrm{subject\ to} &amp; G x \leq h \\ &amp; 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 &quot;cast&quot; 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 &quot;does not change the optimum of the optimization problem&quot;.</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 &amp; = &amp; (R x - s)^T W (R x - s) \\ &amp; = &amp; 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 page on <a class="reference external" href="https://scaron.info/robotics/least-squares.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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).appendChild(mathjaxscript); } </script>Least squares2020-11-02T00:00:00+01:002020-11-02T00:00:00+01:00Stéphane Carontag:scaron.info,2020-11-02:/robotics/least-squares.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/robotics/inverse-kinematics.html">inverse kinematics</a>, etc. They are related to <a class="reference external" href="https://scaron.info/robotics/quadratic-programming.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/robotics/inverse-kinematics.html">inverse kinematics</a>, etc. They are related to <a class="reference external" href="https://scaron.info/robotics/quadratic-programming.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}} &amp; \frac12 \| R x - s \|^2_W = \frac12 (R x - s)^T W (R x - s) \\ \mathrm{subject\ to} &amp; G x \leq h \\ &amp; 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 &quot;close&quot; 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 &quot;close&quot; 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} &amp; \left\| \left[\begin{array}{ccc} 1 &amp; 2 &amp; 0 \\ -8 &amp; 3 &amp; 2 \\ 0 &amp; 1 &amp; 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} &amp; \left[\begin{array}{ccc} 1 &amp; 2 &amp; 1 \\ 2 &amp; 0 &amp; 1 \\ -1 &amp; 2 &amp; -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 &amp; 0 &amp; \cdots &amp; 0 \\ 0 &amp; w_2 &amp; \ddots &amp; \vdots \\ \vdots &amp; \ddots &amp; \ddots &amp; 0 \\ 0 &amp; \cdots &amp; 0 &amp; 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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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 &quot;pole&quot; 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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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/robotics/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/robotics/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/robotics/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/robotics/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 &quot;the area between the feet&quot; 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/robotics/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 &amp; = &amp; \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ(\bfq)^\top \bff \\ \bfF(\bfq) \bff &amp; \leq &amp; \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/robotics/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 &amp; = &amp; \omega^2 (\bfp_G - \bfp_Z) \\ \bfA(\bfp_G) \bfp_Z &amp; \leq &amp; \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/robotics/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/robotics/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 &quot;convex hull of ground contact points&quot;, 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 &gt; 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/robotics/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 &amp; = &amp; y_G f^z - h f^y \\ \sum_{i} -x_i f_i^z &amp; = &amp; -x_G f^z + h f^x \\ \sum_{i} (x_i f_i^y - y_i f_i^x) &amp; = &amp; 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 &amp; = &amp; y_G m g (h - 0) / h = y_G m g \\ h f^y &amp; = &amp; m g (y_G - y_Z) \\ y_G f^z - h f^y &amp; = &amp; m g y_Z \\ mg y_Z &amp; = &amp; \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 &amp; = &amp; \sum_{i} \alpha_i \bfp_i \\ \forall i, \alpha_i &amp; \geq &amp; 0 \end{array} \end{equation*} </div> <p>which is the <a class="reference external" href="https://scaron.info/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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 &quot;best-effort&quot; 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="/presentations/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{\&#39;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="/presentations/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 &quot;standing&quot; or &quot;walking&quot;, 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 &quot;divergent&quot;. 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 &quot;ZMP&quot; by &quot;contact wrench&quot; and &quot;capture point&quot; by &quot;4D DCM&quot;):</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). &quot;Solving infinite-time horizon&quot; 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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/robotics/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/robotics/equations-of-motion.html#point-contacts-and-contact-forces">point contacts</a> or <a class="reference external" href="https://scaron.info/robotics/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/robotics/equations-of-motion.html#inertia-matrix-and-coriolis-tensor">compute these matrices</a> and <a class="reference external" href="https://scaron.info/robotics/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} &amp; \frac{1}{2} \| \bfa - \qdd_{\mathrm{free}}\|_{\bfM}^2 \\ \textrm{subject to} &amp; \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/robotics/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 &amp; \wedge &amp; \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 very good a 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 make our calculations convenient. Defining them as the dual multipliers of motion constraints is a key conceptual step in Lagrangian mechanics. One way to think about this step is to ponder the more general question: &quot;What does it mean to define [force, mass, charge, ...]?&quot; Poincaré's <a class="reference external" href="https://en.wikipedia.org/wiki/Science_and_Hypothesis">Science and Hypothesis</a> (1902) shines a refreshing light on this topic.</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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{\&#39;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=&amp;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{\&#39;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{\&#39;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{\&#39;e}vin and Kanehiro, Fumio and Rabat{\&#39;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:/presentations/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 &quot;smart&quot; 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 &amp; 1 \\ 0 &amp; +1 \end{bmatrix}$$</span> for <span class="math">$$\mathrm{eig}(A) = \{2, 1\}$$</span></li> <li><span class="math">$$A = \begin{bmatrix} -2 &amp; 1 \\ 0 &amp; +1 \end{bmatrix}$$</span> for <span class="math">$$\mathrm{eig}(A) = \{-2, 1\}$$</span></li> <li><span class="math">$$A = \begin{bmatrix} -2 &amp; 1 \\ 0 &amp; -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 &quot;of motion&quot; part. Previously, when the DCM was directly computed by linear combination of the CoM position and velocity, it was clear that &quot;it diverges&quot; and &quot;it is a component of motion&quot; 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 &gt; 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 &quot;classes&quot; 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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{\&#39;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:/robotics/capture-point.html<p>The <em>capture point</em> is a characteristic point of the <a class="reference external" href="https://scaron.info/robotics/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/robotics/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/robotics/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="/presentations/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/robotics/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 &amp; = &amp; \omega (\bfp_C - \bfp_Z) \\ \bfpd_G &amp; = &amp; \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="/presentations/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/linear-inverted-pendulum-model.html<p>The linear inverted pendulum model 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/robotics/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 …</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).appendChild(mathjaxscript); } </script><p>The linear inverted pendulum model 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/robotics/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/robotics/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/robotics/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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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 robot in Multi-contact and Sliding Scenarios}</span><span class="p">,</span> <span class="na">author</span> <span class="p">=</span> <span class="s">{Samadi, Saeid and Caron, St{\&#39;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{\&#39;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:/presentations/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&amp;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 NASA-Caltech JPL</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="/teaching/floating-base-estimation.html">Floating base observer</a> used in this controller</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{\&#39;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> &quot;if everything goes well&quot;™) 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 &amp; 0 &amp; -\mu &amp; 0 &amp; 0 &amp; 0 \\ +1 &amp; 0 &amp; -\mu &amp; 0 &amp; 0 &amp; 0 \\ 0 &amp; -1 &amp; -\mu &amp; 0 &amp; 0 &amp; 0 \\ 0 &amp; +1 &amp; -\mu &amp; 0 &amp; 0 &amp; 0 \\ 0 &amp; 0 &amp; -Y &amp; -1 &amp; 0 &amp; 0 \\ 0 &amp; 0 &amp; -Y &amp; +1 &amp; 0 &amp; 0 \\ 0 &amp; 0 &amp; -X &amp; 0 &amp; -1 &amp; 0 \\ 0 &amp; 0 &amp; -X &amp; 0 &amp; +1 &amp; 0 \\ -Y &amp; -X &amp; -(X + Y) \mu &amp; +\mu &amp; +\mu &amp; -1 \\ -Y &amp; +X &amp; -(X + Y) \mu &amp; +\mu &amp; -\mu &amp; -1 \\ +Y &amp; -X &amp; -(X + Y) \mu &amp; -\mu &amp; +\mu &amp; -1 \\ +Y &amp; +X &amp; -(X + Y) \mu &amp; -\mu &amp; -\mu &amp; -1 \\ +Y &amp; +X &amp; -(X + Y) \mu &amp; +\mu &amp; +\mu &amp; +1 \\ +Y &amp; -X &amp; -(X + Y) \mu &amp; +\mu &amp; -\mu &amp; +1 \\ -Y &amp; +X &amp; -(X + Y) \mu &amp; -\mu &amp; +\mu &amp; +1 \\ -Y &amp; -X &amp; -(X + Y) \mu &amp; -\mu &amp; -\mu &amp; +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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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{\&#39;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:/robotics/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/robotics/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/robotics/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">&lt;</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">&lt;=</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">&quot;__main__&quot;</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">&quot;Standing&quot;</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">&quot;DoubleSupport&quot;</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">&quot;SingleSupport&quot;</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">&quot;Unknown state: &quot;</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 &quot;start&quot; and &quot;run&quot; 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">&quot;Standing&quot;</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">&lt;</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">&quot;DoubleSupport&quot;</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">&lt;=</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">&quot;SingleSupport&quot;</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">&lt;=</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">&lt;</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 &quot;flat foot&quot; 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/robotics/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/robotics/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 &amp; = &amp; m \bfg + \sum_i \bff_i \\ \dot{\bfL}_G &amp; = &amp; \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/robotics/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/robotics/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/robotics/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 &quot;stable&quot; 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 &amp; T &amp; \frac{T^2}{2} \\ 0 &amp; 1 &amp; T \\ 0 &amp; 0 &amp; 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 &amp; 0 &amp; -\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 &amp; 0 &amp; -\frac{h}{g} \\ -1 &amp; 0 &amp; +\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">&lt;</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">&lt;=</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">&lt;</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</tt> for the sagittal direction and <tt class="docutils literal">e</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">&gt;=</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">&quot;DoubleSupport&quot;</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 == &quot;SingleSupport&quot;:</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 the <a class="reference external" href="/presentations/jrl-2019.html">divergent component of motion (DCM)</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/floating-base-estimation.html<p>For a legged system, locomotion is about controlling the <a class="reference external" href="https://scaron.info/robotics/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/robotics/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&amp;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 &amp; = &amp; {}^B \bfu \times {}^W \bfu \\ c &amp; = &amp; {}^B \bfu \cdot {}^W \bfu \\ \bfR^g &amp; = &amp; \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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/presentations/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:/presentations/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:/presentations/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> <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> <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">Video</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:/robotics/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> &lt;openhrp3-folder&gt; ./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:/robotics/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/robotics/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/robotics/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/robotics/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/robotics/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-400px" 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} &amp; = &amp; \omega (\bfxi - \bfp_Z) \\ \bfpd_G &amp; = &amp; \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 &quot;captured&quot;) 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 &quot;spent&quot; 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 &quot;controlled fall&quot;). 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-450px" 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 &lt; 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/robotics/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/robotics/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 here.</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> and <em>hybrid zero dynamics</em> which powers the <a class="reference external" href="https://www.youtube.com/watch?v=1fC7b2LjVW4">DURUS</a> biped.</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).appendChild(mathjaxscript); } </script>Forward dynamics2018-06-03T00:00:00+02:002021-06-03T00:00:00+02:00Stéphane Carontag:scaron.info,2018-06-03:/robotics/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/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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/robotics/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 &amp; = &amp; \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \\ \bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd &amp; = &amp; \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, \tau, \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="articulated-body-algorithm"> <h2>Articulated body algorithm</h2> <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"> <h2>Composite rigid body algorithm</h2> <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 class="section" id="to-go-further"> <h2>To go further</h2> <p>Both ABA and CRBA come from 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/presentations/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 &quot;capture problems&quot; 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 &quot;disrupt&quot; the improvements thus obtained on the &quot;QP part&quot; 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 &quot;straight out of the textbook&quot; 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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{\&#39;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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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/robotics/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">&lt;</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">&lt;</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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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:/robotics/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 &amp; 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:/robotics/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/robotics/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 &amp; 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')).appendChild(configscript); (document.body || document.getElementsByTagName('head')).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>&gt; <span class="o">(</span>0x0000...<span class="o">)</span> libboost_numpy.so.1.64.0 <span class="o">=</span>&gt; /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>&gt; /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>&gt; <span class="o">(</span>0x00007ffc8f5c7000<span class="o">)</span> libboost_numpy.so.1.64.0 <span class="o">=</span>&gt; not found libboost_python.so.1.64.0 <span class="o">=</span>&gt; 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 &quot;not found&quot; 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>&gt; <span class="o">(</span>0x0000...<span class="o">)</span> libboost_numpy.so.1.64.0 <span class="o">=</span>&gt; /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>&gt; /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{\&#39;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{\&#39;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{\&#39;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> Pendular models for walking over rough terrains2017-10-19T00:00:00+02:002017-10-19T00:00:00+02:00Stéphane Carontag:scaron.info,2017-10-19:/presentations/uniroma-2017.html<p class="authors">Talk given at the <a class="reference external" href="http://www.dis.uniroma1.it/node/12929">University of Rome &quot;La Sapienza&quot;</a> on 19 October 2017, and at the <a class="reference external" href="https://jnrh2017.sciencesconf.org/">Journées Nationales de la Robotique Humanoïde</a> on 20 June 2017.</p> <div class="section" id="abstract"> <h2>Abstract</h2> <p>The Newton-Euler equations that govern multi-body systems are not integrable in general. However, they become so in the pendular mode, a specific way …</p></div><p class="authors">Talk given at the <a class="reference external" href="http://www.dis.uniroma1.it/node/12929">University of Rome &quot;La Sapienza&quot;</a> on 19 October 2017, and at the <a class="reference external" href="https://jnrh2017.sciencesconf.org/">Journées Nationales de la Robotique Humanoïde</a> on 20 June 2017.</p> <div class="section" id="abstract"> <h2>Abstract</h2> <p>The Newton-Euler equations that govern multi-body systems are not integrable in general. However, they become so in the pendular mode, a specific way of moving where conservation of the angular momentum is enforced. This property was successfully showcased for locomotion over horizontal floors (2D locomotion) by walking-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 tilted contacts, 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> </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/uniroma-2017.pdf">Slides of the UniRoma version</a> (October 2017)</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/jnrh-2017.pdf">Slides of the JNRH version</a> (June 2017)</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-01349880/document">Paper on the COM-accel model</a> (Humanoids 2016)</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> (IROS 2017)</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>In this presentation, you linearized friction cones using the inner-pyramid approximation. What is the cost of this approximation? Why not using the full second-order friction cones?</strong></p> <blockquote> <p>To answer the first part of your question, in my opinion the cost of this approximation is small: the forces we miss in the corner volumes (between the second-order cone and its inner pyramids) are extreme values anyway, and we want to avoid them in practice. This is to be balanced with the benefits of working with polytopes: (1) the class of optimization problems becomes LP or QP, and these problems are usually solved faster than the SOCPs you get with second-order cones,* and (2) polyhedral manipulations allow you to compute more complex objects, such as <a class="reference external" href="/publications/humanoids-2017-samy.html">torque-limited friction cones</a>. By the way, second-order friction cones are themselves an outer approximation of a more complex phenomenon, see <em>e.g.</em> Coulomb-Orowan pseudo-codes.</p> <blockquote> <p>The debate is not closed at any rate. As a matter of fact it is still ongoing in my team as well, where Hervé Audren uses SOCPs in his computation of <a class="reference external" href="https://hal-lirmm.ccsd.cnrs.fr/lirmm-01477362/document">robust static-equilibrium polytopes</a>.</p> <p>* It is not true in general that &quot;SOCP is slower than LP/QP&quot;, but in my experience it is the case in force optimization.</p> </blockquote> </blockquote> <p><strong>You added friction to your model but for walking on flat floors it is usually neglected. What is the motivation for that?</strong></p> <blockquote> <p>At the end of the day the motivation is always experimental: with common friction coefficients (0.7 and more), the most stringent contact-stability constraint in practice is the ZMP one. When you don't need to model friction to get your experiments to work, it makes sense not to model it. Now, this observation becomes less and less true when your contacts are <em>tilted</em> (especially when they are tilted outward since angular-momentum conservation tends to get forces pointing to the CoM), so for general walking over rough terrains you want to consider modelling friction.</p> <p>The second good reason for doing that is that modelling friction is <strong>easy</strong> in terms of polyhedra (look e.g. at the condition in the FIP paper). The hard part is always the ZMP condition, which you cannot do without at any rate.</p> </blockquote> <p><strong>How do you implement swing foot trajectories in your solution?</strong></p> <blockquote> We interpolate Hermit curves with parameters selected to avoid sharp foot accelerations (HOUBA curves, detailed in <a class="reference external" href="/publications/humanoids-2017.html">Section IV of this paper</a>). There is nothing special here, actually I believe a better way to do these polynomial interpolations is the one given in Section III of the paper <em>Foot-guided Agile Control of a Biped Robot through ZMP Manipulation</em> by Sugihara and Yamamoto (IROS 2017).</blockquote> <p><strong>In the pendular model you keep a constant angular momentum, while other motion generation techniques* also optimize over kinematics to take it into account. What's missing in your solution to make the angular momentum a proper control?</strong></p> <blockquote> <p>* These other solutions are: <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2014.7041375">Dai et al. (2014)</a> and <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2015.7363464">Herzog et al. (2015)</a>.</p> <p>Angular momentum is definitely one of the next big questions. It is an open question even for 2D walking, while here we are doing 3D walking over rough terrains! There are a number of properties that explain why it is not as straightforward as COM control: its nonholonomy (see <a class="reference external" href="https://hal.inria.fr/inria-00390428/file/Final.pdf">Wieber (2009)</a>) and dependence on the full kinematic state (joint angles and velocities). Hence the idea of a kinematics optimization coupled with centroidal dynamics. To the best of my knowledge today, these solutions have been applied so far to <em>motion generation</em> (<em>i.e.</em> with more computation time, not in a closed control loop). When developing the two walking pattern generators presented here, the first problems were to be (1) general enough for 3D walking and (2) fast enough for the control loop. Now that we've made a first step here, I'll be definitely looking forward to the angular momentum in the future.</p> </blockquote> </div> Multi-Contact Interaction Force Sensing from Whole-Body Motion Capture2017-10-10T00:00:00+02:002017-10-10T00:00:00+02:00Stéphane Carontag:scaron.info,2017-10-10:/publications/tii-2017.html<p class="authors"><a class="reference external" href="https://hoa.pm/">Tu-Hoa Pham</a>, <strong>Stéphane Caron</strong> and <strong>Abderrahmane Kheddar</strong>. IEEE Transactions on Industrial Informatics. Submitted November 2016. Published October 2017.</p> <div class="section" id="abstract"> <h2>Abstract</h2> <p>We present a novel technique that unobtrusively estimates forces exerted by human participants in multi-contact interaction with rigid environments. Our method uses motion capture only, thus circumventing the need to setup …</p></div><p class="authors"><a class="reference external" href="https://hoa.pm/">Tu-Hoa Pham</a>, <strong>Stéphane Caron</strong> and <strong>Abderrahmane Kheddar</strong>. IEEE Transactions on Industrial Informatics. Submitted November 2016. Published October 2017.</p> <div class="section" id="abstract"> <h2>Abstract</h2> <p>We present a novel technique that unobtrusively estimates forces exerted by human participants in multi-contact interaction with rigid environments. Our method uses motion capture only, thus circumventing the need to setup cumbersome force transducers at all potential contacts between the human body and the environment. This problem is particularly challenging, as the knowledge of a given motion only characterizes the resultant force, which can generally be caused by an infinity of force distributions over individual contacts. We collect and release a large-scale dataset on how humans instinctively regulate interaction forces on diverse multi-contact tasks and motions. The force estimation framework we propose leverages physics-based optimization and neural networks to reconstruct force distributions that are physically realistic and compatible with real interaction force patterns. We show the effectiveness of our approach on various locomotion and multi-contact scenarios.</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-01610928/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/TII.2017.2760912">10.1109/TII.2017.2760912</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">pham2017tii</span><span class="p">,</span> <span class="na">title</span> <span class="p">=</span> <span class="s">{Multi-Contact Interaction Force Sensing from Whole-Body Motion Capture}</span><span class="p">,</span> <span class="na">author</span> <span class="p">=</span> <span class="s">{Pham, Tu-Hoa and Caron, St{\&#39;e}phane and Kheddar, Abderrahmane}</span><span class="p">,</span> <span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Transactions on Industrial Informatics}</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">doi</span> <span class="p">=</span> <span class="s">{10.1109/TII.2017.2760912}</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="p">}</span> </pre></div> </div>