Stéphane on Locomotionhttps://scaron.info/2022-08-08T00:00:00+02:00Motion control bits for homemade robots2022-07-19T00:00:00+02:002022-07-19T00:00:00+02:00Stéphane Carontag:scaron.info,2022-07-19:/talks/laas-2022.html<p class="authors">Talk given at <a class="reference external" href="https://www.laas.fr/">LAAS-CNRS</a> on 19 July 2022.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<img alt="Picture from the presentation with Upkie onstage" class="side" src="https://scaron.info/images/laas-2022.jpg" />
<p>We have learned a lot from big expensive robots, but their weight and price are likely damping further progress. In this talk, we will follow an alternative made possible by recent innovations in actuators: light homemade robots, of the kind that …</p></div><p class="authors">Talk given at <a class="reference external" href="https://www.laas.fr/">LAAS-CNRS</a> on 19 July 2022.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<img alt="Picture from the presentation with Upkie onstage" class="side" src="https://scaron.info/images/laas-2022.jpg" />
<p>We have learned a lot from big expensive robots, but their weight and price are likely damping further progress. In this talk, we will follow an alternative made possible by recent innovations in actuators: light homemade robots, of the kind that can bump, fall on, or be lifted by us with no harm. By aiming for less actuators and lower complexity, we end up with morphologies that don't fit exactly the bill of previous ideas. This is a good start to revisit them! We will tour some examples from the open source software and hardware of Upkie, a wheeled biped robot that proudly stands on 3D printed parts and sawed broomsticks (among other mechanical marvels).</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/laas-2022.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://peertube.laas.fr/videos/watch/22afb517-ee8c-45ee-ae6c-c1cef70cba45">Recording</a> (42 min)</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/tasts-robots/upkie_locomotion">Motion control software</a> for Upkie</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/user/jamesbruton">James Bruton</a>, great explorator of new morphologies</td>
</tr>
</tbody>
</table>
</div>
Ideas and software for the locomotion of homemade robots2022-06-21T00:00:00+02:002022-06-21T00:00:00+02:00Stéphane Carontag:scaron.info,2022-06-21:/talks/r4-2022.html<p class="authors">Talk given at the <a class="reference external" href="https://www.r4-robotique.fr/">R4 robotics</a> working group on 20 June 2022.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Moving away from big expensive robots to light homemade things (that can bump, fall on or be lifted by us with no harm) is a great way to revisit past design decisions and explore alternatives. Being incented …</p></div><p class="authors">Talk given at the <a class="reference external" href="https://www.r4-robotique.fr/">R4 robotics</a> working group on 20 June 2022.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Moving away from big expensive robots to light homemade things (that can bump, fall on or be lifted by us with no harm) is a great way to revisit past design decisions and explore alternatives. Being incented to make robots cheaper, reduce their number of actuators and complexity, we end up with new morphologies that don't fit exactly the bill of previous ideas. Interesting things lie in this <em>not exactly</em>! In this presentation, we take a look at two existing ideas that are important in legged locomotion, <a class="reference external" href="https://scaron.info/robot-locomotion/contact-stability.html">contact stability</a> and whole-body control, and see how they fit <a class="reference external" href="https://hackaday.io/project/185729-upkie-homemade-wheeled-biped-robot">Upkie</a>, a wheeled biped with a low-cost onboard computer. Another idea comes to the rescue: the theoretical and empirical evidence that <a class="reference external" href="https://arxiv.org/pdf/1907.01805.pdf">balance control is a low-frequency task</a>. We build open source software upon it that allows us to prototype most of our robot's brain in Python, while achieving its goals of balancing, crouching and going places.</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/r4-2022.pdf">Slides</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/tasts-robots">Software</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-02108449/document">Wrench cone inequalities for fixed surface contacts</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://arxiv.org/pdf/1607.08089.pdf">Whole-body impedance control on IHMC's Atlas</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://doi.org/10.1109/IROS40897.2019.8968059">Whole-body admittance control with internal forces</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://arxiv.org/pdf/1809.07073.pdf">Walking controller with whole-body admittance control</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://arxiv.org/pdf/2005.11431.pdf">Whole-body control of Ascento</a></td>
</tr>
</tbody>
</table>
</div>
Kinematics of a symmetric leg2022-05-25T00:00:00+02:002022-05-25T00:00:00+02:00Stéphane Carontag:scaron.info,2022-05-25:/robot-locomotion/kinematics-of-a-symmetric-leg.html<img alt="Kinematics of a symmetric leg" class="right max-width-30pct" src="https://scaron.info/figures/symmetric-leg-kinematics.png" />
<p>In this post, we are taking a look at the kinematics of a simple leg with two links and one knee. Both links have the same length <span class="math">\(\ell\)</span>, making the leg "symmetric" in the sense that the mirror of any configuration with respect to the vertical plane is also a …</p><img alt="Kinematics of a symmetric leg" class="right max-width-30pct" src="https://scaron.info/figures/symmetric-leg-kinematics.png" />
<p>In this post, we are taking a look at the kinematics of a simple leg with two links and one knee. Both links have the same length <span class="math">\(\ell\)</span>, making the leg "symmetric" in the sense that the mirror of any configuration with respect to the vertical plane is also a valid configuration. The joint angles, depicted to the right, are further subject to the following limits:</p>
\begin{align}
| q_1 | & < \frac{\pi}{2} & | q_2 | & < \pi
\end{align}<p>We will derive analytically the forward kinematics of this leg in the sagittal plane, then consider its inverse kinematics for crouching.</p>
<div class="section" id="forward-kinematics">
<h2>Forward kinematics</h2>
<p>Consider the leg depicted in the figure above. The hip angle <span class="math">\(q_1\)</span> and knee angle <span class="math">\(q_2\)</span> fully define the position of every point on every link of the leg, including the origin of the hip <span class="math">\(G\)</span> (this could be the center of gravity of a lumped mass sitting on top of the leg and representing the robot's chassis), the knee <span class="math">\(K\)</span> and the end effector <span class="math">\(C\)</span>. Forward kinematics is the function that maps joint angles <span class="math">\((q_1, q_2)\)</span> to the coordinates <span class="math">\((x_C, z_C) = \mathrm{FK}(q_1, q_2)\)</span> of the end effector.</p>
<p>Our model is an articulated system (the most common case) where each link is connected to exactly one parent link by one (usually <a class="reference external" href="https://scaron.info/robot-locomotion/revolute-joints.html">revolute</a>) joint. For such systems, the forward kinematics function is naturally recursive: the coordinates of a point on a link can be decomposed into (1) a local offset from the joint between the link and its parent, and (2) the coordinates of that joint in the parent link. Applying this general idea to our case, we write:</p>
\begin{align}
x_C & = x_K + (x_C - x_K) \\
z_C & = z_K + (z_C - z_K)
\end{align}<p>The coordinates of the knee point are fully determined by the hip angle and the length of the first link:</p>
\begin{align}
x_K & = x_G + \ell \sin(q_1) \\
z_K & = z_G - \ell \cos(q_1)
\end{align}<p>We see how the recursive argument applies to the knee as well as with the appearance of the coordinates <span class="math">\((x_G, z_G)\)</span> of the origin of the hip, which is also the root of our kinematic tree. Recursion stops there.</p>
<img alt="Deriving the forward kinematics of a symmetric leg" class="right max-width-30pct" src="https://scaron.info/figures/symmetric-leg-kinematics-forward.png" />
<p>Moving on to the second link, the figure to the right illustrates how we get the following sines and cosines:</p>
\begin{align}
x_C - x_K & = \ell \cos\left(\frac{\pi}{2} - q_1 - q_2\right) = \ell \sin(q_1 + q_2) \\
z_C - z_K & = -\ell \sin\left(\frac{\pi}{2} - q_1 - q_2\right) = -\ell \cos(q_1 + q_2)
\end{align}<p>Combining this system of equations with the previous one for <span class="math">\((x_K, z_K)\)</span>, we get the overall formula for the forward kinematics of our leg:</p>
\begin{align}
x_C & = x_G + \ell \sin(q_1) + \ell \sin(q_1 + q_2) \\
z_C & = z_G - \ell cos(q_1) - \ell \cos(q_1 + q_2)
\end{align}<p>This expression has the form <span class="math">\((x_C, z_C) = \mathrm{FK}(q_1, q_2)\)</span>, but the <span class="math">\(\mathrm{FK}\)</span> function implicitly depends on the coordinates <span class="math">\((x_G, z_G)\)</span> of the origin of the hip frame. This frame is known as the <a class="reference external" href="https://scaron.info/robot-locomotion/floating-base-estimation.html">floating base</a> of the mobile robot. We can equivalently consider it as a free joint between the hip and an inertial frame (often called the "world" frame in physics simulators and robotics software), in which case the coordinates of this joint become an additional argument to forward kinematics: <span class="math">\((x_C, z_C) = \mathrm{FK}(x_G, z_G, q_1, q_2)\)</span>. If we had a robotic arm, <span class="math">\(G\)</span> would directly be a point of an inertial frame and we could without loss of generality fix it to, for instance, <span class="math">\(x_G = 0\)</span> and <span class="math">\(z_G = 0\)</span>.</p>
</div>
<div class="section" id="inverse-kinematics-for-crouching">
<h2>Inverse kinematics for crouching</h2>
<img alt="Kinematics of a symmetric leg" class="right max-width-30pct" src="https://scaron.info/figures/symmetric-leg-kinematics.png" />
<p>Let us now compute an inverse kinematics function: given a desired "crouching height" <span class="math">\(h\)</span>, we want to compute the joint angles <span class="math">\((q_1, q_2) = \mathrm{IK}(h)\)</span> such that <span class="math">\(\mathrm{FK}(\mathrm{IK}(h)) = (x_G, z_G - h)\)</span>. Injecting this property in the system of equations we obtained for forward kinematics, this amounts to solve:</p>
\begin{align}
0 & = \sin(q_1) + \sin(q_1 + q_2) \\
h & = \ell cos(q_1) + \ell \cos(q_1 + q_2)
\end{align}<p>One of the trick in the bag to manipulate systems of trigonometric equations is to make the identity <span class="math">\(\cos(x)^2 + \sin(x)^2 = 1\)</span> appear. Let us rewrite the system as:</p>
\begin{align}
\cos(q_1 + q_2) & = \frac{h}{\ell} - \cos(q_1) \\
\sin(q_1 + q_2) & = -\sin(q_1)
\end{align}<p>Taking the sum of the squares of these two lines leads us to:</p>
<div class="math">
\begin{equation*}
1 = \frac{h^2}{\ell^2} - 2 \frac{h}{\ell} \cos(q_1) + 1
\end{equation*}
</div>
<p>Which is great, as we now know that:</p>
<div class="math">
\begin{equation*}
\cos(q_1) = \frac{h}{2 \ell}
\end{equation*}
</div>
<p>Injecting this equation back into the system gives us:</p>
\begin{align}
\cos(q_1 + q_2) & = \frac{h}{\ell} - \cos(q_1) = \frac{h}{2 \ell} = \cos(-q_1) \\
\sin(q_1 + q_2) & = \sin(-q_1)
\end{align}<p>The two angles <span class="math">\(q_1 + q_2\)</span> and <span class="math">\(-q_1\)</span> have the same sine and cosine, therefore they are equal up to some <span class="math">\(2 k \pi, k \in \mathbb{Z}\)</span>. But recall our joint limit assumption:</p>
\begin{align}
| q_1 | & < \frac{\pi}{2} & | q_2 | & < \pi
\end{align}<p>This means <span class="math">\(q_1 + q_2 - (-q_1) = 2 q_1 + q_2 < 2 \pi\)</span>, and the solution to our trigonometric system is unique: <span class="math">\(q_2 = -2 q_1\)</span>. Solving for <span class="math">\(q_1\)</span> alone yields:</p>
\begin{align}
q_1 & = \arccos\left(\frac{h}{2 \ell}\right) \\
q_2 & = -2 \arccos\left(\frac{h}{2 \ell}\right)
\end{align}<p>This is our inverse kinematics function <span class="math">\((q_1, q_2) = \mathrm{IK}(h)\)</span>. We can check that it matches what we expect at the two boundary cases:</p>
<img alt="Kinematics of a crouching leg" class="right max-width-30pct" src="https://scaron.info/figures/symmetric-leg-kinematics-crouch.png" />
<ul class="simple">
<li><strong>Stretched legs:</strong> <span class="math">\(h = 2 \ell\)</span>, then <span class="math">\(q_1 = q_2 \arccos(1) = 0\)</span> and we are indeed in the configuration where the leg is fully vertical.</li>
<li><strong>Full crouch:</strong> <span class="math">\(h = 0\)</span>, then <span class="math">\(q_1 \to +\frac{\pi}{2}\)</span> with a positive sign since <span class="math">\(0 < q_1 < \frac{\pi}{2}\)</span>. Similarly <span class="math">\(q_2 \to -\pi\)</span>, so that the leg is crouching as depicted in the figure to the right.</li>
</ul>
<p>We can feel in the latter case that there is a discontinuity where our IK function can jump and should be used with care, although we will be fine within the continuous domain <span class="math">\(| q_1 | < \frac{\pi}{2}\)</span> that we chose here. In practice the two links would collide before we reach the limit <span class="math">\(h \to 0\)</span>, so the full crouch test here is rather an additional check for us to be convinced that our derivation is correct.</p>
<p>Note how we made another choice along the way: since <span class="math">\(h > 0\)</span>, taking <span class="math">\(q_1 = \arccos(h / 2 \ell)\)</span> implies that <span class="math">\(q_1 > 0\)</span> and the knee will always bend forward (as in human legs). An equally valid solution would be <span class="math">\(q_1 = -\arccos(h / 2 \ell)\)</span>, in which case the knee will bend backward (as in bird legs).</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>For more complex models, it is rare that we derive the forward and inverse kinematics functions of a model analytically, although such a derivation can be done <a class="reference external" href="http://openrave.org/docs/latest_stable/openravepy/ikfast/">automatically for models as complex as 7-DOF robot arms</a>. A more common approach is to write down a <a class="reference external" href="https://wiki.ros.org/urdf/XML">URDF description</a> of the model, use fast rigid-body algorithms for forward kinematics, and <a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">numerical optimization for inverse kinematics</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Open loop and closed loop model predictive control2022-05-12T00:00:00+02:002022-05-12T00:00:00+02:00Stéphane Carontag:scaron.info,2022-05-12:/robot-locomotion/open-closed-loop-model-predictive-control.html<p>There are two ways model predictive control (MPC) has been applied to legged
locomotion so far: open loop and closed loop MPC. In both cases, a model
predictive control (numerical optimization) problem is derived from a model of
the system and solved, providing a sequence of actions that can be …</p><p>There are two ways model predictive control (MPC) has been applied to legged
locomotion so far: open loop and closed loop MPC. In both cases, a model
predictive control (numerical optimization) problem is derived from a model of
the system and solved, providing a sequence of actions that can be used to
drive the actual system. Open loop and closed loop MPC differ in their choice
of initial state.</p>
<div class="section" id="open-loop-model-predictive-control">
<h2>Open loop model predictive control</h2>
<p>Open loop MPC is a motion planning solution where the plan is "unrolled"
progressively. In bipedal walking, it is also known as <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html#walking-pattern-generation">walking pattern
generation</a>. In this
approach, MPC outputs are integrated using the same forward dynamics model as
the one used to derive the MPC problem itself. For instance, that model is
<span class="math">\(\bfx_{k+1} = \bfA \bfx_k + \bfB u_k\)</span> in <a class="reference external" href="https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html#linear-model-predictive-control">linear model predictive control</a>.
The resulting state <span class="math">\(\bfx_{k+1}\)</span> is then used as initial state for the
MPC problem of the next control cycle:</p>
<img alt="Open loop model predictive control" class="center max-width-80pct" src="https://scaron.info/figures/open-loop-model-predictive-control.png" />
<p>Note that the initial state depicted above can be either purely model-based or
observed from sensors. What characterizes open loop MPC is the use of the model
for integration from one step to the next. State can also be reinitialized from
sensors at specific events, such as when the robot stops walking or after an
impact.</p>
<p>One of the first breakthroughs in humanoid walking, the <em>preview control</em>
method <a class="reference external" href="http://doai.io/10.1109/ROBOT.2003.1241826">(Kajita et al., 2003)</a>,
is open loop. It was later shown to be equivalent to linear model predictive
control <a class="reference external" href="http://doai.io/10.1109/ICHR.2006.321375">(Wieber, 2006)</a>. These
seminal papers don't mention the integrator directly, but open loop MPC is how
the generated center of mass trajectories were executed in practice on the
HRP-2 and HRP-4 humanoids. This is explicit in code that was released more
recently, for instance the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">LIPM walking controller</a> from CNRS or
the <a class="reference external" href="https://github.com/isri-aist/CentroidalControlCollection#readme">centroidal control collection</a> from AIST.</p>
</div>
<div class="section" id="closed-loop-model-predictive-control">
<h2>Closed loop model predictive control</h2>
<p>Closed loop MPC is initialized from the latest observation:</p>
<img alt="Closed loop model predictive control" class="center max-width-80pct" src="https://scaron.info/figures/closed-loop-model-predictive-control.png" />
<p>Observations, even when they are filtered, are subject to uncertainty and
measurement errors, which raises new questions and edge cases to handle
compared to open loop MPC. For instance, what if the MPC problem has state
constraints <span class="math">\(\bfC \bfx_k \leq \bfe\)</span>, but the initial state does <em>not</em>
satisfy <span class="math">\(\bfC \bfx_0 \leq \bfe\)</span>?</p>
<p>This question was encountered by <a class="reference external" href="http://doai.io/10.1109/IROS.2017.8206174">(Bellicoso et al., 2017)</a> in the case of <a class="reference external" href="https://scaron.info/robot-locomotion/zmp-support-area.html">ZMP constraints</a> during quadrupedal locomotion. It is at the
origin of the <em>robust</em> closed-loop MPC studied by <a class="reference external" href="https://hal.inria.fr/hal-01649580/file/Villa-Wieber_2017_RCLMPC.pdf">(Villa and Wieber, 2017)</a> in the
case of bipedal locomotion. Closed loop MPC is also the approach followed by
<a class="reference external" href="http://doai.io/10.1109/IROS.2018.8594448">(Di Carlo et al., 2018)</a> to
control the base position and orientation of a walking quadruped through
contact forces.</p>
</div>
<div class="section" id="pros-and-cons">
<h2>Pros and cons</h2>
<p>A benefit of open loop MPC, compared to its closed loop counterpart, is that it
makes it easier to enforce guarantees such as <a class="reference external" href="https://hal.inria.fr/hal-01618881/file/ICHR17_0017_FI.pdf">recursive feasibility</a> by choosing
proper <a class="reference external" href="capture-point.rst#boundedness-condition">terminal constraints</a>.
Recursive feasibility is the guarantee that if the current MPC problem is
feasible, then the next MPC problem (after integration) will be feasible as
well. This is an important property in practice to make sure that the robot
does not run "out of plan" while walking, which is dangerous if its current
state is not a static equilibrium.</p>
<p>Open loop MPC only generates a reference state, and is therefore commonly
cascaded with a <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html#walking-stabilization">walking stabilizer</a> to implement
feedback from the observed state. The main drawback of this approach is that a
stabilizer is often by design more short-sighted than a model predictive
controller, so that the combined system may not be general enough to discover
more complex recovery strategies (re-stepping, crouching, side stepping, ...)
that closed loop MPC can discover.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Recursive feasibility is easier to enforce in open loop MPC, but it can also be
achieved in closed loop. What makes it challenging at first is that the direct
feedback from observations to the initial state of the MPC problem can move it
too far away from the previous solution. One way to palliate this is indirect
feedback, where the initial state of the subsequent MPC problem is constrained
to lie around (not exactly at) the observed one. This approach may let the plan
drift, with a rate parameterized by the extent of the initial state constraint,
but it can also <a class="reference external" href="https://doi.org/10.1016/j.automatica.2004.08.019">preserve recursive feasibility</a>.</p>
<p>In this short overview, we mentioned measurement errors in the initial state of
closed-loop MPC, but we didn't dig into the question of <em>measurement
uncertainty</em>. This point, as well as other sources of uncertainty, can be taken
into account in the more general framework of <a class="reference external" href="https://web.stanford.edu/class/ee364b/lectures/stoch_mpc_slides.pdf">stochastic model predictive
control</a>.</p>
<p>On a related note:</p>
<ul class="simple">
<li>Beyond recursive feasibility lies <a class="reference external" href="https://web.archive.org/web/20170814172055id_/http://spiral.imperial.ac.uk/bitstream/10044/1/4346/1/cued_control_155.pdf">robust set invariance theory</a>.</li>
<li>Open and closed loop MPC have been compared in <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02993058/document">reaching and pick-and-place tasks</a> with a manipulator.</li>
</ul>
<p><strong>Thanks</strong> to <a class="reference external" href="https://scholar.google.com/citations?user=dql3PBQAAAAJ">Nahuel Villa</a>
for his feedback on previous versions of this post!</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Knee torque of a lumped mass model2022-01-26T00:00:00+01:002022-01-26T00:00:00+01:00Stéphane Carontag:scaron.info,2022-01-26:/robot-locomotion/knee-torque-lumped-mass-model.html<img alt="Knee joint in a lumped mass model" class="right max-width-80pct" src="https://scaron.info/figures/knee-torque-lumped-mass-model.png" />
<p>Consider the planar legged model depicted to the right. The robot's mass is
concentrated at its center of mass <span class="math">\(G\)</span>, meaning we neglect the mass of
the two links of the leg. The center of mass is located above the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">center of
pressure</a> <span class="math">\(C\)</span> and the robot is
in static …</p><img alt="Knee joint in a lumped mass model" class="right max-width-80pct" src="https://scaron.info/figures/knee-torque-lumped-mass-model.png" />
<p>Consider the planar legged model depicted to the right. The robot's mass is
concentrated at its center of mass <span class="math">\(G\)</span>, meaning we neglect the mass of
the two links of the leg. The center of mass is located above the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">center of
pressure</a> <span class="math">\(C\)</span> and the robot is
in static equilibrium. The leg has a single <a class="reference external" href="https://scaron.info/robot-locomotion/revolute-joints.html">revolute joint</a> located at the knee <span class="math">\(K\)</span> with joint
angle <span class="math">\(q\)</span>. Both of its links have the same length <span class="math">\(CK = KG = \ell\)</span>.</p>
<p><strong>Question:</strong> what is the joint torque <span class="math">\(\tau\)</span> exerted by the knee to keep
the leg in static equilibrium?</p>
<div class="section" id="note">
<h2>Note</h2>
<p>This is a question page: there is no need to post the answer in the discussion
below. However, if you have an original derivation of the solution, you are
welcome to post it.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>This simplified model was considered to dimension the knee actuators during the
<a class="reference external" href="https://staff.aist.go.jp/k.kaneko/publications/2011_publications/IROS2011-0164.pdf">design of the HRP-4 humanoid robot</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Revolute joints2022-01-26T00:00:00+01:002022-05-02T00:00:00+02:00Stéphane Carontag:scaron.info,2022-01-26:/robot-locomotion/revolute-joints.html<p>Revolute joints are the most common ones in legged robots. Their <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">equations of
motion</a> involve both joint
coordinates <span class="math">\(\bfq\)</span> and joint torques <span class="math">\(\bftau\)</span>:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bfJ_c(\bfq)^\top \bff
\end{equation*}
</div>
<p>But given a joint <span class="math">\(i\)</span> connecting …</p><p>Revolute joints are the most common ones in legged robots. Their <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">equations of
motion</a> involve both joint
coordinates <span class="math">\(\bfq\)</span> and joint torques <span class="math">\(\bftau\)</span>:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bfJ_c(\bfq)^\top \bff
\end{equation*}
</div>
<p>But given a joint <span class="math">\(i\)</span> connecting a link <span class="math">\(i\)</span> to its parent
<span class="math">\(\lambda(i)\)</span>, what is the definition of the joint angle <span class="math">\(q_i\)</span> in
<span class="math">\(\bfq\)</span>? Or of the joint torque <span class="math">\(\tau_i\)</span> in <span class="math">\(\bftau\)</span>?</p>
<div class="section" id="kinematics-of-a-revolute-joint">
<h2>Kinematics of a revolute joint</h2>
<p>Our robot model typically consists in a kinematic tree rooted at a special
link, the <em>floating base</em> or <em>base link</em>. Each <a class="reference external" href="https://wiki.ros.org/urdf/XML/link">link</a> is a rigid body connected to one or more
others by <a class="reference external" href="https://wiki.ros.org/urdf/XML/joint">joints</a> that prevent motion
in some axes while allowing it on some others. Here is the example of a
revolute joint at the elbow of a humanoid robot:</p>
<img alt="Two rigid links connected by a revolute joint" class="center max-height-325px" src="https://scaron.info/figures/rigid-body-system.png" />
<p>In general there are six degrees of freedom between two rigid bodies,
corresponding to the three translation and three rotation coordinates of their
relative <a class="reference external" href="https://scaron.info/robot-locomotion/kinematics-jargon.html">pose</a>. A revolute joint allows
rotation around one axis only, the <em>joint axis</em>, thus leaving one degree of
freedom while applying five degrees of constraint between the two bodies. Such
a joint is realized in practice by a rotary actuator, which looks like this:</p>
<img alt="Stator and rotor of the rotary actuator in a revolute joint" class="center max-height-200px" src="https://scaron.info/figures/stator-rotor.png" />
<p>Regardless of its design (brushless DC electric, series-elastic, quasi-direct
drive, …), the rotary actuator always has two main bodies when we look at it
from the outside: a stator and a rotor. The rotor rotates around (and is
typically symmetric around) the joint axis. The stator is attached to one link
and the rotor to the other, so that the actuator applies <em>joint torque</em> between
the two links. In the figure above we have attached the stator to the parent
link and the rotor to the child link, but it could very well have been the
other way round.</p>
<div class="section" id="relative-pose-of-the-child-link">
<h3>Relative pose of the child link</h3>
<p>The Plücker transform <span class="math">\(\bfX_{\lambda(i), i}\)</span> from the child frame
<span class="math">\(i\)</span> to the parent frame <span class="math">\(\lambda(i)\)</span> can be decomposed as follows:</p>
<div class="math">
\begin{equation*}
\bfX_{\lambda(i), i} = \bfX_{\lambda(i), stator} \cdot \bfX_{stator, rotor}(q_i) \cdot \bfX_{rotor, i}
\end{equation*}
</div>
<p>In the URDF convention, the child frame coincides with the <em>joint frame</em>,
<em>i.e.</em> <span class="math">\(\bfX_{rotor, i} = \bfI_{6 \times 6}\)</span>. This is purely a
convention, for calculation purposes what matters is rather that
<span class="math">\(\bfX_{\lambda(i), stator}\)</span> and <span class="math">\(\bfX_{rotor, i}\)</span> do not depend on
the joint angle <span class="math">\(q_i\)</span>. The only varying part in the equation is then the
stator-to-rotor transform. Say we select the <span class="math">\(z\)</span>-axis of the joint
(rotor) frame as our joint axis, so that our frames look like this:</p>
<img alt="Stator and rotor frames after rotation of the actuator" class="center max-height-200px" src="https://scaron.info/figures/stator-rotor-frames.png" />
<p>The transform from the rotor frame to the stator frame is then:</p>
<div class="math">
\begin{equation*}
\bfX_{stator, rotor}(q_i) =
\left[\begin{array} {ccc|ccc}
\cos(q_i) & -\sin(q_i) & 0 & 0 & 0 & 0 \\
\sin(q_i) & \cos(q_i) & 0 & 0 & 0 & 0 \\
0 & 0 & 1 & 0 & 0 & 0 \\
\hline
0 & 0 & 0 & \cos(q_i) & -\sin(q_i) & 0 \\
0 & 0 & 0 & \sin(q_i) & \cos(q_i) & 0 \\
0 & 0 & 0 & 0 & 0 & 1 \\
\end{array}
\right]
\end{equation*}
</div>
<p>That is, a pure rotation of angle <span class="math">\(q_i\)</span> around the <span class="math">\(z\)</span>-axis of the
joint frame. In general, a revolute joint may rotate around any axis
<span class="math">\(\bfe_i \neq \bfe_z\)</span>, but that's alright, we know how to write rotation
matrices <a class="reference external" href="https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle">from their axis-angle representation</a>
;-) In that case, the transform becomes:</p>
<div class="math">
\begin{equation*}
\bfX_{stator, rotor}(q_i) =
\left[\begin{array} {c|c}
\bfR_{\bfe_i}(q_i) & \bfzero_{3 \times 3} \\
\hline
\bfzero_{3 \times 3} & \bfR_{\bfe_i}(q_i)
\end{array}
\right]
\end{equation*}
</div>
<p>Note that we didn't specify the frame of the Euclidean vector <span class="math">\(\bfe_i\)</span>
here. It is not a mistake yet since the rotation leaves this vector unchanged
(<span class="math">\({}^{stator} \bfe_i = {}^{rotor} \bfe_i = \bfe_i\)</span>), but we will make the
frame explicit when we turn it into a motion vector.</p>
</div>
<div class="section" id="velocity-and-velocity-jacobian-of-the-link">
<h3>Velocity and velocity Jacobian of the link</h3>
<p>The time derivative of the axis-angle rotation is <span class="math">\(\dot{\bfR}_{\bfe}(q) =
\dot{q} \bfe \times \bfR_{\bfe}(q)\)</span>. Therefore, the time derivative of the
transform from the rotor to the stator frame is:</p>
\begin{align}
\dot{\bfR}_{stator, rotor} & = \dot{q_i} \bfe_i \times \bfR_{stator, rotor} \\
\dot{\bfX}_{stator, rotor} & = \dot{q_i} \begin{bmatrix} \bfe_i \\ \bfzero_{3} \end{bmatrix} \times \bfX_{stator, rotor} = \dot{q_i} {}^{stator} \bfs_i \times \bfX_{stator, rotor}
\end{align}<p>where we defined the motion vector <span class="math">\({}^{stator} \bfs_i := [\bfe_i\
\bfzero_{3}]\)</span>. Let us now <a class="reference external" href="https://scaron.info/robot-locomotion/spatial-vector-algebra-cheat-sheet.html#time-derivatives">derivate</a> the
transform <span class="math">\(\bfX_{\lambda(i), i}\)</span> from the link frame to its parent frame:</p>
\begin{align}
\dot{\bfX}_{\lambda(i), i} = {}^{\lambda(i)} (\bfv_i - \bfv_{\lambda(i)}) \times \bfX_{\lambda(i), i} & = \bfX_{\lambda(i), stator} \cdot \dot{\bfX}_{stator, rotor} \cdot \bfX_{rotor, i} \\
& = \dot{q}_i \bfX_{\lambda(i), stator} ({}^{stator} \bfs_i \times) \bfX_{stator, rotor} \bfX_{rotor, i} \\
& = \dot{q}_i {}^{\lambda(i)} \bfs_i \times \bfX_{\lambda(i), stator} \bfX_{stator, rotor} \bfX_{rotor, i} \\
& = \dot{q}_i {}^{\lambda(i)} \bfs_i \times \bfX_{\lambda(i), i}
\end{align}<p>This implies that <span class="math">\({}^{\lambda(i)} (\bfv_i - \bfv_{\lambda(i)}) =
\dot{q}_i {}^{\lambda(i)} \bfs_i\)</span>, or equivalently in the world frame:</p>
<div class="math">
\begin{equation*}
\bfv_i = \bfv_{\lambda(i)} + \bfs_i(\bfq) \dot{q}_i
\end{equation*}
</div>
<p>where <span class="math">\(\bfs_i(\bfq) = \bfX_{world, \lambda(i)}(\bfq)
{}^{\lambda(i)} \bfs_i\)</span> maps the joint velocity <span class="math">\(\dot{q}_i \in
\mathbb{R}\)</span> to the link's spatial velocity <span class="math">\(\bfv_i \in \textsf{M}^6\)</span>. The
calculation of this motion vector <span class="math">\(\bfs_i\)</span>, which we have seen here in the
particular case of a revolute joint, can actually be carried out for any type
of joint: given a joint <span class="math">\(j\)</span> with <span class="math">\(n_j\)</span> degrees of freedom and joint
coordinates <span class="math">\(\bfq_j \in \mathbb{R}^{n_j}\)</span>, there exists a <span class="math">\(6 \times
n_j\)</span> matrix <span class="math">\(\bfS(\bfq)\)</span> of motion vectors such that <span class="math">\(\bfv_i =
\bfv_{\lambda(i)} + \bfS(\bfq) \dot{\bfq}_j\)</span>. Going all the way up to the root
of the kinematic tree, we see that these successive motion vectors make up the
Jacobian of the link:</p>
<div class="math">
\begin{equation*}
\bfv_i
= \bfs_i(\bfq) \dot{q}_i + \bfs_{\lambda(i)}(\bfq) \dot{q}_{\lambda(i)} + \cdots + \bfS_{base}(\bfq) \dot{\bfq}_{base}
= \bfJ_i(\bfq) \dot{\bfq}
\end{equation*}
</div>
<p>where <span class="math">\(\bfJ_i(\bfq)\)</span> is the <span class="math">\(6 \times n\)</span> spatial Jacobian matrix of
our link <span class="math">\(i\)</span>, and this formula hides under the rug an ordering and
flattening of the joint coordinate vectors <span class="math">\([\bfq_{base}, \ldots, q_i,
\ldots]\)</span> into the generalized coordinates <span class="math">\(\bfq\)</span> (libraries like
Pinocchio let us access both). Rigid body libraries factor computations for
better performance, but in essence the Jacobian matrix routinely used in
<a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse kinematics</a> is the matrix thus
derived.</p>
</div>
</div>
<div class="section" id="dynamics-of-a-revolute-joint">
<h2>Dynamics of a revolute joint</h2>
<p>While kinematics derives from the composition of transforms <span class="math">\(\bfX_{CA} =
\bfX_{CB} \bfX_{BA}\)</span>, dynamics derives from the Newton-Euler equations of rigid
bodies:</p>
<div class="math">
\begin{equation*}
\bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i = \bfw^{net}_i
\end{equation*}
</div>
<p>where <span class="math">\(\bfa_i\)</span> is the spatial acceleration of the link, <span class="math">\(\bfI_i\)</span>
its spatial inertia matrix, and <span class="math">\(\bfw^{net}_i\)</span> the net spatial force
applied to it. The left-hand side represents the inertia and motion of the
link, while the right-hand side represents forces. Let us focus on the latter
to see how the joint torque <span class="math">\(\tau_i\)</span> appears there.</p>
<div class="section" id="forces-acting-on-the-link">
<h3>Forces acting on the link</h3>
<p>By convention, we write <span class="math">\(\bfw_{i}\)</span> the spatial force vector representing
forces applied by the <em>parent</em> link <span class="math">\(\lambda(i)\)</span> onto link <span class="math">\(i\)</span>. The
net force applied on the link is then:</p>
<div class="math">
\begin{equation*}
\bfw^{net}_i = \bfw_i + \bfw_i^\mathit{ext} - \sum_{j \in \mathrm{sons}(i)} \bfw_j
\end{equation*}
</div>
<p>where <span class="math">\(\bfw^{ext}_i\)</span> is the sum of external forces applied to the link,
for instance gravity applied at its center of mass, or external forces exerted
by a collision.</p>
<img alt="Two rigid links connected by a revolute joint" class="center max-width-80pct" src="https://scaron.info/figures/force-moment-link.png" />
<p>The force <span class="math">\(\bfw_i\)</span> consists of two components:</p>
<ul class="simple">
<li><strong>Joint forces:</strong> <span class="math">\(\bfw_i^{joint} = (\bfs_i \cdot \bfw_i) \bfs_i\)</span> act
along the joint degrees of freedom. (Note that <span class="math">\(\| \bfs_i \| = 1\)</span>.)
Since the dot product of two vectors does not depend on the frame they are
expressed in, we can evaluate <span class="math">\((\bfs_i \cdot \bfw_i)\)</span> in the joint
frame:</li>
</ul>
\begin{align}
\bfs_i \cdot \bfw_i
& = (\bfX_{world, i} {}^i \bfs_i) \cdot (\bfX_{world, i}^* {}^i \bfw_i) \\
& = {}^i \bfs_i^\top \bfX_{world, i}^\top \bfX_{world, i}^* {}^i \bfw_i \\
& = {}^i \bfs_i \cdot {}^i \bfw_i \\
& = \bfe_i \cdot {}^i \bftau_i \\
& = \tau_i
\end{align}<ul class="simple">
<li><strong>Internal forces:</strong> <span class="math">\(\bfw_i^{int} = {}^i \bfs_i \times {}^i \bfw_i \times {}^i \bfs_i
= \bfw_i - ({}^i \bfs_i \cdot {}^i \bfw_i) {}^i \bfs_i\)</span>, <em>a.k.a.</em> "everything
else". These are the linear forces and torques transmitted through the
mounting of the joint (screws, contacts between parts, any device that
implements a degree of constraint). They don't affect the robot's motion, but
they may be relevant to other robotic activities such as "making sure things
don't break".</li>
</ul>
<p>This shows how, if our rotary actuator is able to control the torque
<span class="math">\(\tau_i\)</span>, this torque affects the equation of motion of link <span class="math">\(i\)</span>
via <span class="math">\(\bfw_i^{joint} = \tau_i \bfs_i\)</span>. For other types of joints a similar
relationship holds, although it is generally written the other way round as
<span class="math">\(\bftau_i = \bfS_i^\top \bfw^{joint}_i = \bfS_i^\top \bfw_i\)</span> (with
internal forces in the nullspace of <span class="math">\(\bfS_i^\top\)</span>).</p>
<p>Note that, from the usual convention that <span class="math">\(\bfw_i\)</span> is the torque applied
by the parent onto the child link, the torque <span class="math">\(\tau_i\)</span> is similarly
applied <em>to</em> our link <span class="math">\(i\)</span> by the joint. By Newton's law of
action-reaction, an opposite torque <span class="math">\(-\tau_i\)</span> is applied to the parent
link <span class="math">\(\lambda(i)\)</span>.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>You can take a bite at the <a class="reference external" href="https://scaron.info/robot-locomotion/knee-torque-lumped-mass-model.html">knee torque of a lumped mass</a> problem to check whether you
are at ease with the kinematics and dynamics of revolute joints. The formulas
we have seen here are presented for other types of joints in Roy Featherstone's
<a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid Body Dynamics Algorithms</a>.
Check out in particular sections 3.5 (Joint Constraints) and 4.4 (Joint
Models).</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Spatial vector algebra cheat sheet2022-01-25T00:00:00+01:002022-01-25T00:00:00+01:00Stéphane Carontag:scaron.info,2022-01-25:/robot-locomotion/spatial-vector-algebra-cheat-sheet.html<p>Spatial vector algebra is <a class="reference external" href="https://scaron.info/robot-locomotion/screw-theory.html">screw algebra</a> with
two conventions that are great for computations: we use spatial vectors rather
than body vectors whenever possible, and Plücker transforms rather than affine
transforms. Like with any other algebra, the more identities we swing, the more
proficient we get at it. This cheat …</p><p>Spatial vector algebra is <a class="reference external" href="https://scaron.info/robot-locomotion/screw-theory.html">screw algebra</a> with
two conventions that are great for computations: we use spatial vectors rather
than body vectors whenever possible, and Plücker transforms rather than affine
transforms. Like with any other algebra, the more identities we swing, the more
proficient we get at it. This cheat sheet lists the ones I have found useful so
far. It references both spatial and body vectors. Because when a spatial vector
formula resists intuition (not the rareliest occurrence), it can help to
explicit all the frames involved.</p>
<div class="section" id="notations">
<h2>Notations</h2>
<p>We adopt the subscript right-to-left convention for transforms, and superscript
notation to indicate the frame of a motion or force vector:</p>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="70%" />
<col width="30%" />
</colgroup>
<tbody valign="top">
<tr><td>Quantity</td>
<td>Notation</td>
</tr>
<tr><td>Affine transform from frame <span class="math">\(A\)</span> to frame <span class="math">\(B\)</span></td>
<td><span class="math">\(\bfT_{BA}\)</span></td>
</tr>
<tr><td>Body angular velocity of frame <span class="math">\(A\)</span> in frame <span class="math">\(B\)</span></td>
<td><span class="math">\({}^A \bfomega_{BA}\)</span></td>
</tr>
<tr><td>Plücker transform from frame <span class="math">\(A\)</span> to frame <span class="math">\(B\)</span></td>
<td><span class="math">\(\bfX_{BA}\)</span></td>
</tr>
<tr><td>Position of frame <span class="math">\(B\)</span> in frame <span class="math">\(A\)</span></td>
<td><span class="math">\({}^A \bfp_B\)</span></td>
</tr>
<tr><td>Rotation matrix from frame <span class="math">\(A\)</span> to frame <span class="math">\(B\)</span></td>
<td><span class="math">\(\bfR_{BA}\)</span></td>
</tr>
<tr><td>Spatial angular velocity of frame <span class="math">\(A\)</span> in frame <span class="math">\(B\)</span></td>
<td><span class="math">\({}^B \bfomega_{BA}\)</span></td>
</tr>
<tr><td>World frame (inertial)</td>
<td><span class="math">\(W\)</span></td>
</tr>
</tbody>
</table>
<p>With these notations frame transforms can be read left to right, for example:</p>
\begin{align}
\bfX_{CA} & = \bfX_{CB} \bfX_{BA} &
{}^{B} \bfomega & = \bfR_{BA} {}^{A} \bfomega &
{}^B \bfp_C & = \bfR_{BA} {}^A \bfp_C + {}^B \bfp_A
\end{align}<p>Note that we part from Roy Featherstone's notation <span class="math">\({}^B \bfX_A\)</span> to be
able to keep track of the original transforms in time derivatives. For example,
the angular velocity <span class="math">\(\bfomega_{BA}\)</span> that derivates from the rotation
<span class="math">\(\bfR_{BA}\)</span> satisfies:</p>
<div class="math">
\begin{equation*}
\dot{\bfR}_{BA} = ({}^{B} \bfomega_{BA} \times) \bfR_{BA} = \bfR_{BA} ({}^{A} \bfomega_{BA} \times)
\end{equation*}
</div>
<p>The operator <span class="math">\(\bfv \mapsto \bfv \times\)</span> turns a 3D vector <span class="math">\(\bfv\)</span>
into its <span class="math">\(3 \times 3\)</span> <a class="reference external" href="https://en.wikipedia.org/wiki/Skew-symmetric_matrix#Cross_product">cross-product skew-symmetric matrix</a>. See
below for some identities related to this operator.</p>
</div>
<div class="section" id="cross-and-dot-products">
<h2>Cross and dot products</h2>
<div class="section" id="euclidean-cross-products">
<h3>Euclidean cross products</h3>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="40%" />
<col width="60%" />
</colgroup>
<tbody valign="top">
<tr><td>Name</td>
<td>Formula</td>
</tr>
<tr><td><a class="reference external" href="https://en.wikipedia.org/wiki/Triple_product#Vector_triple_product">Vector triple product</a></td>
<td><span class="math">\(\bfa \times (\bfb \times \bfc) = (\bfa \cdot \bfc) \bfb - (\bfa \cdot \bfb) \bfc\)</span></td>
</tr>
<tr><td>Rotation of cross product</td>
<td><span class="math">\(\bfR (\bfa \times \bfb) = (\bfR \bfa) \times (\bfR \bfb)\)</span></td>
</tr>
<tr><td>Cross product by rotated vector</td>
<td><span class="math">\((\bfR \bfv) \times = \bfR (\bfv \times) \bfR^\top\)</span></td>
</tr>
<tr><td>Rotation of cross product matrix</td>
<td><span class="math">\(\bfR (\bfv \times) = (\bfR \bfv) \times \bfR\)</span></td>
</tr>
<tr><td>Rotation of cross product matrix</td>
<td><span class="math">\(\bfR_{BA} ({}^A \bfv \times) = {}^B \bfv \times \bfR_{BA}\)</span></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="spatial-cross-products">
<h3>Spatial cross products</h3>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="40%" />
<col width="60%" />
</colgroup>
<tbody valign="top">
<tr><td>Name</td>
<td>Formula</td>
</tr>
<tr><td>Cross product by transformed vector</td>
<td><span class="math">\((\bfX \bfv) \times = \bfX (\bfv \times) \bfX^{-1}\)</span></td>
</tr>
<tr><td>Transform of cross product matrix</td>
<td><span class="math">\(\bfX (\bfv \times) = (\bfX \bfv) \times \bfX\)</span></td>
</tr>
<tr><td>Transform of cross product matrix</td>
<td><span class="math">\(\bfX_{BA} ({}^A \bfv \times) = {}^B \bfv \times \bfX_{BA}\)</span></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="dot-products">
<h3>Dot products</h3>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="40%" />
<col width="60%" />
</colgroup>
<tbody valign="top">
<tr><td>Property</td>
<td>Formula</td>
</tr>
<tr><td>Invariance by rotation</td>
<td><span class="math">\((\bfR \bfa) \cdot (\bfR \bfb) = \bfa \cdot \bfb\)</span></td>
</tr>
<tr><td>Invariance by dual transforms</td>
<td><span class="math">\((\bfX \bfm) \cdot (\bfX^* \bff) = \bfm \cdot \bff\)</span></td>
</tr>
</tbody>
</table>
</div>
</div>
<div class="section" id="kinematics">
<h2>Kinematics</h2>
<div class="section" id="transform-matrices">
<h3>Transform matrices</h3>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="20%" />
<col width="40%" />
<col width="40%" />
</colgroup>
<tbody valign="top">
<tr><td>Coordinates</td>
<td>Transform</td>
<td>Inverse</td>
</tr>
<tr><td>Homogeneous</td>
<td><span class="math">\(\bfT_{BA} = \begin{bmatrix} \bfR_{BA} & {}^B \bfp_A \\ \bfzero_{1 \times 3} & 1 \end{bmatrix}\)</span></td>
<td><span class="math">\(\bfT^{-1}_{BA} = \begin{bmatrix} \bfR_{BA}^T & -\bfR_{BA}^T {}^B \bfp_A \\ \bfzero_{1 \times 3} & 1 \end{bmatrix}\)</span></td>
</tr>
<tr><td>Motion vectors</td>
<td><span class="math">\(\bfX_{BA} = \begin{bmatrix} \bfR_{BA} & ({}^B \bfp_A \times) \bfR_{BA} \\ \bfzero_{3 \times 3} & \bfR_{BA} \end{bmatrix}\)</span></td>
<td><span class="math">\(\bfX_{BA}^{-1} = \begin{bmatrix} \bfR_{BA}^T & -\bfR_{BA}^T ({}^B \bfp_A \times) \\ \bfzero_{3 \times 3} & \bfR_{BA}^T \end{bmatrix}\)</span></td>
</tr>
<tr><td>Force vectors</td>
<td><span class="math">\(\bfX_{BA}^* = \begin{bmatrix} \bfR_{BA} & \bfzero_{3 \times 3} \\ ({}^B \bfp_A \times) \bfR_{BA} & \bfR_{BA} \end{bmatrix}\)</span></td>
<td><span class="math">\(\bfX_{BA}^{-*} = \begin{bmatrix} \bfR_{BA}^T & \bfzero_{3 \times 3} \\ -\bfR_{BA}^T ({}^B \bfp_A \times) & \bfR_{BA}^T \end{bmatrix}\)</span></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="inversions">
<h3>Inversions</h3>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="40%" />
<col width="60%" />
</colgroup>
<tbody valign="top">
<tr><td>Inversion</td>
<td>Formula</td>
</tr>
<tr><td>Rotation matrix</td>
<td><span class="math">\(\bfR_{AB} = \bfR_{BA}^{-1} = \bfR_{BA}^\top\)</span></td>
</tr>
<tr><td>Angular velocity</td>
<td><span class="math">\({}^A \bfomega_{AB} = -{}^A \bfomega_{BA}\)</span></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="time-derivatives">
<h3>Time derivatives</h3>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="20%" />
<col width="40%" />
<col width="40%" />
</colgroup>
<tbody valign="top">
<tr><td>Quantity</td>
<td>Spatial derivative</td>
<td>Body derivative</td>
</tr>
<tr><td>Rotation matrix</td>
<td><span class="math">\({\dot{\bfR}}_{BA} = {}^{B} \bfomega_{BA} \times \bfR_{BA}\)</span></td>
<td><span class="math">\({\dot{\bfR}}_{BA} = \bfR_{BA} ({}^{A} \bfomega_{BA} \times)\)</span></td>
</tr>
</tbody>
</table>
<table border="1" class="colwidths-given cheatsheet docutils">
<colgroup>
<col width="20%" />
<col width="40%" />
<col width="40%" />
</colgroup>
<tbody valign="top">
<tr><td>Quantity</td>
<td>Spatial vector algebra</td>
<td>Screw algebra</td>
</tr>
<tr><td>Rotation matrix</td>
<td><span class="math">\({\dot{\bfR}}_{BA} = {}^{B} (\bfomega_{A} - \bfomega_{B}) \times \bfR_{BA}\)</span></td>
<td><span class="math">\({\dot{\bfR}}_{BA} = {}^{B} (\bfomega_{WA} - \bfomega_{WB}) \times \bfR_{BA}\)</span></td>
</tr>
<tr><td>Plücker transform</td>
<td><span class="math">\({\dot{\bfX}}_{BA} = {}^{B} (\bfv_{A} - \bfv_{B}) \times \bfX_{BA}\)</span></td>
<td><span class="math">\({\dot{\bfX}}_{BA} = {}^{B} (\bfv_{WA} - \bfv_{WB}) \times \bfX_{BA}\)</span></td>
</tr>
<tr><td>Plücker to world</td>
<td><span class="math">\({\dot{\bfX}}_{WA} = \bfv_{A} \times \bfX_{WA}\)</span></td>
<td><span class="math">\({\dot{\bfX}}_{WA} = {}^{W} \bfv_{WA} \times \bfX_{WA}\)</span></td>
</tr>
</tbody>
</table>
</div>
</div>
<div class="section" id="proof-notes">
<h2>Proof notes</h2>
<p>These notes go a bit beyond the cheat sheet. They check that the formulas are
correct and consistent with each other.</p>
<div class="section" id="rotation-time-derivatives">
<h3>Rotation time derivatives</h3>
<p>We can check that <span class="math">\({}^B (\bfomega_{WA} - \bfomega_{WB}) = {}^B
\bfomega_{BA}\)</span> so that the formulas for the rotation time derivatives are
consistent:</p>
\begin{align}
\dot{\bfR}_{BA} = {}^B \bfomega_{BA} \times \bfR_{BA}
& = \frac{\rm d}{{\rm d} t} (\bfR_{BW} \bfR_{WA}) \\
& = \dot{\bfR}_{BW} \bfR_{WA} + \bfR_{BW} \dot{\bfR}_{WA} \\
& = {}^B \bfomega_{BW} \times \bfR_{BW} \bfR_{WA} + \bfR_{BW} \bfR_{WA} ({}^A \bfomega_{WA} \times) \\
& = {}^B \bfomega_{BW} \times \bfR_{BA} + \bfR_{BA} ({}^A \bfomega_{WA} \times) \\
& = (-{}^B \bfomega_{WB}) \times \bfR_{BA} + {}^B \bfomega_{WA} \times \bfR_{BA} \\
& = ({}^B \bfomega_{WA} - {}^B \bfomega_{WB}) \times \bfR_{BA}
\end{align}</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The reference book on spatial vector algebra is Roy Featherstone's <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid Body Dynamics Algorithms</a>. Its tables are cheat sheets of their own. The book itself is better as an implementation reference than for learning things, as it often assumes the reader is already familiar with screw theory. For first-time learners, <a class="reference external" href="http://hades.mech.northwestern.edu/images/2/25/MR-v2.pdf">Modern Robotics</a> might be a better place to start, or <a class="reference external" href="https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.671.7040&rep=rep1&type=pdf">A Mathematical Introduction to Robotic Manipulation</a> for those who like their math fresh.</p>
<p>There are plenty of cheat sheets on the web, which says something about both the importance and the trickiness of Lie algebras ;-) Pinocchio has a useful <a class="reference external" href="https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_c-maths_se3.html">SE(3) algebra cheat sheet</a>. So does the <a class="reference external" href="https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf">Kindr library</a>. For rotations, which are also a Lie algebra denoted by <span class="math">\(SO(3)\)</span> rather than <span class="math">\(SE(3)\)</span>, Diebel's <a class="reference external" href="https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134">Representing attitude: Euler angles, unit quaternions, and rotation vectors</a> is also a great cheat sheet. For spatial vector algebra specifically, Jan Carius has also started some nice <a class="reference external" href="https://jan.carius.io/research/2018/03/29/spatial-velocities.html">notes on spatial velocities</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Au revoir Jean-Paul2021-12-24T00:00:00+01:002021-12-24T00:00:00+01:00Stéphane Carontag:scaron.info,2021-12-24:/blog/au-revoir-jean-paul.html<p>Un message <a class="reference external" href="https://www.laas.fr/jp-laumond/disparition">parmi tant d'autres</a> :</p>
<blockquote>
<div class="line-block">
<div class="line">Sans toi, serions-nous là ? Aurions-nous ressenti</div>
<div class="line">L'appel technologique ? Les algèbres de Lie ?</div>
<div class="line">Le fait intransigeant que <a class="reference external" href="https://www.college-de-france.fr/site/jean-paul-laumond/inaugural-lecture-2012-01-19-18h00.htm">Vulcain</a> dans son marbre</div>
<div class="line">Approuve la nouveauté et <a class="reference external" href="https://lejournal.cnrs.fr/articles/larbre-qui-deambule">donne des roues aux arbres</a>.</div>
<div class="line"><br /></div>
<div class="line">Travailler sans palabres, chercher la vérité,</div>
<div class="line">Enseigner comme on conte et d'un air naturel</div>
<div class="line">Faire sentir …</div></div></blockquote><p>Un message <a class="reference external" href="https://www.laas.fr/jp-laumond/disparition">parmi tant d'autres</a> :</p>
<blockquote>
<div class="line-block">
<div class="line">Sans toi, serions-nous là ? Aurions-nous ressenti</div>
<div class="line">L'appel technologique ? Les algèbres de Lie ?</div>
<div class="line">Le fait intransigeant que <a class="reference external" href="https://www.college-de-france.fr/site/jean-paul-laumond/inaugural-lecture-2012-01-19-18h00.htm">Vulcain</a> dans son marbre</div>
<div class="line">Approuve la nouveauté et <a class="reference external" href="https://lejournal.cnrs.fr/articles/larbre-qui-deambule">donne des roues aux arbres</a>.</div>
<div class="line"><br /></div>
<div class="line">Travailler sans palabres, chercher la vérité,</div>
<div class="line">Enseigner comme on conte et d'un air naturel</div>
<div class="line">Faire sentir, et aimer, le monde intellectuel</div>
<div class="line">Que tu nous dépeignais du tableau au café.</div>
<div class="line"><br /></div>
<div class="line">Aujourd'hui matelots sur le pont t'honorons,</div>
<div class="line">Demain dedans nos cales, de retour au charbon,</div>
<div class="line">Nous reverrons encore ton image et ta voix.</div>
<div class="line"><br /></div>
<div class="line">À tes histoires lyriques, à ton côté rieur,</div>
<div class="line">À ton regard intense de charmant professeur,</div>
<div class="line">À toi qui nous montras. À toi qui a été là.</div>
</div>
</blockquote>
<p>Au revoir Jean-Paul. 守</p>
<img alt="Jean-Paul Laumond" src="https://scaron.info/images/jpl.jpg" />
<p><a class="reference external" href="https://fr.wikipedia.org/wiki/Jean-Paul_Laumond">Jean-Paul Laumond</a>, 1953–2021</p>
Recursive Newton-Euler algorithm2021-12-06T00:00:00+01:002022-08-08T00:00:00+02:00Stéphane Carontag:scaron.info,2021-12-06:/robot-locomotion/recursive-newton-euler-algorithm.html<p>Inverse dynamics refers to the computation of forces from motions. Given the
configuration <span class="math">\(\bfq\)</span>, generalized velocity <span class="math">\(\qd\)</span> and generalized
acceleration <span class="math">\(\qdd\)</span>, it amounts to finding the joint torques
<span class="math">\(\bftau\)</span> and contact forces <span class="math">\(\bff^\mathit{ext}\)</span> such that the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd …<p>Inverse dynamics refers to the computation of forces from motions. Given the
configuration <span class="math">\(\bfq\)</span>, generalized velocity <span class="math">\(\qd\)</span> and generalized
acceleration <span class="math">\(\qdd\)</span>, it amounts to finding the joint torques
<span class="math">\(\bftau\)</span> and contact forces <span class="math">\(\bff^\mathit{ext}\)</span> such that the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> are satisfied:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff^\mathit{ext} \\
\bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = \boldsymbol{0}
\end{align}<p>Mathematically, we could formulate this operation as a function:</p>
<div class="math">
\begin{equation*}
(\bftau, \bff^\mathit{ext}) = \mathrm{ID}(\bfq, \qd, \qdd)
\end{equation*}
</div>
<p>This function is well-defined when our linear system is fully determined, for
instance for an arm with six degrees of freedom, but for mobile robots under
multiple contacts it is often under-determined. In that case, we can postpone
the calculation of external forces to <em>e.g.</em> a contact model and compute joint
torques. The recursive Newton-Euler algorithm (RNEA) gives us an efficient way
to do this:</p>
<div class="math">
\begin{equation*}
\bftau = \mathrm{RNEA}(\bfq, \qd, \qdd, \bff^\mathit{ext})
\end{equation*}
</div>
<p>The algorithm works in two passes: a forward pass which is mostly a
second-order forward kinematics, followed by a backward pass that computes
forces and joint torques.</p>
<div class="section" id="forward-kinematic-pass">
<h2>Forward kinematic pass</h2>
<p>The first pass of RNEA computes body velocities <span class="math">\(\bfv_i\)</span> and
accelerations <span class="math">\(\bfa_i\)</span>. Starting from the root <span class="math">\(i = 0\)</span> of the
kinematic tree, the motion <span class="math">\(\bfv_{i}, \bfa_i\)</span> of body <span class="math">\(i\)</span> is
computed from the motion <span class="math">\(\bfv_{\lambda(i)}, \bfa_{\lambda(i)}\)</span> of its
parent body <span class="math">\(\lambda(i)\)</span>, plus the component caused by the motion
<span class="math">\(\qd_i, \qdd_i\)</span> of the joint between them. Let's start with the body
velocity:</p>
<div class="math">
\begin{equation*}
\bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i
\end{equation*}
</div>
<p>In this equation, <span class="math">\({}^i \bfX_{\lambda(i)}\)</span> is the Plücker transform from
<span class="math">\(\lambda(i)\)</span> to <span class="math">\(i\)</span>, and <span class="math">\(\bfS_i\)</span> is the motion subspace
matrix of the joint <span class="math">\(i\)</span>. Note that <span class="math">\(\qd_i \in \mathbb{R}^k\)</span> is the
velocity of the joint, for instance <span class="math">\(k = 6\)</span> for the floating base
(<em>a.k.a.</em> free flyer) joint, <span class="math">\(k = 2\)</span> for a spherical joint, and <span class="math">\(k
= 1\)</span> for a revolute or a prismatic joint. At any rate, <span class="math">\(\qd_i\)</span> is <em>not</em>
the <span class="math">\(i^\mathrm{th}\)</span> component of the generalized velocity vector
<span class="math">\(\qd\)</span> (which would not make sense since <span class="math">\(i\)</span> is the index of a joint
whereas the vector <span class="math">\(\qd\)</span> is indexed by degrees of freedom). Consequently,
the motion subspace matrix has dimension <span class="math">\(6 \times k\)</span>.</p>
<p>In what follows, let us assume a "common" joint (revolute, prismatic, helical,
cylindrical, planar, spherical, free flyer) so that the apparent time
derivative of the motion subspace matrix is zero. Nevermind this statement
unless you are dealing with a different joint ;-) Then, the body acceleration
computed from the parent during the forward pass is:</p>
<div class="math">
\begin{equation*}
\bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i
\end{equation*}
</div>
<p>So far this forward pass is a second-order forward kinematics. The last thing we want to compute along the way are the body inertial forces produced by the motion <span class="math">\(\bfv_i, \bfa_i\)</span> of body <span class="math">\(i\)</span>:</p>
<div class="math">
\begin{equation*}
\bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}
\end{equation*}
</div>
<p>We will update these force vectors during the backward pass. Note that, since they are force vectors, our notation implies that <span class="math">\(\bff_i^\mathit{ext}\)</span> is a body force vector as well. If the external force is expressed in the inertial frame as <span class="math">\({}^0 \bff_i^\mathit{ext}\)</span>, it can be mapped to the body frame via <span class="math">\(\bff_i = {}^i\bfX_0 {}^0 \bff_i^\mathit{ext}\)</span>.</p>
</div>
<div class="section" id="backward-dynamic-pass">
<h2>Backward dynamic pass</h2>
<p>The second pass of RNEA computes body forces. Starting from leaf nodes of the
kinematic tree, the generalized force <span class="math">\(\bff_{i}\)</span> of body <span class="math">\(i\)</span> is
added to the force <span class="math">\(\bff_{\lambda(i)}\)</span> computed so far for its parent
<span class="math">\(\lambda(i)\)</span>:</p>
<div class="math">
\begin{equation*}
\bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i
\end{equation*}
</div>
<p>Once the generalized force <span class="math">\(\bff_i\)</span> on body <span class="math">\(i\)</span> is computed, we get
the corresponding joint torque <span class="math">\(\bftau_i\)</span> by projecting this 6D body
vector along the joint axis:</p>
<div class="math">
\begin{equation*}
\bftau_i = \bfS_i^\top \bff_i
\end{equation*}
</div>
<p>For a <a class="reference external" href="https://scaron.info/robot-locomotion/revolute-joints.html">revolute joint</a>, <span class="math">\(\bfS_i\)</span> is a
<span class="math">\(6 \times 1\)</span> column vector, so that we end with a single number
<span class="math">\(\tau_i = \bfS_i^\top \bff_i\)</span>: the actuation torque that the joint servo
should provide to track <span class="math">\((\bfq, \qd, \qdd, \bff^\mathit{ext})\)</span>. All other
components correspond to the five degrees of constraint of our revolute joint
and will be provided passively by the joint's mechanics.</p>
</div>
<div class="section" id="pseudocode">
<h2>Pseudocode</h2>
<p>Summing up the math, our function <span class="math">\(\mathrm{RNEA}(\bfq, \qd, \qdd,
\bff^\mathit{ext})\)</span> does the following:</p>
<ul class="simple">
<li><span class="math">\(\bfv_0 = \bfzero\)</span></li>
<li><span class="math">\(\bfa_0 = -\bfa_g\)</span>, where <span class="math">\(\bfa_g\)</span> is the gravitational acceleration vector</li>
<li><dl class="first docutils">
<dt><strong>for</strong> link <span class="math">\(i = 1\)</span> to <span class="math">\(n\)</span>:</dt>
<dd><ul class="first last">
<li><span class="math">\({}^i \bfX_{\lambda(i)}, \bfS_i, \bfI_i = \mathrm{compute\_joint}(\mathrm{joint\_type}_i, \bfq_i, \qd_i)\)</span></li>
<li><span class="math">\(\bfv_i = {}^i \bfX_{\lambda(i)} \bfv_{\lambda(i)} + \bfS_i \qd_i\)</span></li>
<li><span class="math">\(\bfa_i = {}^i \bfX_{\lambda(i)} \bfa_{\lambda(i)} + \bfS_i \qdd_i + \bfv_i \times \bfS_i \qd_i\)</span></li>
<li><span class="math">\(\bff_i = \bfI_i \bfa_i + \bfv_i \times^* \bfI_i \bfv_i - \bff_i^\mathit{ext}\)</span></li>
</ul>
</dd>
</dl>
</li>
<li><dl class="first docutils">
<dt><strong>for</strong> link <span class="math">\(i = n\)</span> to <span class="math">\(1\)</span>:</dt>
<dd><ul class="first last">
<li><span class="math">\(\bff_{\lambda(i)} = \bff_{\lambda(i)} + {}^i \bfX_{\lambda(i)}^\top \bff_i\)</span></li>
<li><span class="math">\(\bftau_i = \bfS_i^\top \bff_i\)</span></li>
</ul>
</dd>
</dl>
</li>
</ul>
<p>The initialization of the acceleration <span class="math">\(\bfa_0 = -\bfa_g\)</span> is a trick to
avoid adding <span class="math">\(\bfI_i \bfa_g\)</span> terms to every acceleration on the forward
pass. It will yield the correct <span class="math">\((\bftau, \bff^\mathit{ext})\)</span> results,
however in this version of the algorithm the intermediate <span class="math">\(\bfa_i\)</span> and
<span class="math">\(\bff_i\)</span> won't equal their physical values (rather, they become
computation intermediates). See page 94 of <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid body dynamics algorithms</a> for details on this point.</p>
<p>Let us now explicit a few more things by doing the same in pseudo-Python. Our
function prototype is:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">qd</span><span class="p">,</span> <span class="n">qdd</span><span class="p">,</span> <span class="n">f_ext</span><span class="p">):</span>
<span class="k">pass</span>
</pre>
<p>Note that <tt class="docutils literal">q</tt> is a list of generalized coordinates for each joint, not a flat
array, and that the same holds for the other parameters. In particular, <tt class="docutils literal">f_ext</tt> is a list of body force vectors <span class="math">\(\bff_i^\mathit{ext}\)</span>. With Python type
annotations, our prototype would therefore look like this:</p>
<pre class="code python literal-block">
<span class="kn">from</span> <span class="nn">typing</span> <span class="kn">import</span> <span class="n">List</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span>
<span class="n">q</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="n">qd</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="n">qdd</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="n">f_ext</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span>
<span class="p">)</span> <span class="o">-></span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]:</span>
<span class="k">pass</span>
</pre>
<p>This extra structure allows more general joints, such as spherical ones (not
common) or a free-flyer joint for the floating base of a mobile robot (common).
If all joints were revolute, then all types would coalesce to flat arrays.</p>
<p>Let us denote by <span class="math">\(\bfv_0 = \bfzero\)</span> the spatial velocity of the root link
of our kinematic tree, and <span class="math">\(\bfa_0\)</span> its spatial acceleration. We
initialize them to respectively zero and the <a class="reference external" href="https://en.wikipedia.org/wiki/Standard_gravity">standard acceleration due to
gravity</a>:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">qd</span><span class="p">,</span> <span class="n">qdd</span><span class="p">,</span> <span class="n">f_ext</span><span class="p">):</span>
<span class="n">n</span> <span class="o">=</span> <span class="nb">len</span><span class="p">(</span><span class="n">qd</span><span class="p">)</span> <span class="o">-</span> <span class="mi">1</span> <span class="c1"># number of links == number of joints - 1</span>
<span class="n">v</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">a</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">f</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">tau</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">(</span><span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span>
<span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="o">-</span><span class="mf">9.81</span><span class="p">])</span>
<span class="c1"># to be continued below...</span>
</pre>
<p>We continue with the forward pass, which ranges from link <span class="math">\(i = 1\)</span> to the
last link <span class="math">\(i = n\)</span> of the tree:</p>
<pre class="code python literal-block">
<span class="c1"># def rnea(q, qd, qdd, f_ext):</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="c1"># p for "parent"</span>
<span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">compute_joint</span><span class="p">(</span><span class="n">joint_type</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">q</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qdd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">])</span> <span class="o">-</span> <span class="n">f_ext</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="c1"># to be continued below...</span>
</pre>
<p>The backward pass traverses the same range in reverse order:</p>
<pre class="code python literal-block">
<span class="c1"># def rnea(q, qd, qdd, f_ext):</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">tau</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">f</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
</pre>
<p>Wrapping it up, we get:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">rnea</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">qd</span><span class="p">,</span> <span class="n">qdd</span><span class="p">,</span> <span class="n">f_ext</span><span class="p">):</span>
<span class="n">n</span> <span class="o">=</span> <span class="nb">len</span><span class="p">(</span><span class="n">qd</span><span class="p">)</span>
<span class="n">v</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">a</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">f</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">tau</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">(</span><span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)]</span>
<span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">6</span><span class="p">,))</span>
<span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="o">-</span><span class="mf">9.81</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="mi">1</span><span class="p">,</span> <span class="n">n</span> <span class="o">+</span> <span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">compute_joint</span><span class="p">(</span><span class="n">joint_type</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">q</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qdd</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">qd</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
<span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">a</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">+</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">I</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">*</span> <span class="n">v</span><span class="p">[</span><span class="n">i</span><span class="p">])</span> <span class="o">-</span> <span class="n">f_ext</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="mi">1</span><span class="p">):</span>
<span class="n">p</span> <span class="o">=</span> <span class="n">lambda_</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">tau</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">S</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="n">f</span><span class="p">[</span><span class="n">p</span><span class="p">]</span> <span class="o">+=</span> <span class="n">X_p_to_i</span><span class="p">[</span><span class="n">i</span><span class="p">]</span><span class="o">.</span><span class="n">T</span> <span class="o">*</span> <span class="n">f</span><span class="p">[</span><span class="n">i</span><span class="p">]</span>
<span class="k">return</span> <span class="n">tau</span>
</pre>
<p>Lists of arrays with different lengths are typically an internal structure in
rigid body dynamics libraries or simulators. A mapping from such lists to a
flat array structure is called an <em>articulation</em>, and decides <em>e.g.</em> how to
represent orientations for spherical and free-flyer joints (unit quaternions?
full rotation matrices? or, heaven forfend, some flavor of Euler angles).</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Dually to forward kinematics being the "easy" problem relative to <a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse
kinematics</a>, inverse dynamics is "easy"
relative to <a class="reference external" href="https://scaron.info/robot-locomotion/forward-dynamics.html">forward dynamics</a>. The
recursive Newton-Euler algorithm we saw here was proposed by <a class="reference external" href="https://doi.org/10.1115/1.3139699">Walker and Orin
(1982)</a>. This <a class="reference external" href="https://www.youtube.com/watch?v=ZASVKAlegfQ">video supplement from
Modern Robotics</a> gives a good
overview of it.</p>
<p>Code-wise, this algorithm is implemented in rigid body dynamics libraries, such
as <a class="reference external" href="https://www.openrave.org/">OpenRAVE</a> and <a class="reference external" href="https://stack-of-tasks.github.io/pinocchio/">Pinocchio</a>, as well as dynamic simulators,
such as <a class="reference external" href="https://raisim.com/">Raisim</a>. It is summed up in concise form in
Table 5.1 of Featherstone's <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid body dynamics algorithms</a>.</p>
<p><strong>Thanks</strong> to Alex C. for <a class="reference external" href="#comment-external-force-and-acceleration">correcting</a> an error in a previous version of this post!</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Point mass model2021-11-03T00:00:00+01:002021-11-03T00:00:00+01:00Stéphane Carontag:scaron.info,2021-11-03:/robot-locomotion/point-mass-model.html<p>The point mass model is a recurring reduced model in physics in general, and in
locomotion modeling in particular. It is a common ancestor to the inverted
pendulum and <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> models. In this post, we will
review the assumptions that define it, and some of their not-so-intuitive
consequences …</p><p>The point mass model is a recurring reduced model in physics in general, and in
locomotion modeling in particular. It is a common ancestor to the inverted
pendulum and <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> models. In this post, we will
review the assumptions that define it, and some of their not-so-intuitive
consequences.</p>
<div class="section" id="sufficient-actuation">
<h2>Sufficient actuation</h2>
<p>The point mass model can be used as a reduced model assuming first that the
robot has enough joint torque to provide for the actuated part of its
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> (Assumption 1). This
way, we focus on the remaining <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton-Euler equations</a> that correspond to the six unactuated
coordinates of the robot's floating base:</p>
<div class="math">
\begin{equation*}
\begin{bmatrix} m \bfpdd_G \\ \dot{\bfL}_G \end{bmatrix}
=
\begin{bmatrix} \bff \\ \bftau_G \end{bmatrix}
+
\begin{bmatrix} m \bfg \\ \boldsymbol{0} \end{bmatrix}
\end{equation*}
</div>
<p>where on the left-hand side <span class="math">\(\bfp_G\)</span> is the position of the center
of mass (CoM) and <span class="math">\(\bfL_G\)</span> is the net angular momentum around the CoM,
while on the right-hand side <span class="math">\(\bff\)</span> is the resultant of contact forces,
<span class="math">\(\bftau_G\)</span> is the moment of contact forces around the CoM, <span class="math">\(m\)</span> is
the robot mass and <span class="math">\(\bfg\)</span> is the gravity vector.</p>
</div>
<div class="section" id="concentrated-mass">
<h2>Concentrated mass</h2>
<p>The second key assumption is that all of the robot's mass is concentrated at
the CoM, yet contact forces can still act on the robot via massless limbs. A
non-straightforward consequence of this is hypothesis is that the reaction
force <em>has to</em> go through the center of mass, where the net angular momentum is
zero. At Assumption 1, Newton-Euler equations leave us with:</p>
<div class="math">
\begin{equation*}
\dot{\bfL}_G = \bftau_G = (\bfp_Z - \bfp_G) \times \bff
\end{equation*}
</div>
<p>where <span class="math">\(Z\)</span> is the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">ZMP</a>, which
we can think of in what follows as the point where the resultant force is
applied (for more precision, see the ZMP <a class="reference external" href="https://scaron.info/robotics/zero-tilting-moment-point.html#definition">definition as a non-central axis</a>). If
our reduced model was a rigid body, its angular velocity would be such that</p>
<div class="math">
\begin{equation*}
\bfI_G \dot{\bfomega} = \dot{\bfL}_G = (\bfp_Z - \bfp_G) \times \bff
\end{equation*}
</div>
<p>where the (locked) inertia matrix <span class="math">\(\bfI_G\)</span> is a positive definite matrix.
We see that the resultant force <span class="math">\(\bff\)</span> does not have to go through the
CoM, and that its deviation from the ZMP-CoM axis yields angular accelerations
<span class="math">\(\dot{\bfomega}\)</span>. For the sake of our thought experiment (this is not a
proper proof), let's assume we keep our angular acceleration constant and look
at what happens when we and concentrate all the robot's mass at its CoM. The
inertia matrix <span class="math">\(\bfI_G\)</span> is obtained by summing up all body inertias:</p>
<div class="math">
\begin{equation*}
\bfI_G = \sum_{\mathrm{body} \ i} \bfI_i + m_i (\bfp_i - \bfp_G)_\times (\bfp_i - \bfp_G)^\top_\times
\end{equation*}
</div>
<p>where <span class="math">\(\bfI_i\)</span> is the standard inertia matrix at body <span class="math">\(i\)</span>'s center
of mass, as is <em>e.g.</em> described in the <a class="reference external" href="https://wiki.ros.org/urdf/XML/link">inertial element of the URDF format</a>. It is calculated by summing similarly
over all volumes of the body with non-zero mass density, with <a class="reference external" href="https://en.wikipedia.org/wiki/List_of_moments_of_inertia">formulas
available</a> for
primitive volumes.</p>
<p>Now, what happens as we shift away all of the mass to the CoM? Body inertia
matrices <span class="math">\(\bfI_i \to \bfzero\)</span> as all bodies become massless and their
respective mass density functions become zero. Distances <span class="math">\(\| \bfp_i -
\bfp_G \| \to 0\)</span> for non-zero masses become zero as well, since all the mass is
moved to the CoM. We then end up, at the limit, with <span class="math">\(\bfI_G \to
\bfzero\)</span>. Since for the sake of the example we are keeping
<span class="math">\(\dot{\bfomega}\)</span> at a fixed value, this means:</p>
<div class="math">
\begin{equation*}
(\bfp_Z - \bfp_G) \times \bff = \bfI_G \dot{\bfomega} \to \bfzero
\end{equation*}
</div>
<p>That is, <span class="math">\(\bff\)</span> and <span class="math">\((\bfp_Z - \bfp_G)\)</span> become aligned, and the
resultant force goes through the center of mass. Here is a visual summary of
this property:</p>
<img alt="From rigid body to point mass model" class="center max-width-80pct" src="https://scaron.info/figures/rigid-body-to-point-mass.png" />
<p>In the limit of this process, angular coordinates have vanished from our
equations of motion and we are left with:</p>
\begin{align}
m \bfpdd_G & = \bff \\
(\bfp_Z - \bfp_G) \times \bff & = \bfzero
\end{align}<p>This property that the resultant force goes through the center of mass is
common to all point mass models, including the inverted pendulum and linear
inverted pendulum models.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The point mass model is frequently used in legged locomotion via the <em>inverted
pendulum</em> or <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> models. It is also used
directly, for instance for <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01574687/file/tro_capture_point.pdf">fall prediction</a> of
limbed robots making multiple contacts with their environment.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Simple linear regression with online updates2021-10-07T00:00:00+02:002021-10-07T00:00:00+02:00Stéphane Carontag:scaron.info,2021-10-07:/blog/simple-linear-regression-with-online-updates.html<p>Simple linear regression is a particular case of linear regression where we
assume that the output <span class="math">\(y \in \mathbb{R}\)</span> is an affine transform of the
input <span class="math">\(x \in \mathbb{R}\)</span>:</p>
<div class="math">
\begin{equation*}
y = \alpha + \beta x
\end{equation*}
</div>
<p>We gather observations <span class="math">\((x_i, y_i)\)</span> and search for the parameters
<span class="math">\((\alpha …</span></p><p>Simple linear regression is a particular case of linear regression where we
assume that the output <span class="math">\(y \in \mathbb{R}\)</span> is an affine transform of the
input <span class="math">\(x \in \mathbb{R}\)</span>:</p>
<div class="math">
\begin{equation*}
y = \alpha + \beta x
\end{equation*}
</div>
<p>We gather observations <span class="math">\((x_i, y_i)\)</span> and search for the parameters
<span class="math">\((\alpha, \beta)\)</span> that minimize a loss function such as the residual sum
of squares. Graphically, we are fitting the line that minimizes the sum of
squared distances to every point:</p>
<img alt="Simple linear regression" class="center" src="https://scaron.info/figures/simple-linear-regression.png" />
<p>Compared to a full-fledged <a class="reference external" href="https://en.wikipedia.org/wiki/Linear_regression">linear regression</a>, simple linear regression
has a number of properties we can use for faster computations and online
updates. In this post, we will see how to implement it in Python, first with a
fixed dataset, then with online updates, and finally with a sliding-window
variant where recent observations have more weight than older ones.</p>
<div class="section" id="id1">
<h2>Linear regression</h2>
<p>Let us start with a vanilla solution using an ordinary linear regression
solver. We stack our inputs into a vector <span class="math">\(\bfx = [x_1, \ldots,
x_N]^\top\)</span> and their corresponding outputs into <span class="math">\(\bfy = [y_1, \ldots,
y_N]^\top\)</span>. Our objective function is the residual sum of squares:</p>
<div class="math">
\begin{equation*}
\underset{\alpha, \beta}{\mathrm{minimize}} \ \| \bfy - \alpha \bfone - \beta \bfx \|_2^2
\end{equation*}
</div>
<p>where <span class="math">\(\bfone\)</span> is the column vector filled with ones that has the same
dimension as <span class="math">\(\bfx\)</span> and <span class="math">\(\bfy\)</span>. The parameter <span class="math">\(\alpha\)</span> is
called the <em>intercept</em> while <span class="math">\(\beta\)</span> is the <em>slope</em> (or coefficient).</p>
<div class="section" id="using-a-machine-learning-library">
<h3>Using a machine learning library</h3>
<p>For our tests, we will generate data using the
same model as for the figure above:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="k">def</span> <span class="nf">generate_dataset</span><span class="p">(</span><span class="n">size</span><span class="o">=</span><span class="mi">10000</span><span class="p">,</span> <span class="n">alpha</span><span class="o">=</span><span class="mf">1.5</span><span class="p">,</span> <span class="n">beta</span><span class="o">=</span><span class="mf">3.15</span><span class="p">):</span>
<span class="n">x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">random</span><span class="p">(</span><span class="n">size</span><span class="p">)</span>
<span class="n">y</span> <span class="o">=</span> <span class="n">alpha</span> <span class="o">+</span> <span class="n">beta</span> <span class="o">*</span> <span class="n">x</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mf">0.4</span><span class="p">,</span> <span class="n">size</span><span class="o">=</span><span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">)</span>
<span class="k">return</span> <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
</pre>
<p>Any machine learning library will happily estimate the intercept and
coefficients of a linear model from such observation data. Try it out with your
favorite one! Here is for example how linear regression goes with <a class="reference external" href="https://scikit-learn.org/stable/modules/linear_model.html#ordinary-least-squares">scikit-learn</a>:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">sklearn.linear_model</span>
<span class="k">def</span> <span class="nf">simple_linear_regression_sklearn</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
<span class="n">regressor</span> <span class="o">=</span> <span class="n">sklearn</span><span class="o">.</span><span class="n">linear_model</span><span class="o">.</span><span class="n">LinearRegression</span><span class="p">()</span>
<span class="n">regressor</span><span class="o">.</span><span class="n">fit</span><span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">),</span> <span class="n">y</span><span class="p">)</span>
<span class="n">intercept</span> <span class="o">=</span> <span class="n">regressor</span><span class="o">.</span><span class="n">intercept_</span>
<span class="n">slope</span> <span class="o">=</span> <span class="n">regressor</span><span class="o">.</span><span class="n">coef_</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
<span class="k">return</span> <span class="p">(</span><span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span><span class="p">)</span>
</pre>
<p>Yet, our problem is simpler than a general linear regression. What if we solved
the <a class="reference external" href="https://scaron.info/blog/linear-least-squares-in-python.html">least squares</a> problem
directly?</p>
</div>
<div class="section" id="using-a-least-squares-solver">
<h3>Using a least squares solver</h3>
<p>We can rewrite our objective function in matrix form as:</p>
<div class="math">
\begin{equation*}
\| \bfy - \alpha \bfone - \beta \bfx \|_2^2
= \left\|
\begin{bmatrix} \bfone & \bfx \end{bmatrix}
\begin{bmatrix} \alpha \\ \beta \end{bmatrix}
- \bfy
\right\|_2^2
= \| \bfR \bfx - \bfy \|_2^2
\end{equation*}
</div>
<p>with <span class="math">\(\bfR\)</span> the <span class="math">\(2 \times N\)</span> matrix with a first column of ones and
<span class="math">\(\bfx\)</span> as second column. We can give these matrices to any least squares
solver, for instance in Python using the <a class="reference external" href="https://scaron.info/doc/qpsolvers/least-squares.html#qpsolvers.solve_ls">solve_ls</a>
function from <a class="reference external" href="https://pypi.org/project/qpsolvers/">qpsolvers</a>:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">qpsolvers</span>
<span class="k">def</span> <span class="nf">simple_linear_regression_ls</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="s2">"quadprog"</span><span class="p">):</span>
<span class="n">shape</span> <span class="o">=</span> <span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="mi">1</span><span class="p">)</span>
<span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">np</span><span class="o">.</span><span class="n">ones</span><span class="p">(</span><span class="n">shape</span><span class="p">),</span> <span class="n">x</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="n">shape</span><span class="p">)])</span>
<span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span> <span class="o">=</span> <span class="n">qpsolvers</span><span class="o">.</span><span class="n">solve_ls</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">s</span><span class="o">=</span><span class="n">y</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="n">solver</span><span class="p">)</span>
<span class="k">return</span> <span class="p">(</span><span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span><span class="p">)</span>
</pre>
<p>This results in a faster implementation than the previous one, for instance on
my machine:</p>
<pre class="code literal-block">
In [1]: x, y = generate_dataset(1000 * 1000)
In [2]: %timeit simple_linear_regression_sklearn(x, y)
21.2 ms ± 22.8 µs per loop (mean ± std. dev. of 7 runs, 10 loops each)
In [3]: %timeit simple_linear_regression_ls(x, y, solver="quadprog")
11.4 ms ± 73.9 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)
</pre>
<p>The improvement may be due to avoiding extra conversions inside
<tt class="docutils literal"><span class="pre">scikit-learn</span></tt>, or because the solver it uses internally is less efficient
than <a class="reference external" href="https://pypi.org/project/quadprog/">quadprog</a> for this type of
problems. It is more significant for small datasets, and ultimately settles
around <span class="math">\(2\times\)</span> for large datasets. Yet, we are still relying on a
general least squares solution (we can add columns to <span class="math">\(\bfR\)</span> for more
input variables), which means we are not squeezing out all the improvements we
can get from <em>simple</em> linear regression.</p>
</div>
</div>
<div class="section" id="simple-linear-regression">
<h2>Simple linear regression</h2>
<p>Let us expand our objective function using the vector dot product:</p>
\begin{align}
& \| \bfy - (\alpha \bfone + \beta \bfx) \|_2^2 \\
& = ((\alpha \bfone + \beta \bfx) - \bfy) \cdot ((\alpha \bfone + \beta \bfx) - \bfy) \\
& = \| \alpha \bfone + \beta \bfx \|_2^2 - 2 \bfy \cdot (\alpha \bfone + \beta \bfx) + (\bfy \cdot \bfy) \\
& = \alpha^2 (\bfone \cdot \bfone) + 2 \alpha \beta (\bfone \cdot \bfx) + \beta^2 (\bfx \cdot \bfx) - 2 (\bfone \cdot \bfy) \alpha - 2 (\bfx \cdot \bfy) \beta + (\bfy \cdot \bfy)
\end{align}<p>Since this function is strictly convex, its global minimum is the (unique)
<a class="reference external" href="https://en.wikipedia.org/wiki/Critical_point_(mathematics)#Several_variables">critical point</a>
where its gradient vanishes:</p>
\begin{align}
\frac{\partial}{\partial \alpha} \| \bfy - (\alpha \bfone + \beta \bfx) \|_2^2 & = 0 &
\frac{\partial}{\partial \beta} \| \bfy - (\alpha \bfone + \beta \bfx) \|_2^2 & = 0
\end{align}<p>Using the dot product expansion, this condition becomes:</p>
\begin{align}
2 (\bfone \cdot \bfone) \alpha + 2 (\bfone \cdot \bfx) \beta + 0 - 2 (\bfone \cdot \bfy) + 0 + 0 & = 0 \\
0 + 2 (\bfone \cdot \bfx) \alpha + 2 (\bfx \cdot \bfx) \beta - 0 - 2 (\bfx \cdot \bfy) + 0 & = 0
\end{align}<p>We end up with a <span class="math">\(2 \times 2\)</span> system of linear equations:</p>
\begin{align}
(\bfone \cdot \bfone) \alpha + (\bfone \cdot \bfx) \beta & = (\bfone \cdot \bfy) \\
(\bfone \cdot \bfx) \alpha + (\bfx \cdot \bfx) \beta & = (\bfx \cdot \bfy)
\end{align}<p>We know its analytical solution by (manual Gaussian elimination or remembering
the formula for) <a class="reference external" href="https://en.wikipedia.org/wiki/Cramer%27s_rule">Cramer's rule</a>:</p>
\begin{align}
\alpha & = \frac{(\bfone \cdot \bfy) (\bfx \cdot \bfx) - (\bfx \cdot \bfy) (\bfone \cdot \bfx)}{(\bfone \cdot \bfone) (\bfx \cdot \bfx) - (\bfone \cdot \bfx)^2} &
\beta & = \frac{(\bfone \cdot \bfone) (\bfx \cdot \bfy) - (\bfone \cdot \bfx) (\bfone \cdot \bfy)}{(\bfone \cdot \bfone) (\bfx \cdot \bfx) - (\bfone \cdot \bfx)^2}
\end{align}<p>The numerators and denominator of these two expressions have a statistical
interpretation detailed in <a class="reference external" href="https://en.wikipedia.org/w/index.php?title=Simple_linear_regression&oldid=1046955640#Fitting_the_regression_line">fitting the regression line</a>
(Wikipedia). The denominator is positive by the <a class="reference external" href="https://en.wikipedia.org/wiki/Cauchy%E2%80%93Schwarz_inequality">Cauchy-Schwarz inequality</a>. It can be
zero in the degenerate case where <span class="math">\(\bfx = x \bfone\)</span>, that is, when all
input values are equal, but we will assume that's not the case in what follows.
All in all, the Python code corresponding to this solution needs only NumPy and
floating-point arithmetic:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">simple_linear_regression_cramer</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
<span class="n">dot_1_1</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="c1"># == np.dot(ones, ones)</span>
<span class="n">dot_1_x</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == np.dot(x, ones)</span>
<span class="n">dot_1_y</span> <span class="o">=</span> <span class="n">y</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == np.dot(y, ones)</span>
<span class="n">dot_x_x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">x</span><span class="p">)</span>
<span class="n">dot_x_y</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">**</span> <span class="mi">2</span> <span class="c1"># not checking det == 0</span>
<span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_y</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_x_y</span> <span class="o">*</span> <span class="n">dot_1_x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_y</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">*</span> <span class="n">dot_1_y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="k">return</span> <span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span>
</pre>
<p>This implementation is not only faster than the previous ones, it's also the
first one where we leveraged some particular structure of our problem:</p>
<pre class="code literal-block">
In [1]: x, y = generate_dataset(1000 * 1000)
In [2]: %timeit simple_linear_regression_ls(x, y, solver="quadprog")
11.4 ms ± 36 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)
In [3]: %timeit simple_linear_regression_cramer(x, y)
3.72 ms ± 160 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)
</pre>
<p>We can use some IPython magic to compare the three approaches we have seen so
far in a quick benchmark:</p>
<pre class="code python literal-block">
<span class="n">sizes</span> <span class="o">=</span> <span class="nb">sorted</span><span class="p">([</span><span class="n">k</span> <span class="o">*</span> <span class="mi">10</span> <span class="o">**</span> <span class="n">d</span> <span class="k">for</span> <span class="n">k</span> <span class="ow">in</span> <span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">5</span><span class="p">,</span> <span class="mi">10</span><span class="p">]</span> <span class="k">for</span> <span class="n">d</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">6</span><span class="p">)])</span>
<span class="k">for</span> <span class="n">size</span> <span class="ow">in</span> <span class="n">sizes</span><span class="p">:</span>
<span class="n">x</span><span class="p">,</span> <span class="n">y</span> <span class="o">=</span> <span class="n">generate_dataset</span><span class="p">(</span><span class="n">size</span><span class="p">)</span>
<span class="n">instructions</span> <span class="o">=</span> <span class="p">[</span>
<span class="s2">"simple_linear_regression_sklearn(x, y)"</span><span class="p">,</span>
<span class="s1">'simple_linear_regression_ls(x, y, solver="quadprog")'</span><span class="p">,</span>
<span class="s2">"simple_linear_regression_cramer(x, y)"</span><span class="p">,</span>
<span class="p">]</span>
<span class="k">for</span> <span class="n">instruction</span> <span class="ow">in</span> <span class="n">instructions</span><span class="p">:</span>
<span class="nb">print</span><span class="p">(</span><span class="n">instruction</span><span class="p">)</span> <span class="c1"># helps manual labor to gather the numbers ;)</span>
<span class="n">get_ipython</span><span class="p">()</span><span class="o">.</span><span class="n">magic</span><span class="p">(</span><span class="sa">f</span><span class="s2">"timeit </span><span class="si">{</span><span class="n">instruction</span><span class="si">}</span><span class="s2">"</span><span class="p">)</span>
</pre>
<p>Here is the outcome in a plot (raw data in the image's <tt class="docutils literal">alt</tt> attribute):</p>
<img alt="Benchmark of computation times for all three solutions. Here is the raw data: [(221e-6, 36.2e-6, 5.55e-6), (218e-6, 37.3e-6, 5.56e-6), (217e-6, 36.3e-6, 5.62e-6), (216e-6, 36.5e-6, 5.64e-6), (219e-6, 36.8e-6, 5.74e-6), (219e-6, 37.2e-6, 5.7e-6), (222e-6, 38.3e-6, 6.01e-6), (230e-6, 39.8e-6, 6.32e-6), (241e-6, 42.7e-6, 7.09e-6), (251e-6, 47.6e-6, 8.42e-6), (288e-6, 141e-6, 12.4e-6), (346e-6, 190e-6, 19.5e-6), (452e-6, 283e-6, 52.4e-6), (775e-6, 544e-6, 83.1e-6), (2.32e-3, 987e-6, 138e-6), (3.94e-3, 2.02e-3, 232e-6), (13.2e-3, 5.66e-3, 1.41e-3), (21.8e-3, 11.7e-3, 4.0e-3)]." class="center" src="https://scaron.info/figures/simple-linear-regression-benchmark.png" />
<p>We now have a reasonably efficient solution for simple linear regression over
small- to medium-sized (millions of entries) datasets. All the solutions we
have seen so far have been offline, meaning the dataset was generated then
processed all at once. Let us now turn to the <em>online</em> case, where new
observations <span class="math">\((x_i, y_i)\)</span> come one after the other.</p>
</div>
<div class="section" id="online-simple-linear-regression">
<h2>Online simple linear regression</h2>
<p>The dot products we use to calculate the intercept <span class="math">\(\alpha\)</span> and slope
<span class="math">\(\beta\)</span> are ready for recursive updates. Suppose we already now their
values for <span class="math">\(k\)</span> previous observations. When a new observation
<span class="math">\((x_{k+1}, y_{k+1})\)</span> comes in, we can update them by:</p>
\begin{align}
(\bfone \cdot \bfone)_{k+1} & = (\bfone \cdot \bfone)_k + 1 \\
(\bfone \cdot \bfx)_{k+1} & = (\bfone \cdot \bfx)_k + x_{k+1} \\
(\bfone \cdot \bfy)_{k+1} & = (\bfone \cdot \bfy)_k + y_{k+1} \\
(\bfx \cdot \bfx)_{k+1} & = (\bfx \cdot \bfx)_k + x_{k+1}^2 \\
(\bfx \cdot \bfy)_{k+1} & = (\bfx \cdot \bfy)_k + x_{k+1} y_{k+1}
\end{align}<p>The same logic generalizes to a new batch of observations <span class="math">\((\bfx_{k+1},
\bfy_{k+1})\)</span>:</p>
\begin{align}
(\bfone \cdot \bfone)_{k+1} & = (\bfone \cdot \bfone)_k + \mathrm{dim}(\bfx_{k+1}) \\
(\bfone \cdot \bfx)_{k+1} & = (\bfone \cdot \bfx)_k + (\bfone \cdot \bfx_{k+1}) \\
(\bfone \cdot \bfy)_{k+1} & = (\bfone \cdot \bfy)_k + (\bfone \cdot \bfy_{k+1}) \\
(\bfx \cdot \bfx)_{k+1} & = (\bfx \cdot \bfx)_k + (\bfx_{k+1} \cdot \bfx_{k+1}) \\
(\bfx \cdot \bfy)_{k+1} & = (\bfx \cdot \bfy)_k + (\bfx_{k+1} \cdot \bfy_{k+1})
\end{align}<p>Keep in mind that, although we write them in vector form, dot products by
<span class="math">\(\bfone\)</span> are simply sums. We can wrap these update rules in a Python
class for a simple linear regressor that keeps track of dot products
internally, and outputs <span class="math">\(\alpha\)</span> and <span class="math">\(\beta\)</span> upon new observations:</p>
<pre class="code python literal-block">
<span class="k">class</span> <span class="nc">SimpleLinearRegressor</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="kc">None</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="kc">None</span>
<span class="k">def</span> <span class="nf">update</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">+=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span>
<span class="p">[</span>
<span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>
<span class="n">x</span><span class="o">.</span><span class="n">sum</span><span class="p">(),</span>
<span class="n">y</span><span class="o">.</span><span class="n">sum</span><span class="p">(),</span>
<span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">x</span><span class="p">),</span>
<span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">),</span>
<span class="p">]</span>
<span class="p">)</span>
<span class="n">size</span><span class="p">,</span> <span class="n">sum_x</span><span class="p">,</span> <span class="n">sum_y</span><span class="p">,</span> <span class="n">sum_xx</span><span class="p">,</span> <span class="n">sum_xy</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">dots</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">size</span> <span class="o">*</span> <span class="n">sum_xx</span> <span class="o">-</span> <span class="n">sum_x</span> <span class="o">**</span> <span class="mi">2</span>
<span class="k">if</span> <span class="n">det</span> <span class="o">></span> <span class="mf">1e-10</span><span class="p">:</span> <span class="c1"># determinant may be zero initially</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">sum_xx</span> <span class="o">*</span> <span class="n">sum_y</span> <span class="o">-</span> <span class="n">sum_xy</span> <span class="o">*</span> <span class="n">sum_x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">sum_xy</span> <span class="o">*</span> <span class="n">size</span> <span class="o">-</span> <span class="n">sum_x</span> <span class="o">*</span> <span class="n">sum_y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
</pre>
<p>This solution is equivalent to <a class="reference external" href="https://stackoverflow.com/a/52071283">this answer from StackOverflow</a>. It treats all observations the same,
which is pertinent for non-time series data. But we can also tweak our
derivation so far to the more specific use case of sliding-window regression
over time series data.</p>
</div>
<div class="section" id="sliding-window-simple-linear-regression">
<h2>Sliding-window simple linear regression</h2>
<p>Suppose now that single observations <span class="math">\((x_i, y_i)\)</span> come in succession
every <span class="math">\(\delta t\)</span> timesteps, and we want to estimate the <em>instantaneous</em>
slope <span class="math">\(\beta\)</span> and intercept <span class="math">\(\alpha\)</span> of our recent data. Similarly
to the exponential moving average filter, we can define a <em>forgetting factor</em>
<span class="math">\(\lambda \in (0, 1)\)</span> as:</p>
<div class="math">
\begin{equation*}
\lambda = \exp\left(-\frac{\delta t}{T}\right)
\end{equation*}
</div>
<p>where <span class="math">\(T\)</span> is the time constant that characterizes the span of our sliding
window. Note that, by the <a class="reference external" href="https://en.wikipedia.org/wiki/Nyquist%E2%80%93Shannon_sampling_theorem">Nyquist–Shannon sampling theorem</a>,
<span class="math">\(T\)</span> has to be at least <span class="math">\(2 \delta t\)</span> to avoid aliasing. We can use
this forgetting factor in our objective function as a discount rate applied to
previous observations:</p>
<div class="math">
\begin{equation*}
\mathit{fit}(\alpha, \beta, \bfx_k, \bfy_k) = \sum_{i=0}^{k-1} \lambda^{i} (y_{k-i} - \alpha - \beta x_{k-i})^2
\end{equation*}
</div>
<p>To calculate this exponentially discounted residual sum of squares efficiently,
we can modify the definition of our vectors <span class="math">\(\bfx_k\)</span>, <span class="math">\(\bfy_k\)</span>, and
<span class="math">\(\bfone_k\)</span>, to incorporate the forgetting factor:</p>
\begin{align}
\bfx_{k+1} & = \begin{bmatrix} \sqrt{\lambda} \bfx_k \\ x_{k+1} \end{bmatrix} &
\bfy_{k+1} & = \begin{bmatrix} \sqrt{\lambda} \bfy_k \\ y_{k+1} \end{bmatrix} &
\bfone_{k+1} & = \begin{bmatrix} \sqrt{\lambda} \bfone_k \\ 1 \end{bmatrix}
\end{align}<p>The benefit of this approach is that the squared norms of our new vectors now
correspond to the exponentially discounted residual sum of squares:</p>
\begin{align}
\| \bfy_{k+1} - \alpha \bfone_{k+1} - \beta \bfx_{k+1} \|_2^2
& = (y_{k+1} - \alpha - \beta x_{k+1})^2 + \lambda \| \bfy_{k} - \alpha \bfone_k - \beta \bfx_k \|_2^2 \\
& = \ \ldots \\
& = \sum_{i=0}^{k} \lambda^{i} (y_{k+1-i} - \alpha - \beta x_{k+1-i})^2 \\
& = \mathit{fit}(\alpha, \beta, \bfx_{k+1}, \bfy_{k+1})
\end{align}<p>Granted, we took some liberties with the notation <span class="math">\(\bfone_k\)</span> as it's not
a vector of ones any more ;-) But the expression <span class="math">\((\bfone_k \cdot
\bfv_k)\)</span> keeps the semantic of "summing the coordinates of <span class="math">\(\bfv_k\)</span>",
although is now in an exponentially discounted sense. In terms of dot products,
our update rules translate to:</p>
\begin{align}
(\bfone \cdot \bfone)_{k+1} & = \lambda (\bfone \cdot \bfone)_k + 1 \\
(\bfone \cdot \bfx)_{k+1} & = \lambda (\bfone \cdot \bfx)_k + x_{k+1} \\
(\bfone \cdot \bfy)_{k+1} & = \lambda (\bfone \cdot \bfy)_k + y_{k+1} \\
(\bfx \cdot \bfx)_{k+1} & = \lambda (\bfx \cdot \bfx)_k + x_{k+1}^2 \\
(\bfx \cdot \bfy)_{k+1} & = \lambda (\bfx \cdot \bfy)_k + x_{k+1} y_{k+1}
\end{align}<p>This leads to a regressor class similar to the previous one:</p>
<pre class="code python literal-block">
<span class="k">class</span> <span class="nc">SlidingWindowLinearRegressor</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">dt</span><span class="p">,</span> <span class="n">time_constant</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="kc">None</span>
<span class="bp">self</span><span class="o">.</span><span class="n">forgetting_factor</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">exp</span><span class="p">(</span><span class="o">-</span><span class="n">dt</span> <span class="o">/</span> <span class="n">time_constant</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="kc">None</span>
<span class="k">def</span> <span class="nf">update</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">:</span> <span class="nb">float</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="nb">float</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">*=</span> <span class="bp">self</span><span class="o">.</span><span class="n">forgetting_factor</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dots</span> <span class="o">+=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">1</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">x</span> <span class="o">*</span> <span class="n">x</span><span class="p">,</span> <span class="n">x</span> <span class="o">*</span> <span class="n">y</span><span class="p">])</span>
<span class="n">dot_11</span><span class="p">,</span> <span class="n">dot_1x</span><span class="p">,</span> <span class="n">dot_1y</span><span class="p">,</span> <span class="n">dot_xx</span><span class="p">,</span> <span class="n">dot_xy</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">dots</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">dot_11</span> <span class="o">*</span> <span class="n">dot_xx</span> <span class="o">-</span> <span class="n">dot_1x</span> <span class="o">**</span> <span class="mi">2</span>
<span class="k">if</span> <span class="n">det</span> <span class="o">></span> <span class="mf">1e-10</span><span class="p">:</span> <span class="c1"># determinant is zero initially</span>
<span class="bp">self</span><span class="o">.</span><span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_xx</span> <span class="o">*</span> <span class="n">dot_1y</span> <span class="o">-</span> <span class="n">dot_xy</span> <span class="o">*</span> <span class="n">dot_1x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="bp">self</span><span class="o">.</span><span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_xy</span> <span class="o">*</span> <span class="n">dot_11</span> <span class="o">-</span> <span class="n">dot_1x</span> <span class="o">*</span> <span class="n">dot_1y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
</pre>
<p>Let's evaluate its performance on synthetic data:</p>
<pre class="code python literal-block">
<span class="n">dt</span> <span class="o">=</span> <span class="mf">1e-3</span> <span class="c1"># [s]</span>
<span class="n">T</span> <span class="o">=</span> <span class="mf">5e-2</span> <span class="c1"># [s]</span>
<span class="n">sliding_window_regressor</span> <span class="o">=</span> <span class="n">SlidingWindowLinearRegressor</span><span class="p">(</span><span class="n">dt</span><span class="p">,</span> <span class="n">T</span><span class="p">)</span>
<span class="k">for</span> <span class="n">t</span> <span class="ow">in</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">100.</span><span class="p">,</span> <span class="n">dt</span><span class="p">):</span>
<span class="n">alpha</span> <span class="o">=</span> <span class="mf">1.5</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="mf">0.1</span> <span class="o">*</span> <span class="n">t</span><span class="p">)</span>
<span class="n">beta</span> <span class="o">=</span> <span class="mf">3.15</span> <span class="o">+</span> <span class="mf">0.5</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="mf">1.1</span> <span class="o">*</span> <span class="n">t</span><span class="p">)</span>
<span class="n">x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">t</span><span class="p">)</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mf">0.05</span><span class="p">)</span>
<span class="n">y</span> <span class="o">=</span> <span class="n">alpha</span> <span class="o">+</span> <span class="n">beta</span> <span class="o">*</span> <span class="n">x</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">normal</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mf">0.1</span><span class="p">)</span>
<span class="n">sliding_window_regressor</span><span class="o">.</span><span class="n">update</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
</pre>
<p>This small test results in the following observations and estimates:</p>
<img alt="Estimates during sliding-window simple linear regression." class="center" src="https://scaron.info/figures/sliding-window-simple-linear-regression.png" />
<p>Contrary to regular simple linear regression, our filter now has a
time-constant parameter <span class="math">\(T\)</span> that defines the length of our sliding
window. Its ideal value depends on our data. In this example, it would depend
on all the inline parameters <tt class="docutils literal">[1.5, 0.1, 3.15, 0.5, 1.1, 0.05, 0.1]</tt> we use
to produce <span class="math">\(\alpha(t), \beta(t)\)</span> and the white noise on <span class="math">\(x(t),
y(t)\)</span>. Although, mainly, it will depend on the two constants <tt class="docutils literal">[0.1, 1.1]</tt>
that define how fast <span class="math">\(\alpha(t)\)</span> and <span class="math">\(\beta(t)\)</span> change. If we
choose it too small, our estimates will be snappy but noisy. For example with
<tt class="docutils literal">T = <span class="pre">5e-3</span></tt> s:</p>
<img alt="Estimates during sliding-window linear regression with a small time constant." class="center" src="https://scaron.info/figures/sliding-window-simple-linear-regression-small-time-constant.png" />
<p>If we choose it too large, our estimates will be smooth but delayed. For
example with <tt class="docutils literal">T = 5.0</tt> s:</p>
<img alt="Estimates during sliding-window linear regression with a large time constant." class="center" src="https://scaron.info/figures/sliding-window-simple-linear-regression-large-time-constant.png" />
<p>In this instance, the filter may be acceptable for <span class="math">\(\alpha(t)\)</span> but it
fails to capture the time variations of <span class="math">\(\beta(t)\)</span> (if we crank <span class="math">\(T\)</span>
up further we can even average them out completely). How to tune it from there
depends on the desired behavior.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Sliding window linear regression combines statistics and signal processing. Its
parameter <span class="math">\(T\)</span> is the same time constant as in a <a class="reference external" href="https://en.wikipedia.org/wiki/Low-pass_filter">low-pass filter</a>, and it can be selected by
specifying a <a class="reference external" href="https://en.wikipedia.org/wiki/Frequency_response">frequency response</a>, which does formally what
we commented on informally in our example when selecting <span class="math">\(T\)</span> to "capture"
or not the time-varying dynamics of <span class="math">\(\alpha(t)\)</span> and <span class="math">\(\beta(t)\)</span>. The
overall algorithm itself is a simplification of the <a class="reference external" href="https://en.wikipedia.org/wiki/Recursive_least_squares_filter">recursive least squares
filter</a> to our
use case.</p>
<p>When deriving our system of linear inequalities for offline regression, the
condition we used that the gradient vanishes at the global minimum of the
objective function is a particular case of <a class="reference external" href="https://en.wikipedia.org/wiki/Karush%E2%80%93Kuhn%E2%80%93Tucker_conditions">Karush–Kuhn–Tucker (KKT) conditions</a>
applied to our unconstrained problem. KKT conditions provide the general
solution when the problem also has inequality or equality constraints.</p>
<div class="section" id="bonus-simple-linear-regression-with-jax">
<h3>Bonus: simple linear regression with JAX</h3>
<p>For fun, we can look at the performance of the offline algorithm using <a class="reference external" href="https://github.com/google/jax">JAX</a>'s just-in-time compiler. JAX also gives us a
convenient way to try running the function on the CPU or GPU, so let's try
both:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">jax</span>
<span class="kn">import</span> <span class="nn">jax.numpy</span> <span class="k">as</span> <span class="nn">jnp</span>
<span class="k">def</span> <span class="nf">simple_linear_regression_cramer_jax</span><span class="p">(</span><span class="n">x</span><span class="p">:</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">):</span>
<span class="n">dot_1_1</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="c1"># == jnp.dot(ones, ones)</span>
<span class="n">dot_1_x</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == jnp.dot(x, ones)</span>
<span class="n">dot_1_y</span> <span class="o">=</span> <span class="n">y</span><span class="o">.</span><span class="n">sum</span><span class="p">()</span> <span class="c1"># == jnp.dot(y, ones)</span>
<span class="n">dot_x_x</span> <span class="o">=</span> <span class="n">jnp</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">x</span><span class="p">)</span>
<span class="n">dot_x_y</span> <span class="o">=</span> <span class="n">jnp</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="n">det</span> <span class="o">=</span> <span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">**</span> <span class="mi">2</span> <span class="c1"># not checking det == 0</span>
<span class="n">intercept</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_y</span> <span class="o">*</span> <span class="n">dot_x_x</span> <span class="o">-</span> <span class="n">dot_x_y</span> <span class="o">*</span> <span class="n">dot_1_x</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="n">slope</span> <span class="o">=</span> <span class="p">(</span><span class="n">dot_1_1</span> <span class="o">*</span> <span class="n">dot_x_y</span> <span class="o">-</span> <span class="n">dot_1_x</span> <span class="o">*</span> <span class="n">dot_1_y</span><span class="p">)</span> <span class="o">/</span> <span class="n">det</span>
<span class="k">return</span> <span class="n">intercept</span><span class="p">,</span> <span class="n">slope</span>
<span class="n">cpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">devices</span><span class="p">(</span><span class="s2">"cpu"</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>
<span class="n">gpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">devices</span><span class="p">(</span><span class="s2">"gpu"</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>
<span class="n">cramer_jax_cpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">jit</span><span class="p">(</span><span class="n">simple_linear_regression_cramer_jax</span><span class="p">,</span> <span class="n">device</span><span class="o">=</span><span class="n">cpu</span><span class="p">)</span>
<span class="n">cramer_jax_gpu</span> <span class="o">=</span> <span class="n">jax</span><span class="o">.</span><span class="n">jit</span><span class="p">(</span><span class="n">simple_linear_regression_cramer_jax</span><span class="p">,</span> <span class="n">device</span><span class="o">=</span><span class="n">gpu</span><span class="p">)</span>
<span class="k">for</span> <span class="n">size</span> <span class="ow">in</span> <span class="n">sizes</span><span class="p">:</span>
<span class="n">x</span><span class="p">,</span> <span class="n">y</span> <span class="o">=</span> <span class="n">generate_dataset</span><span class="p">(</span><span class="n">size</span><span class="p">)</span>
<span class="n">jx</span><span class="p">,</span> <span class="n">jy</span> <span class="o">=</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">x</span><span class="p">),</span> <span class="n">jnp</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">y</span><span class="p">)</span>
<span class="n">instructions</span> <span class="o">=</span> <span class="p">[</span><span class="s2">"cramer_jax_cpu(jx, jy)"</span><span class="p">,</span> <span class="s2">"cramer_jax_gpu(jx, jy)"</span><span class="p">]</span>
<span class="k">for</span> <span class="n">instruction</span> <span class="ow">in</span> <span class="n">instructions</span><span class="p">:</span>
<span class="n">get_ipython</span><span class="p">()</span><span class="o">.</span><span class="n">magic</span><span class="p">(</span><span class="sa">f</span><span class="s2">"timeit </span><span class="si">{</span><span class="n">instruction</span><span class="si">}</span><span class="s2">"</span><span class="p">)</span>
</pre>
<p>Here are the results of this benchmark on my machine (CPU: Intel® Core™
i7-6700K at 4.00 GHz, GPU: NVIDIA GeForce GTX 1070 with CUDA 10.1), combined
with the ones from the previous benchmark above:</p>
<img alt="Benchmark of computation times for all three solutions. Here is the raw data: [(221e-6, 36.2e-6, 4.86e-6, 36.9e-6, 31.0e-6), (218e-6, 37.3e-6, 4.86e-6, 33.7e-6, 30.0e-6), (217e-6, 36.3e-6, 4.92e-6, 36.6e-6, 32.4e-6), (216e-6, 36.5e-6, 4.86e-6, 34.6e-6, 30.9e-6), (219e-6, 36.8e-6, 4.96e-6, 35.0e-6, 33.0e-6), (219e-6, 37.2e-6, 4.97e-6, 35.3e-6, 33.2e-6), (222e-6, 38.3e-6, 5.11e-6, 36.2e-6, 32.0e-6), (230e-6, 39.8e-6, 5.48e-6, 39.7e-6, 33.8e-6), (241e-6, 42.7e-6, 6.12e-6, 42.0e-6, 33.7e-6), (251e-6, 47.6e-6, 7.10e-6, 43.2e-6, 35.7e-6), (288e-6, 141e-6, 10.1e-6, 74.8e-6, 34.4e-6), (346e-6, 190e-6, 14.0e-6, 80.3e-6, 33.2e-6), (452e-6, 283e-6, 46.9e-6, 126e-6, 32.9e-6), (775e-6, 544e-6, 80.1e-6, 257e-6, 32e-6), (2.32e-3, 987e-6, 138e-6, 470e-6, 31.4e-6), (3.94e-3, 2.02e-3, 269e-6, 895e-6, 32.9e-6), (13.2e-3, 5.66e-3, 772e-6, 2.13e-3, 36.4e-6), (21.8e-3, 11.7e-3, 2.53e-3, 4.75e-3, 56.6e-6)]." class="center" src="https://scaron.info/figures/simple-linear-regression-benchmark-jax.png" />
<p>Nothing beats simple NumPy on small datasets, and the turning point when
transferring to GPU becomes attractive seems to be around 20,000 observations.</p>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Joint torques and Jacobian transpose2021-09-29T00:00:00+02:002021-09-29T00:00:00+02:00Stéphane Carontag:scaron.info,2021-09-29:/blog/joint-torques-and-jacobian-transpose.html<p>Some concepts, when you learn them, seem to connect with what you already know
and build new objects that make sense, that is, with the approval of your
reason. This is the ideal scenario. It is likely that things flow nicely
because the intellectual path has been "polished" by previous …</p><p>Some concepts, when you learn them, seem to connect with what you already know
and build new objects that make sense, that is, with the approval of your
reason. This is the ideal scenario. It is likely that things flow nicely
because the intellectual path has been "polished" by previous travelers.
(Sometimes the fruit of centuries of effort and heated debates, such as the
conciliation of Euclidean and analytical geometry in mathematics.) And then,
there are the unpolished ones, those ideas and techniques that are still rough
around the edges. Papers and code seem to take for granted, so most likely they
work, yet there's no consensual explanation easy to find in the literature.</p>
<p>At the time I was a graduate student, the following equation was one of these
rough edges for me. Every once in a while I would run into a paper on humanoid
robots that would compute the joint torques cause by contact forces using the
transpose of the contact Jacobian:</p>
<div class="math">
\begin{equation*}
\bftau_c = \bfJ_c(\bfq)^\top \bff
\end{equation*}
</div>
<p>with <span class="math">\(\bftau_c\)</span> the vector of joint torques caused by contact forces,
<span class="math">\(\bfJ_c\)</span> the Jacobian of the contact constraint (for instance foot
contacts with the ground for a biped), <span class="math">\(\bfq\)</span> the configuration vector of
joint angles and floating-base coordinates, and <span class="math">\(\bff\)</span> the vector of
contact forces applied by the environment onto the robot. Papers would mention
it derives from the <a class="reference external" href="https://scaron.info/robot-locomotion/principle-of-virtual-work.html">principle of virtual work</a>, sometimes with no reference,
sometimes with a reference to a textbook on fixed-base manipulators (humanoids
are floating-base robots).</p>
<p>This equation is correct, yet I've been on the lookout for years before finding
a proof that did not cost me a leap of faith. (Are the assumptions clear? Is
every step of the demonstration correct?) Part of this is due to the evolution
of the literature from manipulators to floating-base robots: while the
confusion was not a big deal with the former, with the latter we should bear in
mind that <span class="math">\(\bftau_c\)</span> is <em>not</em> the vector of actuated joint torques of the
robot, but only the <em>contribution</em> to these torques caused by contact forces.
As of today, the clearest demonstration I could find is the <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html#principle-of-least-constraint">derivation from
the principle of least constraint</a>.
It leads us to the constrained equation of motion, where we can see other
contributions from gravity, the centripetal and Coriolis effects:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bfJ_c(q)^\top \bff
\end{equation*}
</div>
<p>Enough with the history! Here is a bonus question to see if you understand
these concepts better from today's literature: did you know that the vector of
joint torques caused by gravity can in fact be written as follows?</p>
<div class="math">
\begin{equation*}
\bftau_g(\bfq) = m \bfJ_{\mathrm{CoM}}(\bfq)^\top \bfg
\end{equation*}
</div>
<p>Here <span class="math">\(\bfg\)</span> is the three-dimensional acceleration due to gravity,
<span class="math">\(m\)</span> is the total mass of the robot and <span class="math">\(\bfJ_{\mathrm{CoM}}\)</span> is the
Jacobian of the center-of-mass position. Hint: the answer can be derived
directly from the principle of least action. It is also written down somewhere
linked from this website ;-)</p>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Principle of virtual work2021-09-28T00:00:00+02:002021-09-28T00:00:00+02:00Stéphane Carontag:scaron.info,2021-09-28:/robot-locomotion/principle-of-virtual-work.html<p>Although derivations based on <a class="reference external" href="https://en.wikipedia.org/wiki/Gauss%27s_principle_of_least_constraint">Gauss's principle of least constraint</a> are
now more commonplace (thank goodness!), the principle of virtual work is an
equivalent approach that we can still find in the literature. Also called
<a class="reference external" href="https://en.wikipedia.org/wiki/D%27Alembert%27s_principle">d'Alembert's principle</a>, it relates
constrained motions to their constraint forces.</p>
<div class="section" id="virtual-displacements">
<h2>Virtual displacements</h2>
<p>Consider the <a class="reference external" href="https://en.wikipedia.org/wiki/Holonomic_constraints">holonomic</a>
constraint …</p></div><p>Although derivations based on <a class="reference external" href="https://en.wikipedia.org/wiki/Gauss%27s_principle_of_least_constraint">Gauss's principle of least constraint</a> are
now more commonplace (thank goodness!), the principle of virtual work is an
equivalent approach that we can still find in the literature. Also called
<a class="reference external" href="https://en.wikipedia.org/wiki/D%27Alembert%27s_principle">d'Alembert's principle</a>, it relates
constrained motions to their constraint forces.</p>
<div class="section" id="virtual-displacements">
<h2>Virtual displacements</h2>
<p>Consider the <a class="reference external" href="https://en.wikipedia.org/wiki/Holonomic_constraints">holonomic</a>
constraint <span class="math">\(\bfc(\bfq) = \bfzero\)</span>, which stacks a set of equations that
our system must follow troughout its motions. It could be for instance a biped
keeping its feet in contact with the ground, or a humanoid pushing against a
wall. The corresponding constraint Jacobian is:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \qd = \frac{\partial \bfc}{\partial \bfq}(\bfq) \qd = \bfzero
\end{equation*}
</div>
<p>Let us define a <em>virtual displacement</em> <span class="math">\(\Delta \bfq\)</span> as any vector in the
nullspace of the constraint Jacobian:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \Delta \bfq = \bfzero
\end{equation*}
</div>
<p>Note that a virtual displacement <span class="math">\(\Delta \bfq\)</span> is not the same thing as
an actual displacement of the robot. At the acceleration level, a virtual
displacement <span class="math">\((\Delta \bfq, \Delta \qd)\)</span> only satisfies the constraint:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \Delta \qd + \Delta \bfq^\top \bfH_c(\bfq) \Delta \bfq = \bfzero
\end{equation*}
</div>
<p>Whereas an actual displacement <span class="math">\((\qd, \qdd)\)</span> satisfies both the
constraint and the equation of motion:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^T \bftau + \bftau_g(\bfq) + \bfJ_c(\bfq)^\top \bff \\
\bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd & = \bfzero
\end{align}<p>This distinction shows that virtual displacements are more general than actual
displacements. It is important to keep in mind for what follows.</p>
</div>
<div class="section" id="virtual-work">
<h2>Virtual work</h2>
<p>The principle of virtual work applies to <a class="reference external" href="https://en.wikipedia.org/wiki/Udwadia%E2%80%93Kalaba_formulation#Non-ideal_constraints">ideal</a>
constraints, <em>i.e.</em>, constraints whose virtual displacements are reversible. In
robotics, this basically means all constraints except those that involve
friction. The principle states that constraint forces, <em>i.e.</em>, the joint
torques <span class="math">\(\bftau_c\)</span> that maintain the constraint <span class="math">\(\bfc(\bfq) =
\bfzero\)</span> along a virtual displacement <span class="math">\(\Delta \bfq\)</span>, do not produce any
virtual work. That is:</p>
<div class="math">
\begin{equation*}
\bftau_c^\top \Delta \bfq = \bfzero
\end{equation*}
</div>
<p>Since <span class="math">\(\Delta \bfq\)</span> can be any virtual displacement in the nullspace
<span class="math">\({\cal N}(\bfJ_c)\)</span> of the constraint Jacobian, this means
<span class="math">\(\bftau_c\)</span> belongs to the orthogonal to this nullspace, which is also
<a class="reference external" href="https://math.stackexchange.com/a/2568906">equal to</a> the range of the
Jacobian transpose:</p>
<div class="math">
\begin{equation*}
\bftau_c \in {\cal N}(\bfJ_c)^\bot = {\cal R}(\bfJ_c^\top)
\end{equation*}
</div>
<p>As a consequence, there exists a vector <span class="math">\(\bff\)</span> of dual vectors such that
<span class="math">\(\bftau_c = \bfJ_c^\top \bff\)</span>. We <em>define</em> constraint forces as these
dual vectors. To continue our example, if the constraint is that the biped's
feet stay in contact with the ground, then <span class="math">\(\bff\)</span> are the ground contact
forces; if our constraint is that the humanoid's hands push against the wall,
then <span class="math">\(\bff\)</span> are the wall contact forces.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The alternative <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html#principle-of-least-constraint">Lagrangian derivation from the principle of least constraint</a>
characterizes forces further than what we did above. As a matter of fact, the
principle of virtual work can be seen as the critical condition associated with
the principle of least constraint: for the latter, motions minimize an
objective function (deviation from unconstrained motion), for the former,
motions are in the nullspace of the gradient of this objective function.
Informally, since the critical point of a strictly <a class="reference external" href="https://en.wikipedia.org/wiki/Convex_function">convex function</a> is its global optimum, the
two principles are equivalent.</p>
<p>The principle of virtual work is not always applicable: it only works with
ideal constraints where forces do not work along virtual displacements. This is
in particular not true for sliding friction where forces are opposite
end-effector velocities. In that case, the most straightforward solution is to
use a model of the friction force and map it through the Jacobian transpose in
the constrained equations of motion. For a proof of why this is valid, see for
instance equation (13) in Wieber's <a class="reference external" href="https://hal.inria.fr/inria-00390428/document">comments on the structure of the dynamics
of articulated motion</a>.
Alternatively, the <a class="reference external" href="https://en.wikipedia.org/wiki/Udwadia%E2%80%93Kalaba_formulation">Udwadia-Kalaba formulation</a> provides
an equivalent generalization of the principle of virtual work to <em>non-ideal</em>
constraints. It also derives from the principle of least constraint, yet it
involves pseudoinverses which are not as numerically stable as working directly
with the constrained equation of motion.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Forward dynamics2021-06-03T00:00:00+02:002021-06-03T00:00:00+02:00Stéphane Carontag:scaron.info,2021-06-03:/robot-locomotion/forward-dynamics.html<p>Forward dynamics 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 is the calculation of joint accelerations <span class="math">\(\qdd\)</span> satisfying the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a>:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd^\top …<p>Forward dynamics 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 is the calculation of joint accelerations <span class="math">\(\qdd\)</span> satisfying the
<a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a>:</p>
\begin{align}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \\
\bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = \boldsymbol{0}
\end{align}<p>Mathematically, we can write it as a function:</p>
<div class="math">
\begin{equation*}
\qdd = \mathrm{FD}(\bfq, \qd, \bftau, \bff)
\end{equation*}
</div>
<p>Note that this function implicitly depends on the underlying robot model, as
for instance different inertias yield different inertia matrices
<span class="math">\(\bfM(\bfq)\)</span>, thus different equations of motion that have different
solutions. There are two main algorithms to compute forward dynamics:</p>
<ul class="simple">
<li>The articulared body algorithm: <span class="math">\(\qdd = \mathrm{ABA}(\bfq, \qd, \bftau, \bff^\mathit{ext})\)</span></li>
<li>The composite rigid body algorithm: <span class="math">\(\qdd = \bfM(\bfq)^{-1} (\bftau - \bftau_0)\)</span></li>
</ul>
<p>Let us dive into these two formulas.</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>
\begin{align}
\qdd = & \mathrm{ABA}(\bfq, \qd, \bftau, \bff) \\
& \mathrm{s.t.} \ \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff
\end{align}<p>Its output <span class="math">\(\qdd\)</span> satisfies the unconstrained equation of motion, but it
doesn't take into account the holonomic contact constraint <span class="math">\(\bfJ(\bfq)
\qdd + \qd^\top \bfH(\bfq) \qd = \bfzero\)</span>.</p>
<blockquote>
<em>This article is a stub. Post a comment below if I don't come back to it!</em></blockquote>
</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>
<div class="math">
\begin{equation*}
\bfM(\bfq) = \mathrm{CRBA}(\bfq)
\end{equation*}
</div>
<p>Once the inertia matrix is known, we can solve forward dynamics as:</p>
\begin{align}
\qdd & = \bfM(\bfq)^{-1} (\bftau - \bftau_0) \\
\bftau_0 & = \mathrm{RNEA}(\bfq, \qd, \bfzero, \bff^\mathit{ext})
\end{align}<p>where the zero-acceleration torque <span class="math">\(\bftau_0\)</span> is computed using the
<a class="reference external" href="https://scaron.info/robot-locomotion/recursive-newton-euler-algorithm.html">recursive Newton-Euler algorithm</a>.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Both the articulated body algorithm (ABA) and composite rigid body algorithm
(CRBA) are described in Featherstone's <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid body dynamics algorithms</a>, a seminal book that has been
implemented in rigid body dynamics libraries, such as <a class="reference external" href="https://stack-of-tasks.github.io/pinocchio/">Pinocchio</a> or <a class="reference external" href="https://rbdl.github.io/">RBDL</a>, as well as dynamic simulators, such as <a class="reference external" href="https://pybullet.org/">Bullet</a> or <a class="reference external" href="https://dartsim.github.io/">Dart</a>. ABA is the
main algorithm in both Bullet and Dart.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>3D biped locomotion control including seamless transition between walking and running via 3D ZMP manipulation2021-05-31T00:00:00+02:002021-05-31T00:00:00+02:00Stéphane Carontag:scaron.info,2021-05-31:/publications/icra-2021.html<p class="authors"><strong>Tomomichi Sugihara</strong>, <strong>Kenta Imanishi</strong>, <strong>Takanobu Yamamoto</strong> and
<strong>Stéphane Caron</strong>. IEEE International Conference on Robotics and Automation,
May 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>A novel control scheme for biped robots to manipulate the ZMP
three-dimensionally apart from the actual ground profile is presented. It is
shown that the linear inverted-pendulum-like dynamics with this scheme …</p></div><p class="authors"><strong>Tomomichi Sugihara</strong>, <strong>Kenta Imanishi</strong>, <strong>Takanobu Yamamoto</strong> and
<strong>Stéphane Caron</strong>. IEEE International Conference on Robotics and Automation,
May 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>A novel control scheme for biped robots to manipulate the ZMP
three-dimensionally apart from the actual ground profile is presented. It is
shown that the linear inverted-pendulum-like dynamics with this scheme can
represent a wider class of movements including variation of the body height.
Moreover, this can also represent the motion in aerial phase. Based on this,
the foot-guided controller proposed by the authors is enhanced to enable the
robots to locomote on highly uneven terrains and also to seamlessly transition
between walking and running without pre-planning the overall motion reference.
The controller guarantees the capturability at landing and defines the motion
by a time-variant state feedback, which is analytically derived from a model
predictive optimization. It is verified through some computer simulations.</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.1109/ICRA48506.2021.9561503">10.1109/ICRA48506.2021.9561503</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">sugihara2021icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{3D biped locomotion control including seamless transition between walking and running via 3D ZMP manipulation}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Sugihara, Tomomichi and Imanishi, Kenta and Yamamoto, Takanobu and Caron, St{\'e}phane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{6258--6263}</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">may</span><span class="p">,</span>
<span class="na">organization</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/ICRA48506.2021.9561503}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Kinematics jargon2021-05-01T00:00:00+02:002021-06-03T00:00:00+02:00Stéphane Carontag:scaron.info,2021-05-01:/robot-locomotion/kinematics-jargon.html<p>Like any field with a big body of knowledge, robotics has its own jargon that is ever evolving and takes practice to get used to. Also, some concepts are close to each other, yet different, and they tend to be more easily confused. The goal of this page is to …</p><p>Like any field with a big body of knowledge, robotics has its own jargon that is ever evolving and takes practice to get used to. Also, some concepts are close to each other, yet different, and they tend to be more easily confused. The goal of this page is to act like a cheat sheet, stressing the differences between concepts and giving you the pointers to dig deeper.</p>
<div class="section" id="lexicon">
<h2>Lexicon</h2>
<p>Let us first look at the most important concepts.</p>
<dl class="docutils">
<dt>Configuration:</dt>
<dd>A vector <span class="math">\(\bfq\)</span> of the <a class="reference external" href="https://en.wikipedia.org/wiki/Configuration_space_(physics)">configuration space</a>, that is, a
vector of generalized coordinates (a.k.a. joint coordinates) that
characterizes the positions of all points in the multi-body model of our
robot.</dd>
<dt>Orientation:</dt>
<dd>The angular coordinates of a rigid body in space. Not exactly the same
thing as a rotation, although there is an isomorphism between rotations and
orientations (similarly to the isomorphism between positions and
translations). Rotation matrices are one way to represent orientation,
others are listed below.</dd>
<dt>Pose:</dt>
<dd>Both position and orientation of a rigid body. Not exactly the same thing
as a transformation, although there is an isomorphism between poses and
their transformation group.</dd>
<dt>Position:</dt>
<dd>The linear coordinates <span class="math">\(\bfp \in \mathbb{R}^3\)</span> of a body in space.
Not exactly the same thing as a translation, although there is an
isomorphism between the Euclidean space <span class="math">\(\mathbb{R}^3\)</span> and its
<a class="reference external" href="https://en.wikipedia.org/wiki/Translation_(geometry)#As_a_group">translation group</a>.</dd>
<dt>Rotation:</dt>
<dd>A transformation <span class="math">\(R \in SO(3)\)</span> that acts vectors <span class="math">\(\bfe \in
\mathbb{R}^3\)</span> and preserves their lengths and the angles between them. The
rotation matrix <span class="math">\(\bfR_{AB}\)</span> represents equivalently the orientation
of frame <span class="math">\(B\)</span> in frame <span class="math">\(A\)</span> or the rotation <span class="math">\(R : {}_B \bfe
\mapsto {}_A \bfe\)</span>.</dd>
<dt>Translation:</dt>
<dd>A transformation <span class="math">\(t : \mathbb{R}^3 \to \mathbb{R}^3\)</span> that maps the
position <span class="math">\(\bfp\)</span> of a rigid body to a new position <span class="math">\(t(\bfp)\)</span>.</dd>
<dt>Transform:</dt>
<dd>Shorthand for a <em>Direct isometry</em> <span class="math">\(T \in SE(3)\)</span>. Due to the bijection
between <em>Poses</em> and <span class="math">\(SE(3)\)</span>, it is common to read "the transform of a
rigid body", although more precise expressions are "the pose of a rigid
body" and "the transform from frame A to frame B".</dd>
</dl>
<div class="section" id="full-package">
<h3>Full package</h3>
<p>Here comes a wider list of expressions you will find in the literature. Follow the links or scroll down for extra info on each:</p>
<dl class="docutils">
<dt>Affine transformation:</dt>
<dd>A geometric transformation <span class="math">\(A : \mathbb{R}^3 \to \mathbb{R}^3\)</span> that
preserves lines and parallelism. Direct isometries <span class="math">\(SE(3)\)</span> used to
represent the <em>Pose</em> of a rigid body are, in particular, affine
transformations. Affine transformations are conveniently represented by
<span class="math">\(4 \times 4\)</span> matrices of <em>Homogenous coordinates</em>.</dd>
<dt>Attitude:</dt>
<dd>Synonym for orientation, this word is more common in aeronautics such as in
<a class="reference external" href="https://en.wikipedia.org/wiki/Attitude_and_heading_reference_system">attitude and heading reference system</a>.</dd>
<dt>Body:</dt>
<dd>Shorthand for <em>Rigid body</em>.</dd>
<dt>Direct isometry:</dt>
<dd>A geometric transformation <span class="math">\(T \in SE(3)\)</span> that preserves distances and
handedness. Direct isometries form a mathematical group <span class="math">\(SE(3)\)</span>
called the special Euclidean group. They are the most commonly used
representation for the <em>Pose</em> of rigid bodies.</dd>
<dt>Displacement:</dt>
<dd>Synonym for <em>Direct isometry</em>.</dd>
<dt>Heading:</dt>
<dd>A vector that hints at the direction of motion, especially for systems such
as cars or humands that have a preferred direction of motion (forward). For
these two, a heading vector can be any vector in the intersection between
the <a class="reference external" href="https://en.wikipedia.org/wiki/Sagittal_plane">sagittal plane</a> and
<a class="reference external" href="https://en.wikipedia.org/wiki/Median_plane">median plane</a>.</dd>
<dt>Homogeneous coordinates:</dt>
<dd>Convenient way to <a class="reference external" href="https://en.wikipedia.org/wiki/Transformation_matrix#Affine_transformations">represent</a>
an <em>Affine transformation</em> as a <span class="math">\(4 \times 4\)</span> matrix.</dd>
<dt>Joint:</dt>
<dd>Connection between two rigid bodies that constrains their relative degree
of freedom. The most common in robotics is the <a class="reference external" href="https://en.wikipedia.org/wiki/Revolute_joint">revolute joint</a> actuated by a servomotor.</dd>
<dt>Joint coordinates:</dt>
<dd>Synonym for <em>Configuration</em>. Sometimes used to distinguish actuated
coordinates (<em>e.g.</em> a robot's joints) from unactuated degrees of freedom
(<em>e.g.</em> the floating base of a legged robot).</dd>
<dt>Link:</dt>
<dd>Rigid body of a robot, connected to at least one <em>Joint</em>.</dd>
<dt>Transformation:</dt>
<dd>In general, this expression may refer to any <a class="reference external" href="https://en.wikipedia.org/wiki/Geometric_transformation">geometric transformation</a>, for example an
<em>Affine transformation</em>. In robotics, this expression is generally used as
a shorthand for a <em>Direct isometry</em> <span class="math">\(T \in SE(3)\)</span>.</dd>
</dl>
</div>
</div>
<div class="section" id="representing-orientation">
<h2>Representing orientation</h2>
<p>Because all points on a rigid body hold, by definition, the same relative positions between each other, we can describe them all with just six numbers: the <em>position</em> of any one of them, and the three degrees of freedom of the body's <em>orientation</em>. There are several ways to represent orientations. Three common ones are:</p>
<ul class="simple">
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Euler_angles">Euler angles</a>: only three angles, but tricky to work with because of the <a class="reference external" href="https://en.wikipedia.org/wiki/Gimbal_lock">gimbal lock</a> and the many different conventions (intrinsic or extrinsic rotations? upward or downward z-axis?)</li>
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Versor">Unit quaternions</a>: four numbers, better behaved than Euler angles with only one convention to figure out (are the coordinates ordered as <span class="math">\([w\ x\ y\ z]\)</span> or <span class="math">\([x\ y\ z\ w]\)</span>?).</li>
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Rotation_matrix">Rotation matrices</a>: nine numbers, no confusion regarding conventions, applies to position coordinates by direct matrix product.</li>
</ul>
<p>It is best practice to <em>avoid</em> using Euler angles in software (colorful experimental and bug-hunting stories abound). In legged robotics, the most common convention is roll-pitch-yaw convention with upward z-axis. It corresponds to intrinsic Z-Y-X angles, or equivalently extrinsic x-y-z angles: first roll around the fixed x-axis of the parent frame, then pitch around the fixed y-axis of the parent frame, finally yaw around the fixed z-axis of the parent frame.</p>
<p>To go further, check out <em>e.g.</em> <a class="reference external" href="https://www.astro.rug.nl/software/kapteyn-beta/_downloads/attitude.pdf">Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors</a> by James Diebel.</p>
</div>
<div class="section" id="representing-transformations">
<h2>Representing transformations</h2>
<p>The minimum number of coordinates necessary to describe the displacement of a rigid body is six, yet 6D displacement vectors include a flavor of Euler angles to represent orientations and the slew of problems they carry along.</p>
<ul class="simple">
<li>Pose vectors: 7D vectors that combine a quaternion with a position vector. This is for instance the representation used in <a class="reference external" href="https://github.com/rdiankov/openrave">OpenRAVE</a>.</li>
<li><a class="reference external" href="https://en.wikipedia.org/wiki/Transformation_matrix">Transformation matrices</a>: <span class="math">\(4 \times 4\)</span> matrices that combine a rotation matrix with a position vector.</li>
<li>Plücker matrices: <span class="math">\(6 \times 6\)</span> that act on the <a class="reference external" href="https://scaron.info/robot-locomotion/screw-theory.html">screws</a> that describe the motions and forces of rigid bodies. This is for instance the representation used in <a class="reference external" href="https://jrl-umi3218.github.io/mc_rtc/">mc_rtc</a>.</li>
</ul>
<p>To go further, check out <em>e.g.</em> <a class="reference external" href="https://doi.org/10.1007/978-1-4899-7560-7">Rigid Body Dynamics Algorithms</a> by Roy Featherstone.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Humanoid Control Under Interchangeable Fixed and Sliding Unilateral Contacts2021-04-08T00:00:00+02:002021-04-08T00:00:00+02:00Stéphane Carontag:scaron.info,2021-04-08:/publications/ral-2021.html<p class="authors"><strong>Saeid Samadi</strong>, <strong>Julien Roux</strong>, <strong>Arnaud Tanguy</strong>, <strong>Stéphane Caron</strong> and
<strong>Abderrahmane Kheddar</strong>. Robotics and Automation Letters. April 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this letter, we propose a whole-body control strategy for humanoid robots in
multi-contact settings that enables switching between fixed and sliding
contacts under active balance. We compute, in real-time, a safe …</p></div><p class="authors"><strong>Saeid Samadi</strong>, <strong>Julien Roux</strong>, <strong>Arnaud Tanguy</strong>, <strong>Stéphane Caron</strong> and
<strong>Abderrahmane Kheddar</strong>. Robotics and Automation Letters. April 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this letter, we propose a whole-body control strategy for humanoid robots in
multi-contact settings that enables switching between fixed and sliding
contacts under active balance. We compute, in real-time, a safe center-of-mass
position and wrench distribution of the contact points based on the Chebyshev
center. Our solution is formulated as a quadratic programming problem without a
priori computation of balance regions. We assess our approach with experiments
highlighting switches between fixed and sliding contact modes in multi-contact
configurations. A humanoid robot demonstrates such contact interchanges from
fully-fixed to multi-sliding and also shuffling of the foot. The scenarios
illustrate the performance of our control scheme in achieving the desired
forces, CoM position attractor, and planned trajectories while actively
maintaining balance.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-03158364/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/LRA.2021.3066965">10.1109/LRA.2021.3066965</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/SaeidSamadi/Multi-sliding-Contacts">Source code of the controller</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">samadi2021ral</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Samadi, Saeid and Roux, Julien and Tanguy, Arnaud and Caron, Stéphane and Kheddar, Abderrahmane}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Robotics and Automation Letters}</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Humanoid Control Under Interchangeable Fixed and Sliding Unilateral Contacts}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2021}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">apr</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{6}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{2}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{4032-4039}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/LRA.2021.3066965}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Conversion from least squares to quadratic programming2020-11-08T00:00:00+01:002020-11-08T00:00:00+01:00Stéphane Carontag:scaron.info,2020-11-08:/blog/conversion-from-least-squares-to-quadratic-programming.html<p>Casting a linear least squares to a quadratic program is a common question for
newcomers, who see this operation routinely mentioned or taken for granted in
writing. Let us review the details of this derivation using linear algebra over
<a class="reference external" href="https://en.wikipedia.org/wiki/Normed_vector_space">normed vector spaces</a>.</p>
<div class="section" id="definitions">
<h2>Definitions</h2>
<div class="section" id="least-squares">
<h3>Least Squares</h3>
<p>A constrained weighted linear least …</p></div></div><p>Casting a linear least squares to a quadratic program is a common question for
newcomers, who see this operation routinely mentioned or taken for granted in
writing. Let us review the details of this derivation using linear algebra over
<a class="reference external" href="https://en.wikipedia.org/wiki/Normed_vector_space">normed vector spaces</a>.</p>
<div class="section" id="definitions">
<h2>Definitions</h2>
<div class="section" id="least-squares">
<h3>Least Squares</h3>
<p>A constrained weighted linear least squares (LS) problem is written as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\underset{x \in \mathbb{R}^n}{\mathrm{minimize}} & \frac{1}{2} \| R x - s \|^2_W = \frac{1}{2} (R x - s)^T W (R x - s) \\
\mathrm{subject\ to} & G x \leq h \\ & A x = b
\end{array}
\end{equation*}
</div>
<p>This problem seeks the vector <span class="math">\(x\)</span> of optimization variables such that
<span class="math">\(R x\)</span> is as "close" as possible to a prescribed vector <span class="math">\(s\)</span>,
meanwhile satisfying a set of linear inequality and equality constraints
defined by the matrix-vector couples <span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span>,
respectively. Vector inequalities apply coordinate by coordinate. How close the
two vectors <span class="math">\(R x\)</span> and <span class="math">\(s\)</span> are is measured via the weighted norm
<span class="math">\(\| y \|_W = \sqrt{y^T W y}\)</span>, where the weight matrix <span class="math">\(W\)</span> is
<a class="reference external" href="https://en.wikipedia.org/wiki/Definite_symmetric_matrix">positive semi-definite</a> and usually
diagonal. The overall function <span class="math">\(x \mapsto (1/2) \| R x - s \|^2_W\)</span> is
called the <em>objective function</em> (<em>a.k.a.</em> cost function) of the problem.</p>
</div>
<div class="section" id="quadratic-programming">
<h3>Quadratic Programming</h3>
<p>A quadratic program (QP) is written in standard form as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\underset{x \in \mathbb{R}^n}{\mathrm{minimize}} & \frac12 x^T P x + q^T x \\
\mathrm{subject\ to} & G x \leq h \\ & A x = b
\end{array}
\end{equation*}
</div>
<p>This problem seeks the vector <span class="math">\(x\)</span> of optimization variables such that the
quadratic objective function defined by the (positive semi-definite) matrix
<span class="math">\(P\)</span> and vector <span class="math">\(q\)</span> is minimized, meanwhile satisfying a set of
linear inequality and equality constraints defined by the matrix-vector couples
<span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span>, respectively. Vector inequalities apply
coordinate by coordinate.</p>
</div>
<div class="section" id="equivalence-between-optimization-problems">
<h3>Equivalence between optimization problems</h3>
<p>Let us define formally what it means to "cast" a least squares to a quadratic
program. Since they have the same inequality and equality constraints, the only
difference between them lies in their objective functions. We will say that two
objective functions <span class="math">\(f(x)\)</span> and <span class="math">\(g(x)\)</span> are <em>equivalent</em>, written as
<span class="math">\(f(x) \propto g(x)\)</span>, if they have the same optimum:</p>
<div class="math">
\begin{equation*}
f(x) \propto g(x)
\ \Leftrightarrow \ \underset{x \in \mathbb{R}^n}{\mathrm{argmin}}
\left\{ f(x) | G x \leq h, A x = b \right\}
=
\underset{x \in \mathbb{R}^n}{\mathrm{argmin}}
\left\{ g(x) | G x \leq h, A x = b \right\}
\end{equation*}
</div>
<p>Informally, the symbol <span class="math">\(\propto\)</span> thus means "does not change the optimum
of the optimization problem".</p>
</div>
</div>
<div class="section" id="conversion-to-a-quadratic-objective">
<h2>Conversion to a quadratic objective</h2>
<p>Our goal, starting from the LS objective function <span class="math">\(f(x) = (1/2) \| R x -
s \|^2_W\)</span>, is to find a QP objective function <span class="math">\(g(x) = (1/2) x^T P x + q^T
x\)</span> such that <span class="math">\(f(x) \propto g(x)\)</span>. The one-half multiplicative constant
being here by convention (and not affecting the optimum in any way), we can
forget it for now and expand the weighted norm as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\| R x - s \|_W^2
& = & (R x - s)^T W (R x - s) \\
& = & x^T R^T W R x - x^T R^T W s - s^T W R x + s^T W s \\
\end{array}
\end{equation*}
</div>
<p>We can discard the constant term <span class="math">\(s^T W s\)</span> as it does not depend on the
optimization vector <span class="math">\(x\)</span> and therefore doesn't affect the optimum of our
problem. We then have:</p>
<div class="math">
\begin{equation*}
\| R x - s \|_W^2 \propto x^T R^T W R x - x^T R^T W s - s^T W R x
\end{equation*}
</div>
<p>Next, recall that <span class="math">\(W\)</span> is positive semi-definite, hence in particular
symmetric: <span class="math">\(W^T = W\)</span>. The number <span class="math">\(s^T W R x\)</span> being real, it is
equal to its own transpose, that is:</p>
<div class="math">
\begin{equation*}
s^T W R x = (s^T W R x)^T = x^T R^T W^T s = x^T R^T W s
\end{equation*}
</div>
<p>Back to our rewriting of the objective function, we get:</p>
<div class="math">
\begin{equation*}
\| R x - s \|_W^2 \propto x^T (R^T W R) x - 2 (R^T W s)^T x
\end{equation*}
</div>
<p>Multiplying by the positive one-half constant yields:</p>
<div class="math">
\begin{equation*}
\frac12 \| R x - s \|_W^2 \propto \frac12 x^T P x + q^T x
\end{equation*}
</div>
<p>with <span class="math">\(P = R^T W R\)</span> and <span class="math">\(q = - R^T W s\)</span>.</p>
<div class="section" id="example-in-python">
<h3>Example in Python</h3>
<p>The <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a> Python module
for quadratic programming provides a <tt class="docutils literal">solve_ls</tt> function alongside its main
<tt class="docutils literal">solve_qp</tt> function. This function boils down to:</p>
<pre class="code python literal-block">
<span class="k">def</span> <span class="nf">solve_ls</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">s</span><span class="p">,</span> <span class="n">G</span><span class="p">,</span> <span class="n">h</span><span class="p">,</span> <span class="n">A</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">lb</span><span class="p">,</span> <span class="n">ub</span><span class="p">,</span> <span class="n">W</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="s1">'quadprog'</span><span class="p">):</span>
<span class="n">WR</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</span><span class="n">W</span><span class="p">,</span> <span class="n">R</span><span class="p">)</span>
<span class="n">P</span> <span class="o">=</span> <span class="n">dot</span><span class="p">(</span><span class="n">R</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">WR</span><span class="p">)</span>
<span class="n">q</span> <span class="o">=</span> <span class="o">-</span><span class="n">dot</span><span class="p">(</span><span class="n">s</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">WR</span><span class="p">)</span>
<span class="k">return</span> <span class="n">solve_qp</span><span class="p">(</span><span class="n">P</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">G</span><span class="p">,</span> <span class="n">h</span><span class="p">,</span> <span class="n">A</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">lb</span><span class="p">,</span> <span class="n">ub</span><span class="p">,</span> <span class="n">solver</span><span class="o">=</span><span class="n">solver</span><span class="p">)</span>
</pre>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>LS and QP are not exactly equivalent (thanks to <a class="reference external" href="https://sites.google.com/site/adrienescandehomepage/">Adrien Escande</a> for correcting this in
an earlier version of this post): LS can be cast to QP, but QP can only be cast
to LS when the vector <span class="math">\(q\)</span> is in the image of the matrix <span class="math">\(P\)</span>.</p>
<p>The post on <a class="reference external" href="https://scaron.info/blog/linear-least-squares-in-python.html">least squares</a>
gives more details on <em>e.g.</em> using the weight matrix to steer the optimum of
the problem, or switching to lexicographic optimization when LS is not
expressive enough. For more focus on implementation, you can also check out the
more practical blog post on <a class="reference external" href="https://scaron.info/blog/quadratic-programming-in-python.html">quadratic programming in Python</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Linear least squares in Python2020-11-02T00:00:00+01:002020-11-02T00:00:00+01:00Stéphane Carontag:scaron.info,2020-11-02:/blog/linear-least-squares-in-python.html<p>Least squares is a widely used class of convex optimization that can be applied
to solve a variety of problems, for instance: in statistics for curve fitting,
in machine learning to compute <a class="reference external" href="https://en.wikipedia.org/wiki/Support_vector_machine">support vector machines</a>, in robotics to solve
<a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse kinematics</a>,
etc. They are related to <a class="reference external" href="https://scaron.info/blog/quadratic-programming-in-python.html">quadratic programming</a> (QP), having …</p><p>Least squares is a widely used class of convex optimization that can be applied
to solve a variety of problems, for instance: in statistics for curve fitting,
in machine learning to compute <a class="reference external" href="https://en.wikipedia.org/wiki/Support_vector_machine">support vector machines</a>, in robotics to solve
<a class="reference external" href="https://scaron.info/robot-locomotion/inverse-kinematics.html">inverse kinematics</a>,
etc. They are related to <a class="reference external" href="https://scaron.info/blog/quadratic-programming-in-python.html">quadratic programming</a> (QP), having a slightly more
intuitive objective function, and the first step beyond linear programming (LP)
in convex optimization.</p>
<div class="section" id="standard-form">
<h2>Standard form</h2>
<p>A constrained weighted linear least squares (LS) problem is written as:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\underset{x \in \mathbb{R}^n}{\mathrm{minimize}} & \frac12 \| R x - s \|^2_W = \frac12 (R x - s)^T W (R x - s) \\
\mathrm{subject\ to} & G x \leq h \\ & A x = b
\end{array}
\end{equation*}
</div>
<p>This problem seeks the vector <span class="math">\(x\)</span> of optimization variables such that
<span class="math">\(R x\)</span> is as "close" as possible to a prescribed vector <span class="math">\(s\)</span>,
meanwhile satisfying a set of linear inequality and equality constraints
defined by the matrix-vector couples <span class="math">\((G, h)\)</span> and <span class="math">\((A, b)\)</span>,
respectively. Vector inequalities apply coordinate by coordinate. How close the
two vectors <span class="math">\(R x\)</span> and <span class="math">\(s\)</span> are is measured via the weighted norm
<span class="math">\(\| y \|_W = \sqrt{y^T W y}\)</span>, where the weight matrix <span class="math">\(W\)</span> is
<a class="reference external" href="https://en.wikipedia.org/wiki/Definite_symmetric_matrix">positive semi-definite</a> and usually
diagonal. The overall function <span class="math">\(x \mapsto (1/2) \| R x - s \|^2_W\)</span> is
called the <em>objective function</em> of the problem.</p>
</div>
<div class="section" id="graphical-interpretation">
<h2>Graphical interpretation</h2>
<p>Let us illustrate the intuition behind this in 2D:</p>
<img alt="LS is about finding the minimum of a weighted norm (circles) over a linear set (polygon)" class="center" src="https://scaron.info/figures/least-squares.png" />
<p>In the figure above, the level sets of the weighted norm are represented by
ellipses centered on the target <span class="math">\(s\)</span>, while the linear set of inequality
constraints corresponds to the orange polygon. (There is no equality constraint
in this example.) Since the target <span class="math">\(s\)</span> of the objective function is
outside of the polygon, the optimum <span class="math">\(x^*\)</span> of the LS lies at the boundary
of the linear set.</p>
<p>In this example, the matrix <span class="math">\(R\)</span> of the objective function is the
identity, <em>i.e.</em> <span class="math">\(x\)</span> tries to be as "close" as possible to <span class="math">\(s\)</span>, and
the norm to measure how close the two vectors are is defined by the weight
matrix <span class="math">\(W = \mathrm{diag}(w_1, w_2)\)</span>:</p>
<div class="math">
\begin{equation*}
\| x - s \|^2_W = w_1 (x_1 - s_1)^2 + w_2 (x_2 - s_2)^2
\end{equation*}
</div>
<p>The weights <span class="math">\(w_1\)</span> and <span class="math">\(w_2\)</span> shape the relative importance of
proximity along the first coordinate <span class="math">\((x_1 - s_1)^2\)</span> compared to
proximity along the second coordinate <span class="math">\((x_2 - s_2)^2\)</span>. Note that the
absolute value of these weights has no effect on the optimum of our problem:
the only thing that matters is their relative value <span class="math">\(w_1 / w_2\)</span>. The
higher this ration, the more the optimum will trade a larger <span class="math">\((x_2 -
s_2)^2\)</span> for a smaller <span class="math">\((x_1 - s_1)^2\)</span>.</p>
<p><em>Question</em>: in the figure above, what is the largest weight, <span class="math">\(w_1\)</span> or
<span class="math">\(w_2\)</span>?</p>
</div>
<div class="section" id="python-example">
<h2>Python example</h2>
<p>Suppose we have access to a <tt class="docutils literal">solve_ls</tt> function that solves least squares
problems in standard form. For instance, the <a class="reference external" href="https://github.com/stephane-caron/qpsolvers">qpsolvers</a> module provides one directly if
you are using Python. Let us then see how to solve the following LS:</p>
<div class="math">
\begin{align*}
\begin{array}{rl}
\mathrm{minimize} & \left\| \left[\begin{array}{ccc}
1 & 2 & 0 \\
-8 & 3 & 2 \\
0 & 1 & 1 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\
x_3\end{array}\right] - \left[\begin{array}{c} 3 \\ 2 \\
3\end{array}\right] \right\|^2 \\
\mathrm{subject\ to} & \left[\begin{array}{ccc}
1 & 2 & 1 \\
2 & 0 & 1 \\
-1 & 2 & -1 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\
x_3\end{array}\right] \leq \left[\begin{array}{c}
3 \\ 2 \\ -2 \end{array} \right]
\end{array}
\end{align*}
</div>
<p>First, we write our LS matrices in proper format:</p>
<pre class="code python literal-block">
<span class="n">R</span> <span class="o">=</span> <span class="n">array</span><span class="p">([[</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">],</span> <span class="p">[</span><span class="o">-</span><span class="mf">8.</span><span class="p">,</span> <span class="mf">3.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">]])</span>
<span class="n">s</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">3.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="mf">3.</span><span class="p">])</span>
<span class="n">G</span> <span class="o">=</span> <span class="n">array</span><span class="p">([[</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">],</span> <span class="p">[</span><span class="mf">2.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">],</span> <span class="p">[</span><span class="o">-</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="o">-</span><span class="mf">1.</span><span class="p">]])</span>
<span class="n">h</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">3.</span><span class="p">,</span> <span class="mf">2.</span><span class="p">,</span> <span class="o">-</span><span class="mf">2.</span><span class="p">])</span>
<span class="n">W</span> <span class="o">=</span> <span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
</pre>
<p>Finally, we compute the solution using the <tt class="docutils literal">solve_ls</tt> function:</p>
<pre class="code python literal-block">
<span class="n">In</span> <span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">solve_ls</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">s</span><span class="p">,</span> <span class="n">G</span><span class="p">,</span> <span class="n">h</span><span class="p">,</span> <span class="n">W</span><span class="o">=</span><span class="n">W</span><span class="p">)</span>
<span class="n">Out</span><span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">array</span><span class="p">([</span> <span class="mf">0.12997347</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.06498674</span><span class="p">,</span> <span class="mf">1.74005305</span><span class="p">])</span>
</pre>
</div>
<div class="section" id="problem-shaping">
<h2>Problem shaping</h2>
<p>In fields such as optimal control, a lot of human work lies in solving the
inverse problem: think of a desired behavior, formulate it as a numerical
optimization problem, run the system, realize that the optimal solution to the
problem is not what one wanted in the first place, update the problem
accordingly, iterate. This process challenges us to get better at problem
shaping: how to modify the problem to steer the behavior of its optimum towards
what we want?</p>
<div class="section" id="diagonal-weight-matrix">
<h3>Diagonal weight matrix</h3>
<p>The first trick in the book is to use a diagonal weight matrix, and play with
its weight to adjust the relative importance of one sub-objective, that is, one
term <span class="math">\((R_i x - s_i)^2\)</span> of the objective function, compared to the others.
Consider a diagonal weight matrix:</p>
<div class="math">
\begin{equation*}
W = \mathrm{diag}(w_1, \dots, w_k) =
\begin{bmatrix}
w_1 & 0 & \cdots & 0 \\
0 & w_2 & \ddots & \vdots \\
\vdots & \ddots & \ddots & 0 \\
0 & \cdots & 0 & w_k
\end{bmatrix}
\end{equation*}
</div>
<p>This weighting produces the objective function:</p>
<div class="math">
\begin{equation*}
\| R x - s \|^2_W = \sum_{i=1}^k w_i (R_i x - s_i)^2
\end{equation*}
</div>
<p>If the behavior of the solution is too poor on one sub-objective <span class="math">\(i\)</span>
among our <span class="math">\(k\)</span> sub-objectives, meaning <span class="math">\((R_i x^* - s_i)^2\)</span> is larger
than what we want at the optimum <span class="math">\(x^*\)</span>, we can improve it by increasing
<span class="math">\(w_i\)</span>. However, this will come at the price of decreasing the performance
on some other sub-objectives <span class="math">\(j \neq i\)</span>.</p>
<p>To explain this informally, let us get rid of the scale invariance of these
weights. The facts that all weights are positive makes the expression <span class="math">\(\|
R x - s \|^2_W\)</span> a <a class="reference external" href="https://en.wikipedia.org/wiki/Conical_combination">conical combination</a> of squared residuals
<span class="math">\((R_i x - s_i)^2\)</span>. We can account for this invariance by choosing weights
as a <a class="reference external" href="https://en.wikipedia.org/wiki/Convex_combination">convex combination</a>:</p>
<div class="math">
\begin{equation*}
\sum_{i=1}^k w_i = 1
\end{equation*}
</div>
<p>Given any weighted LS problem, we can always renormalize the weights (dividing
them by their collective sum) to get to an equivalent problem where weights
form a convex combination. We can then focus without loss of generality on
problems whose weights form a convex combination.</p>
<p>Intuitively, increasing a weight <span class="math">\(w_i\)</span> results in improved performance of
the corresponding objective, that is, a decrease in its corresponding squared
residual <span class="math">\((R_i x^* - s_i)^2\)</span> at the optimum. But now, the sum of weights
being equal to 1, we see how an increase in <span class="math">\(w_i\)</span> must be balanced by
decreasing other weights <span class="math">\(w_j\)</span> <span class="math">\((j \neq i)\)</span>, and thus lower the
performance on other sub-objectives. Overall, there is no free lunch: when it
is not possible to solve all sub-objectives perfectly (either because they
contradict each other or because of inequality and equality constraints), the
next best thing we can do is trade some sub-objectives for others.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Least squares is covered in Chapter 10 of Nocedal and Wright's <a class="reference external" href="https://doi.org/10.1007/978-0-387-40065-5">Numerical
Optimization</a>, as well as in
Section 1.2 of Boyd and Vandenberghe's <a class="reference external" href="https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf">Convex Optimization</a>. It is closely
related to quadratic programming, and we can <a class="reference external" href="https://scaron.info/blog/conversion-from-least-squares-to-quadratic-programming.html">cast LS problems to QP</a> in
order to use readily available QP solvers.</p>
<p>Using relative weights in a linearly constrained optimization is a particular
instance of <a class="reference external" href="https://hal.archives-ouvertes.fr/file/index/docid/751924/filename/m.pdf">lexicographic optimization</a>
with two levels of priority. Thinking in terms of levels of priority sheds a
new light on problem shaping. For instance, when one weight gets predominant
over the others, our LS problem becomes (increasingly difficult to solve and)
closer to a three-level optimization. In that case, we can get better
performance by using a proper lexicographic solver. See Section 2 of <a class="reference external" href="https://hal.inria.fr/hal-01418462/document">Geometric
and numerical aspects of redundancy</a> for a concrete discussion on
this.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>ZMPの3次元的操作による可捕性規範凹凸地面上二脚運動制御2020-10-10T00:00:00+02:002020-10-10T00:00:00+02:00Stéphane Carontag:scaron.info,2020-10-10:/publications/rsj-2020.html<p class="authors"><strong>Tomomichi Sugihara</strong> and <strong>Stéphane Caron</strong>. RSJ 2020, Japan, October 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>二脚ロボットを実用する上で，高低差のある地形を移動する機能は必須である．限られた足場で着地点を選び，転倒を避けながら重心を制御する方法が求められる．梶田ら [1] はその先駆的な研究において，凹凸地形に合わせて上下動を伴う重心の運動と支持足の切り替えを適切に連動させる歩行制御法を提案した．その際，重心運動を直線上に拘束することで導かれた線形な倒立振子様の力学系を用いて，議論を簡明にした．</p>
<p>(There are no abstract in RSJ papers, this is actually the first paragraph of the introduction.)</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/papers/conf/sugihara-rsj-2020.pdf">Short paper</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">sugihara2020rsj</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{ZMPの3次元的操作による可捕性規範凹凸地面上二脚運動制御}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Sugihara, Tomomichi and …</span></pre></div></div><p class="authors"><strong>Tomomichi Sugihara</strong> and <strong>Stéphane Caron</strong>. RSJ 2020, Japan, October 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>二脚ロボットを実用する上で，高低差のある地形を移動する機能は必須である．限られた足場で着地点を選び，転倒を避けながら重心を制御する方法が求められる．梶田ら [1] はその先駆的な研究において，凹凸地形に合わせて上下動を伴う重心の運動と支持足の切り替えを適切に連動させる歩行制御法を提案した．その際，重心運動を直線上に拘束することで導かれた線形な倒立振子様の力学系を用いて，議論を簡明にした．</p>
<p>(There are no abstract in RSJ papers, this is actually the first paragraph of the introduction.)</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/papers/conf/sugihara-rsj-2020.pdf">Short paper</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">sugihara2020rsj</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{ZMPの3次元的操作による可捕性規範凹凸地面上二脚運動制御}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Sugihara, Tomomichi and Caron, St{\'e}phane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{日本ロボット学会学術講演会予稿集}</span><span class="p">,</span>
<span class="na">volume</span><span class="p">=</span><span class="s">{38}</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">oct</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Position and coordinate systems2020-09-27T00:00:00+02:002020-09-27T00:00:00+02:00Stéphane Carontag:scaron.info,2020-09-27:/robot-locomotion/position-and-coordinate-systems.html<p>The simplest object we can study in kinematics is the <em>point</em>, also known as
<em>particle</em> in classical mechanics. We can describe a point <span class="math">\(A\)</span> in the
Euclidean space by providing a vector <span class="math">\({}^\calB \bfp_A \in \mathbb{R}^3\)</span>
of <em>coordinates</em> in the reference frame <span class="math">\(\calB = (B, {}^\calB \bfe_x,
{}^\calB \bfe_y …</span></p><p>The simplest object we can study in kinematics is the <em>point</em>, also known as
<em>particle</em> in classical mechanics. We can describe a point <span class="math">\(A\)</span> in the
Euclidean space by providing a vector <span class="math">\({}^\calB \bfp_A \in \mathbb{R}^3\)</span>
of <em>coordinates</em> in the reference frame <span class="math">\(\calB = (B, {}^\calB \bfe_x,
{}^\calB \bfe_y, {}^\calB \bfe_z)\)</span>.</p>
<div class="section" id="cartesian-coordinates">
<h2>Cartesian coordinates</h2>
<p>The three vectors <span class="math">\(({}^\calB \bfe_x, {}^\calB \bfe_y, {}^\calB \bfe_z)\)</span>
form an <a class="reference external" href="https://en.wikipedia.org/wiki/Orthonormal_basis">orthonormal basis</a>
of <span class="math">\(\mathbb{R}^3\)</span>. We can use it to express the <em>Cartesian coordinates</em>
of <span class="math">\(A\)</span>, which consist of:</p>
<ul class="simple">
<li>The signed distance <span class="math">\(x\)</span> along the x-axis.</li>
<li>The signed distance <span class="math">\(y\)</span> along the y-axis.</li>
<li>The signed distance <span class="math">\(z\)</span> along the z-axis.</li>
</ul>
<p>Mathematically:</p>
<div class="math">
\begin{equation*}
{}^\calB \bfp_A = \begin{bmatrix} x \\ y \\ z \end{bmatrix} = x\,{}^\calB \bfe_x + y\,{}^\calB \bfe_y + z\,{}^\calB \bfe_z
\end{equation*}
</div>
<p>Graphically:</p>
<img alt="Cartesian coordinates in the Euclidean space" class="align-center max-height-200px" src="https://scaron.info/figures/cartesian-coordinates.png" />
<p>On a side note, the figure above introduces a color convention from computer
graphics where the x-axis (sagittal) is red, the y-axis (lateral) green and the
z-axis (vertical) blue. This convention is common in off-the-shelf robotics
software such as <a class="reference external" href="https://wiki.ros.org/rviz">RViz</a> and <a class="reference external" href="https://github.com/rdiankov/openrave">OpenRAVE</a>.</p>
</div>
<div class="section" id="cylindrical-coordinates">
<h2>Cylindrical coordinates</h2>
<p>Cylindrical coordinates represent the position of a point relative to an axis
of interest, typically the vertical z-axis, and a direction of interest,
typically the sagittal x-axis. They consist of three numbers:</p>
<ul class="simple">
<li>The distance <span class="math">\(\rho\)</span> from <span class="math">\(A\)</span> to the z-axis.</li>
<li>The angle <span class="math">\(\varphi\)</span>, called <em>azimuthal angle</em> (or just <em>the azimuth</em>, which sounds way cooler), between the vector from <span class="math">\(B\)</span> to <span class="math">\(A\)</span> and <span class="math">\(\bfe_x\)</span>.</li>
<li>The axis coordinate <span class="math">\(z\)</span>.</li>
</ul>
<p>Mathematically, we can map the vector <span class="math">\({}^\cal B\bfchi_A\)</span> of cylindrical coordinates to Cartesian ones by:</p>
<div class="math">
\begin{equation*}
{}^\calB \bfchi_A = \begin{bmatrix} \rho \\ \varphi \\ z \end{bmatrix} \ \Longrightarrow
\ {}^\calB \bfp_A({}^\calB \bfchi_A) = \begin{bmatrix} \rho \cos \varphi \\ \rho \sin \varphi \\ z \end{bmatrix}
\end{equation*}
</div>
<p>Graphically:</p>
<img alt="Cylindrical coordinates in the Euclidean space" class="align-center max-height-200px" src="https://scaron.info/figures/cylindrical-coordinates.png" />
<p>These coordinates are sometimes useful to analyze quantities that act on a body
frame of a car or mobile robot. For example, when <a class="reference external" href="https://doi.org/10.1109/AMC.2012.6197026">an external disturbance is
applied to a humanoid robot</a>,
looking at the direction and amplitude of the disturbance in the horizontal
plane can be more insightful than looking at individual x- and y-coordinates.</p>
</div>
<div class="section" id="spherical-coordinates">
<h2>Spherical coordinates</h2>
<p>Spherical coordinates represent the position of a point by:</p>
<ul class="simple">
<li>Its distance <span class="math">\(r\)</span> to the origin <span class="math">\(B\)</span> of the reference frame.</li>
<li>The angle <span class="math">\(\varphi\)</span>, the azimuth™, identical to the one used in cylindrical coordinates.</li>
<li>The angle <span class="math">\(\theta\)</span>, called <em>polar</em>, corresponding to the amount of rotation to apply in the resulting plane to go from the "pole" to the desired point.</li>
</ul>
<p>Mathematically, we can map the vector <span class="math">\({}^\cal B\bfsigma_A\)</span> of spherical coordinates to Cartesian ones by:</p>
<div class="math">
\begin{equation*}
{}^\calB \bfsigma_A = \begin{bmatrix} r \\ \varphi \\ \theta \end{bmatrix} \ \Longrightarrow
\ {}^\calB \bfp_A({}^\calB \bfsigma_A) = \begin{bmatrix} r \cos \varphi \sin \theta \\ r \sin \varphi \sin \theta \\ r \cos \theta \end{bmatrix}
\end{equation*}
</div>
<p>Graphically:</p>
<img alt="Spherical coordinates in the Euclidean space" class="align-center max-height-200px" src="https://scaron.info/figures/spherical-coordinates.png" />
</div>
<div class="section" id="observations">
<h2>Observations</h2>
<div class="section" id="difference-between-the-object-and-its-coordinates">
<h3>Difference between the object and its coordinates</h3>
<p>These three coordinate systems can be used equivalently to represent the
position of our point <span class="math">\(A\)</span> in the reference frame <span class="math">\(\calB\)</span>. Note how
we have made, and will consistently make, the distinction between the point
itself <span class="math">\(A\)</span> and the vector <span class="math">\({}^\calB \bfp_A\)</span> (of coordinates (with
respect to a given reference frame)) used to represent this point
mathematically. The same point can for instance be written <span class="math">\({}^\calC p_A\)</span>
in another reference frame <span class="math">\(\calC\)</span> translated with respect to
<span class="math">\(\calB\)</span>. This mental gymnastics of keeping track of both the object and
its representation will become all the more important as we move on to linear
velocities, angular velocities, <a class="reference external" href="https://scaron.info/robot-locomotion/screw-theory.html">twists</a>, etc.</p>
</div>
<div class="section" id="omitting-the-reference-frame">
<h3>Omitting the reference frame</h3>
<p>When there is no ambiguity, the reference frame exponent is often dropped and
the position of point <span class="math">\(A\)</span> is abbreviated as <span class="math">\(\bfp_A\)</span>. Yet, physical
quantities are always expressed with respect to a <a class="reference external" href="https://en.wikipedia.org/wiki/Frame_of_reference">reference frame</a>, so implicitly there is
always an underlying frame to any vector <span class="math">\(\bfp_A\)</span> of position
coordinates.</p>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>ZMP support area2020-07-05T00:00:00+02:002021-10-16T00:00:00+02:00Stéphane Carontag:scaron.info,2020-07-05:/robot-locomotion/zmp-support-area.html<p>When a legged robot walks over a regular terrain, we can simplify its dynamics
to a reduced model like the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> where the center of mass (CoM)
is controlled via the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point</a> (ZMP) of contact forces with the
ground. To be physically feasible, the ZMP must …</p><p>When a legged robot walks over a regular terrain, we can simplify its dynamics
to a reduced model like the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum</a> where the center of mass (CoM)
is controlled via the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point</a> (ZMP) of contact forces with the
ground. To be physically feasible, the ZMP must lie in a <em>support area</em>, also
known as <em>support polygon</em>, which is roughly "the area between the feet" in
simple cases. In more complex cases, such as a hand on a wall or a knee on the
ground, the area is a bit more complex but it is still computable.</p>
<div class="section" id="modeling">
<h2>Modeling</h2>
<p>The <a class="reference external" href="https://scaron.info/robot-locomotion/constrained-equations-of-motion.html">constrained equations of motion</a> describe the dynamics of our
articulated system without simplification:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = & \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ(\bfq)^\top \bff \\
\bfF(\bfq) \bff & \leq & \bfzero
\end{array}
\end{equation*}
</div>
<p>The configuration <span class="math">\(\bfq\)</span> consists of its <span class="math">\(n\)</span> actuated degrees of
freedom and 6 unactuated floating base coordinates. Contact forces from the
environment are stacked in the vector <span class="math">\(\bff\)</span> and constrained by the
matrix <span class="math">\(\bfF(\bfq)\)</span> of <a class="reference external" href="https://scaron.info/robot-locomotion/friction-cones.html">Coulomb friction cones</a>. The interest in switching to a simplified
model like the linear inverted pendulum (LIP) is to reduce the dimension of
this equation. The constrained equations of motion for the LIP are line-by-line
similar to those of the whole-body:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfpdd_G & = & \omega^2 (\bfp_G - \bfp_Z) \\
\bfA(\bfp_G) \bfp_Z & \leq & \bfb(\bfp_G)
\end{array}
\end{equation*}
</div>
<p>The configuration <span class="math">\(\bfp_G\)</span> consists of the the 2D horizontal position of
the center of mass. Contact forces from the environment are aggregated in the
position <span class="math">\(\bfp_Z\)</span> of the ZMP and constrained by the second line of
<a class="reference external" href="https://scaron.info/blog/polyhedra-and-polytopes.html#halfspace-representation">half-space inequalities</a>. The
matrix <span class="math">\(\bfA(\bfp_G)\)</span> and vector <span class="math">\(\bfb(\bfp_G)\)</span> define the support
area, which is a polygon as soon as we take into account actuation limits of
the robot. They can be computed from the contact geometry and friction
information encoded in <span class="math">\(\bfJ(\bfq)\)</span> and <span class="math">\(\bfF(\bfq)\)</span> by <a class="reference external" href="https://scaron.info/blog/projecting-polytopes.html">polyhedral
projection</a>. Further technical
details on how to do this are given in <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02108589/document">Section IV of this paper</a>.</p>
</div>
<div class="section" id="simplification-on-flat-floors">
<h2>Simplification on flat floors</h2>
<p>In pre-2010 robotics papers, we often see the ZMP support area defined as the
"convex hull of ground contact points", where <span class="math">\(\bfA\)</span> and <span class="math">\(\bfb\)</span> do
not depend on <span class="math">\(\bfp_G\)</span> any more and can be readily computed from ground
contact points by a convex hull algorithm (a particular case of polyhedral
projection). This simplification is only valid if we make two assumptions:</p>
<ul class="simple">
<li>All ground contact points are coplanar.</li>
<li>The coefficient of friction between robot feet and the ground is infinite.</li>
</ul>
<p>If we follow the general algorithm from the previous section, which takes the
CoM position <span class="math">\(\bfp_G\)</span> as an input, it is not clear how the ZMP support
area becomes independent from <span class="math">\(\bfp_G\)</span> under these two conditions. Let us
detail this step by step.</p>
<div class="section" id="infinite-friction">
<h3>Infinite friction</h3>
<p>Let us consider for simplicity of calculations that our flat ground contact
plane is horizontal (othogonal to gravity). In the absence of friction
constaints, our friction cone inequalities are reduced to contact unilaterality
constraints:</p>
<div class="math">
\begin{equation*}
\forall \textrm{ contact } i, \ f_i^z > 0
\end{equation*}
</div>
<p>This means we can exert arbitrary horizontal forces <span class="math">\(f_i^x\)</span> and
<span class="math">\(f_i^y\)</span> at every contact point <span class="math">\(i\)</span>.</p>
</div>
<div class="section" id="newton-euler-equations">
<h3>Newton-Euler equations</h3>
<p>Our contact forces are bound to the CoM and ZMP by <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton and Euler equations</a>. First, Newton equation gives us:</p>
<div class="math">
\begin{equation*}
\sum_{\textrm{contact } i} \bff_i = \bff = \frac{m g}{h} (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>with <span class="math">\(m\)</span> the total mass, <span class="math">\(g\)</span> the gravity constant and <span class="math">\(h\)</span> the
CoM height above ground. Second, Euler equation (with no angular momentum at
the center of mass since we are in a pendular model) gives us:</p>
<div class="math">
\begin{equation*}
\sum_{\textrm{ contact } i} \bfp_i \times \bff_i = \bfp_G \times \bff
\end{equation*}
</div>
<p>Let's unwrap it to understand better what it says. On flat floor, we have
<span class="math">\(z_i = 0\)</span> for all contact points <span class="math">\(i\)</span>, so that the vector cross
products expand to:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\sum_{i} y_i f_i^z & = & y_G f^z - h f^y \\
\sum_{i} -x_i f_i^z & = & -x_G f^z + h f^x \\
\sum_{i} (x_i f_i^y - y_i f_i^x) & = & x_G f^y - y_G f^x
\end{array}
\end{equation*}
</div>
<p>Since we are not limited by friction, the third equation always has a
solution and we can trim it out. In the other two equations, we can replace
<span class="math">\(\bff\)</span> by its expression <span class="math">\(mg (\bfp_G - p_Z) / h\)</span> from the linear
inverted pendulum model, for instance:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
y_G f^z & = & y_G m g (h - 0) / h = y_G m g \\
h f^y & = & m g (y_G - y_Z) \\
y_G f^z - h f^y & = & m g y_Z \\
mg y_Z & = & \sum_i {f_i^z} y_i
\end{array}
\end{equation*}
</div>
<p>and similarly for the other equation. Defining <span class="math">\(\alpha_i =
\frac{f_i^z}{mg}\)</span>, we get:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfp_Z & = & \sum_{i} \alpha_i \bfp_i \\
\forall i, \alpha_i & \geq & 0
\end{array}
\end{equation*}
</div>
<p>which is the <a class="reference external" href="https://scaron.info/blog/polyhedra-and-polytopes.html#vertex-representation">vertex representation</a> of the
convex hull of ground contact points <span class="math">\(\bfp_i\)</span>.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>ZMP support areas can be derived in any <a class="reference external" href="https://scaron.info/publications/tro-2016.html">multi-contact configurations</a>, but in this case they depend
(nonlinearly) on the position of the center of mass. Simplified ZMP support
areas can be applied for walking over both horizontal and inclined. For
instance, they have been used by <a class="reference external" href="https://scaron.info/publications/icra-2019.html">biped robots to climb stairs</a> and <a class="reference external" href="https://doi.org/10.3929/ethz-b-000221541">quadruped robots to trot over
inclined surfaces</a>. Joint torque
limits can also be included to compute <a class="reference external" href="https://arxiv.org/pdf/1903.07999.pdf">actuation-aware ZMP support polygons</a>.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model2020-05-31T00:00:00+02:002020-05-31T00:00:00+02:00Stéphane Carontag:scaron.info,2020-05-31:/publications/icra-2020.html<p class="authors"><strong>Stéphane Caron</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The variable-height inverted pendulum (VHIP) model enables a new balancing
strategy by height variations of the center of mass, in addition to the
well-known ankle strategy. We propose a biped stabilizer based on linear
feedback of the VHIP that is simple to implement …</p></div><p class="authors"><strong>Stéphane Caron</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The variable-height inverted pendulum (VHIP) model enables a new balancing
strategy by height variations of the center of mass, in addition to the
well-known ankle strategy. We propose a biped stabilizer based on linear
feedback of the VHIP that is simple to implement, coincides with the
state-of-the-art for small perturbations and is able to recover from larger
perturbations thanks to this new strategy. This solution is based on
"best-effort" pole placement of a 4D divergent component of motion for the VHIP
under input feasibility and state viability constraints. We complement it with
a suitable whole-body admittance control law and test the resulting stabilizer
on the HRP-<span class="math">\(4\)</span> humanoid robot.</p>
</div>
<div class="section" id="videos">
<h2>Videos</h2>
<div class="section" id="chatty-presentation">
<h3>Chatty Presentation</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/icra-2020.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="tl-dw">
<h3>TL;DW</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/vhip-stabilization.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02289919/document">Paper</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/icra-2020/slides.pdf">Slides</a></td>
</tr>
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://scaron.info/videos/icra-2020.mp4">Presentation given at ICRA 2020</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/talks/jrl-2019.html">Presentation given at JRL</a> on 29 October 2019</td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/vhip_walking_controller/">Source code of the controller</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA40945.2020.9196715">10.1109/ICRA40945.2020.9196715</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">caron2020icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-02289919}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA40945.2020.9196715}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Constrained equations of motion2020-04-12T00:00:00+02:002021-10-11T00:00:00+02:00Stéphane Carontag:scaron.info,2020-04-12:/robot-locomotion/constrained-equations-of-motion.html<p>Let us take a closer look at how <em>Jacobian-transpose</em> term appears in the
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> of legged robots in
contact with their environment:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c(\bfq)^\top \bff
\end{equation*}
</div>
<p>The Jacobian-transpose …</p><p>Let us take a closer look at how <em>Jacobian-transpose</em> term appears in the
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> of legged robots in
contact with their environment:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c(\bfq)^\top \bff
\end{equation*}
</div>
<p>The Jacobian-transpose term maps external contact forces to joint torques (for
actuated coordinates) and to the net floating-base wrench (for unactuated
coordinates). We will see how to derive it from the principle of least
constraint, using the method of Lagrange multipliers.</p>
<div class="section" id="constraint-derivatives">
<h2>Constraint derivatives</h2>
<img alt="Humanoid whole-body dynamics involve contact forces and the dynamic momentum" class="right" src="https://scaron.info/figures/humanoid-2d-eom.png" />
<p>We consider a legged robot, for example the humanoid depicted in the figure to
the right, making <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#point-contacts-and-contact-forces">point contacts</a> or
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#surface-contacts-and-contact-wrenches">surface contacts</a>
with its environment. These kinematic constraints can be summarized into a
<a class="reference external" href="https://en.wikipedia.org/wiki/Holonomic_constraints">holonomic constraint</a>
<span class="math">\(\bfc(\bfq) = \bfzero\)</span>. In the case of point contacts, it would be the
stack of contact point equality constraints <span class="math">\(\bfp_C(\bfq) = \bfp_{D}\)</span>,
with <span class="math">\(\bfp_C\)</span> the mobile point on the robot's foot and <span class="math">\(\bfp_D\)</span> the
ground contact point in the inertial frame. Differentiating <span class="math">\(\bfc(\bfq) =
\bfzero\)</span> twice with respect to time yields:</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}
\end{equation*}
</div>
<p>where the constraint Jacobian and Hessian matrices are defined by:</p>
\begin{align}
\bfJ_c(\bfq) & = \frac{\partial \bfc}{\partial \bfq}(\bfq) &
\bfH_c(\bfq) & = \frac{\partial^2 \bfc}{\partial \bfq^2}(\bfq)
\end{align}<p>The Jacobian is a <span class="math">\(3 k \times n\)</span> matrix, with <span class="math">\(k\)</span> the number of
contact poins and with <span class="math">\(n\)</span> the degree of freedom of our robot (number of
actuated joints plus six), while the Hessian is an <span class="math">\(n \times 3 k \times
n\)</span> tensor. Notations are the same for surface contacts, except that the degree
of constraint for each contact is six rather than three.</p>
</div>
<div class="section" id="principle-of-least-constraint">
<h2>Principle of least constraint</h2>
<p>Remember how, in the absence of constraint, the <em>equations of free motion</em> of
our system would be:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd_{\mathrm{free}} + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_{\mathit{ext}}
\end{equation*}
</div>
<p>where:</p>
<ul class="simple">
<li><span class="math">\(\bfM(\q)\)</span> is the <span class="math">\(n \times n\)</span> joint-space inertia matrix,</li>
<li><span class="math">\(\bfC(\q)\)</span> is the <span class="math">\(n \times n \times n\)</span> joint-space Coriolis tensor,</li>
<li><span class="math">\(\bftau\)</span> is the <span class="math">\(n\)</span>-dimensional vector of actuated joint torques,</li>
<li><span class="math">\(\bftau_g(\q)\)</span> is the <span class="math">\(n\)</span>-dimensional vector of gravity torques,</li>
<li><span class="math">\(\bftau_{\mathit{ext}}\)</span> is an optional vector of external forces and torques.</li>
</ul>
<p>We know how to <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#inertia-matrix-and-coriolis-tensor">compute these matrices</a> and
<a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#gravity-torques-and-external-forces">vectors</a> from
link inertias and Jacobians. However, when the robot is in contact, its motion
doesn't follow the vector <span class="math">\(\qdd_{\mathrm{free}}\)</span> defined by this equation
because of the additional contact constraint: the actual <span class="math">\(\qdd\)</span> has to be
such that</p>
<div class="math">
\begin{equation*}
\bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}
\end{equation*}
</div>
<p><a class="reference external" href="https://en.wikipedia.org/wiki/Gauss%27s_principle_of_least_constraint">Gauss's principle of least constraint</a>
states that the robot's actual acceleration <span class="math">\(\qdd\)</span> is <em>as close as
possible</em> to the free acceleration <span class="math">\(\qdd_{\mathrm{free}}\)</span>, subject to
this constraint:</p>
<div class="math">
\begin{equation*}
\begin{array}{rl}
\qdd = \arg\min_{\bfa} & \frac{1}{2} \| \bfa - \qdd_{\mathrm{free}}\|_{\bfM}^2 \\
\textrm{subject to} & \bfJ_c(\bfq) \bfa + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0}
\end{array}
\end{equation*}
</div>
<p>Note how the inertia matrix appears in the objective function to define what
<em>as close as possible</em> means here. The inertia matrix is positive definite so
this objective function is strictly convex.</p>
<div class="section" id="introducing-contact-forces">
<h3>Introducing contact forces</h3>
<p>We can write the Lagrangian of this equality-constrained least-squares problem
as:</p>
<div class="math">
\begin{equation*}
\calL(\bfa, \bflambda) = \frac{1}{2} (\bfa - \qdd_{\mathrm{free}})^\top \bfM (\bfa -
\qdd_{\mathrm{free}}) - \bflambda^\top \left(\bfJ_c \bfa + \qd^\top \bfH_c \qd\right)
\end{equation*}
</div>
<p>where we omit dependencies on <span class="math">\(\bfq\)</span> for concision. Dual variables
stacked in the vector <span class="math">\(\bflambda\)</span> correspond to contact forces. Why? By
definition! In <a class="reference external" href="https://en.wikipedia.org/wiki/Lagrangian_mechanics">Lagrangian mechanics</a>, there are two kinds of
forces: constraint and non-constraint forces. Non-constraint forces are somehow
easier to deal with: they correspond to the vectors <span class="math">\(\bftau_g(\bfq)\)</span> and
<span class="math">\(\bftau_{\mathit{ext}}\)</span> in the equations of free motion, and we already
know how to compute them. Contact forces are the constraint forces arising from
our kinematic contact constraint.</p>
<p>We can think of forces as dual variables that appear in the equations of motion
to ensure that the system's motion ends up satisfying the kinematic constraint.
Our assumption here is that the constraint will hold, <em>i.e.</em> that the robot
will stay in contact with its environment. In practice this is not true for any
command: if we send maximum joint torques, our robot will likely shake and jump
all over the place, breaking the assumption of fixed contact. To make sure that
this assumption holds, we will complement our kinematic constraint with a
dynamic <a class="reference external" href="https://scaron.info/robot-locomotion/contact-stability.html">contact stability</a> condition, but
this is another story. Let us get back to our derivation.</p>
</div>
<div class="section" id="method-of-lagrange-multipliers">
<h3>Method of Lagrange multipliers</h3>
<p>From the <a class="reference external" href="https://en.wikipedia.org/wiki/Lagrange_multiplier">method of Lagrange multipliers</a>, the optimum
<span class="math">\((\bfa^*, \bflambda^*)\)</span> of our constrained problem is a critical point of
the Lagrange function:</p>
<div class="math">
\begin{equation*}
\begin{array}{ccc}
\displaystyle \frac{\partial \calL}{\partial \bfa}(\bfa^*, \bflambda^*) = \bfzero
& \wedge &
\displaystyle \frac{\partial \calL}{\partial \bflambda}(\bfa^*, \bflambda^*) = \bfzero
\end{array}
\end{equation*}
</div>
<p>The second equation is, by construction, our contact constraint. The first one
gives us:</p>
<div class="math">
\begin{equation*}
\bfM (\bfa^* - \qdd_{\mathrm{free}}) - \bfJ_c^T \bflambda^* = \bfzero
\end{equation*}
</div>
<p><em>In fine</em>, replacing <span class="math">\(\bfa^*\)</span> by our notation for joint accelerations
<span class="math">\(\qdd\)</span> and similarly <span class="math">\(\bflambda^*\)</span> by <span class="math">\(\bff\)</span>, we obtain the
constrained equations of motion in joint space:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff
\end{equation*}
</div>
<p>with <span class="math">\(\bfM(\q)\)</span> the joint-space inertia matrix, <span class="math">\(\bfC(\q)\)</span> the
joint-space Coriolis tensor, <span class="math">\(\bftau_g(\q)\)</span> the vector of gravity
torques, <span class="math">\(\bftau\)</span> the vector of actuated joint torques, <span class="math">\(\bfJ_c\)</span> the
stacked Jacobian of kinematic contact constraints and <span class="math">\(\bff\)</span> the
corresponding stacked vector of contact forces.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>This page is based on Wieber's <a class="reference external" href="https://hal.inria.fr/inria-00390428/document">comments on the structure of the dynamics of
articulated motion</a>, as well as
on a very good summary in <a class="reference external" href="https://arxiv.org/pdf/1904.05072.pdf">Section IV.A of this paper</a> by Budhiraja <em>et al</em>.</p>
<p>Forces are virtual quantities that balance out our calculations. Defining them
as the dual multipliers of motion constraints is a key step in Lagrangian
mechanics: constraints "pull" on the system, which aspires to be somewhere else
according to the principle of least motion, and multipliers can be seen as a
measure of how hard the constraints need to pull to keep the system on track.
Check out Jensen's explanation on <a class="reference external" href="http://www.slimy.com/~steuard/teaching/tutorials/Lagrange.html#Meaning">the meaning of the multiplier</a> for
some great examples to build up intuition around this.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Feasible Region: an Actuation-Aware Extension of the Support Region2020-02-22T00:00:00+01:002020-02-22T00:00:00+01:00Stéphane Carontag:scaron.info,2020-02-22:/publications/tro-2020.html<p class="authors"><strong>Romeo Orsolino</strong>, <strong>Michele Focchi</strong>, <strong>Stéphane Caron</strong>, <strong>Gennaro Raiola</strong>,
<strong>Victor Barasuol</strong> and <strong>Claudio Semini</strong>. IEEE Transactions on Robotics.
Submitted March 2019. Accepted February 2020. Published August 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In legged locomotion the support region is defined as the 2D horizontal convex
area where the robot is able to support its own …</p></div><p class="authors"><strong>Romeo Orsolino</strong>, <strong>Michele Focchi</strong>, <strong>Stéphane Caron</strong>, <strong>Gennaro Raiola</strong>,
<strong>Victor Barasuol</strong> and <strong>Claudio Semini</strong>. IEEE Transactions on Robotics.
Submitted March 2019. Accepted February 2020. Published August 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In legged locomotion the support region is defined as the 2D horizontal convex
area where the robot is able to support its own body weight in static
conditions. Despite this definition, when the joint-torque limits (actuation
limits) are hit, the robot can be unable to carry its own body weight, even
when the projection of its Center of Mass (CoM) lies inside the support region.
In this manuscript we overcome this inconsistency by defining the Feasible
Region, a revisited support region that guarantees both global static stability
of the robot and the existence of a set of joint torques that are able to
sustain the body weight. Thanks to the usage of an Iterative Projection (IP)
algorithm, we show that the Feasible Region can be efficiently employed for
online motion planning of loco-manipulation tasks for both humanoids and
quadrupeds. Unlike the classical support region, the Feasible Region represents
a local measure of the robots robustness to external disturbances and it must
be recomputed at every configuration change. For this, we also propose a global
extension of the Feasible Region that is configuration independent and only
needs to be recomputed at every stance change.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://arxiv.org/pdf/1903.07999.pdf">Paper</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/orsoromeo/jet-leg">Jet-Leg library</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/TRO.2020.2983318">10.1109/TRO.2020.2983318</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">orsolino2020tro</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Feasible Region: an Actuation-Aware Extension of the Support Region}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Orsolino, Romeo and Focchi, Michele and Caron, St{\'e}phane and Raiola, Gennaro and Barasuol, Victor and Semini, Claudio}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Transactions on Robotics}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">aug</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{36}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{4}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{1239--1255}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://arxiv.org/pdf/1903.07999.pdf}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{IEEE}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/TRO.2020.2983318}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
On scaling control systems for bipedal locomotion2019-12-12T00:00:00+01:002022-02-06T00:00:00+01:00Stéphane Carontag:scaron.info,2019-12-12:/blog/control-systems-for-bipedal-locomotion.html<p>What follows is an opinion piece I wrote in 2019 after our team ran a demo of
<a class="reference external" href="https://scaron.info/publications/humanoids-aircraft-manufacturing.html">humanoid robots at an Airbus factory</a>. It was
displayed on the site's landing page for a while, and is now saved here as a
memory of thoughts from that time.</p>
<div class="section" id="systems-for-planning-and-balancing">
<h2>Systems for planning …</h2></div><p>What follows is an opinion piece I wrote in 2019 after our team ran a demo of
<a class="reference external" href="https://scaron.info/publications/humanoids-aircraft-manufacturing.html">humanoid robots at an Airbus factory</a>. It was
displayed on the site's landing page for a while, and is now saved here as a
memory of thoughts from that time.</p>
<div class="section" id="systems-for-planning-and-balancing">
<h2>Systems for planning and balancing</h2>
<p>I'm always wondering why we don't have abler mobile machines around us already.
One reason, I thought at first, is that their control algorithms are designed
to <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">track narrow trajectories</a> and they are only as smart as we are
good at planning these trajectories. Digging in this direction, with Adrien
Escande, Leonardo Lanari and Bastien Mallein, we found ways to <a class="reference external" href="https://scaron.info/publications/tro-2019.html">generate more
general bipedal trajectories</a> that
reflect height variations of the center of mass, for example when the robot
climbs stairs. We then turned these trajectories into a controller by model
predictive control.</p>
<p>Meanwhile, my engineering self was developing a <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">walking controller</a> for the Airbus
demonstration, where the focus on robustness and repeatability strongly favored
control systems with the <em>least</em> critical parameters to tune. In that context,
more general trajectories were not a game changer. What helped more was the
simplification of the robot's <a class="reference external" href="https://scaron.info/publications/icra-2019.html">stabilizer</a>, which is the reflexive part that
decides how to react to disturbances (roll the ankle? shift the pelvis? maybe
tilt the torso as well?). On HRP-4 all these balancing behaviors ended up
implemented by hand-tuned control systems such as linear feedback and linear
model predictive control. The end result looked like this:</p>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/vFCFKAunsYM?start=22" allowfullscreen seamless frameBorder="0"></iframe></div><p>This behavior worked repeatably both in the lab and on-site at the factory, yet
soon we wanted the robot to act smarter under perturbations. What if it starts
leaning to the side during climbing, can it reach our for additional support?
Maybe grab the handrail? What if we want the robot to push a workload while
climbing, can we include an external force observer in the pipeline? The
answers to all these questions are positive, but a key limitation of control
systems is that <em>system complexity</em> grows superlinearly with the number of
components. The more these components interact with each other, the more
superlinearly this complexity grows, and the more challenging hand tuning all
their parameters becomes. Consequently, there are diminishing returns to adding
people or time to scale up a project, and we are hitting an invisible limit to
the richness of robot behaviors we implement in practice with control systems.</p>
<p>This observation does not mean our robots' future is bleak! Only that it is not
100% made of control systems. HRP-4 and its fellow robots have the potential to
walk and recover from disturbances in general scenarios, carry loads, see and
feel enough from the world to react in meaningful ways. How do we get them to
the next level? How can we make legged robots walk better?</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The tutorial on <a class="reference external" href="https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html">prototyping a walking pattern generator</a> goes step by step
through the higher-level components of the walking controller from the demo. If
you are at the stage of building your intuition, this Python example is for
you.</p>
<p>The stabilizer relies on linear feedback of the 3D divergent component of
motion. In the course of 2019 (well after the Airbus demo), I realized we could
get the height variations from the more general trajectories for <em>cheap</em>
(cheaper, in engineering time and system complexity, than replacing the whole
model predictive controller) by doing linear feedback on a <a class="reference external" href="https://scaron.info/talks/jrl-2019.html">4D divergent
component of motion</a> instead. If you are a
control theorist, the connection with exponential dichotomies is for you. Thank
<a class="reference external" href="https://www.diag.uniroma1.it/%7Elanari/">Leonardo Lanari</a> for pointing it
out.</p>
</div>
Humanoid Robots in Aircraft Manufacturing: The Airbus Use Cases2019-10-31T00:00:00+01:002019-10-31T00:00:00+01:00Stéphane Carontag:scaron.info,2019-10-31:/publications/humanoids-aircraft-manufacturing.html<p class="authors"><strong>Abderrahmane Kheddar</strong>, <strong>Stéphane Caron</strong>, <strong>Pierre Gergondet</strong>, <strong>Andrew
Comport</strong>, <strong>Arnaud Tanguy</strong>, <strong>Christian Ott</strong>, <strong>Bernd Henze</strong>, <strong>George
Mesesan</strong>, <strong>Johannes Englsberger</strong>, <strong>Máximo A. Roa</strong>, <strong>Pierre-Brice
Wieber</strong>, <strong>François Chaumette</strong>, <strong>Fabien Spindler</strong>, <strong>Giuseppe Oriolo</strong>,
<strong>Leonardo Lanari</strong>, <strong>Adrien Escande</strong>, <strong>Kevin Chappellet</strong>, <strong>Fumio
Kanehiro</strong> and <strong>Patrice Rabaté</strong>. IEEE Robotics and Automation Magazine.
December 2019. Best Paper Award …</p><p class="authors"><strong>Abderrahmane Kheddar</strong>, <strong>Stéphane Caron</strong>, <strong>Pierre Gergondet</strong>, <strong>Andrew
Comport</strong>, <strong>Arnaud Tanguy</strong>, <strong>Christian Ott</strong>, <strong>Bernd Henze</strong>, <strong>George
Mesesan</strong>, <strong>Johannes Englsberger</strong>, <strong>Máximo A. Roa</strong>, <strong>Pierre-Brice
Wieber</strong>, <strong>François Chaumette</strong>, <strong>Fabien Spindler</strong>, <strong>Giuseppe Oriolo</strong>,
<strong>Leonardo Lanari</strong>, <strong>Adrien Escande</strong>, <strong>Kevin Chappellet</strong>, <strong>Fumio
Kanehiro</strong> and <strong>Patrice Rabaté</strong>. IEEE Robotics and Automation Magazine.
December 2019. Best Paper Award.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We report results from a collaborative project that investigated the deployment
of <a class="reference external" href="https://cordis.europa.eu/article/id/386867-humanoids-may-soon-conquer-airplane-assembly">humanoid robotic solutions in aircraft manufacturing</a>
for some assembly operations where access is not possible for wheeled or
rail-ported robotic platforms. Recent developments in multi-contact planning
and control, bipedal walking, embedded SLAM, whole-body multi-sensory task
space optimization control, and contact detection and safety, suggest that
humanoids could be a plausible solution for automation given the specific
requirements in such large-scale manufacturing sites. The main challenge is to
integrate these scientific and technological advances into two existing
humanoid platforms: the position controlled HRP-4 and the torque controlled
TORO. This integration effort was demonstrated in a bracket assembly operation
inside a 1:1 scale A350 mockup of the front part of the fuselage at the Airbus
Saint-Nazaire site. We present and discuss the main results that have been
achieved in this project and provide recommendations for future work.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal-lirmm.ccsd.cnrs.fr/lirmm-02303117/document">Paper</a></td>
</tr>
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://ieeexplore.ieee.org/ielx7/100/4600619/8889461/mra2943395-kheddar-mm.zip?tp=&arnumber=8889461">Demonstration at the Airbus Saint-Nazaire factory</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">Walking controller of HRP-4</a> (Section IV.A)</td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/MRA.2019.2943395">10.1109/MRA.2019.2943395</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">kheddar2019ram</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Humanoid robots in aircraft manufacturing}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Kheddar, Abderrahmane and Caron, St{\'e}phane and Gergondet, Pierre and Comport, Andrew and Tanguy, Arnaud and Ott, Christian and Henze, Bernd and Mesesan, George and Englsberger, Johannes and Roa, M{\'a}ximo A and Wieber, Pierre-Brice and Chaumette, Fran{\c c}ois and Spindler, Fabien and Oriolo, Giuseppe and Lanari, Leonardo and Escande, Adrien and Chappellet, K{\'e}vin and Kanehiro, Fumio and Rabat{\'e}, Patrice}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Robotics and Automation Magazine}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{IEEE}</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{26}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{4}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2019}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">dec</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/MRA.2019.2943395}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Divergent components of motion2019-10-29T00:00:00+01:002019-10-29T00:00:00+01:00Stéphane Carontag:scaron.info,2019-10-29:/talks/jrl-2019.html<p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 29 October 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Some nonlinear control systems admit an <a class="reference external" href="https://en.wikipedia.org/wiki/Exponential_dichotomy">exponential dichotomy</a> (Coppel, 1966), that is
to say, their dynamics can be decomposed into (exponentially) stable and
unstable components. Walking robots fall into this category, and we call their
unstable …</p></div><p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 29 October 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Some nonlinear control systems admit an <a class="reference external" href="https://en.wikipedia.org/wiki/Exponential_dichotomy">exponential dichotomy</a> (Coppel, 1966), that is
to say, their dynamics can be decomposed into (exponentially) stable and
unstable components. Walking robots fall into this category, and we call their
unstable components <em>divergent components of motion</em> (DCM). The concept of DCM
has been fruitfully applied to the linear inverted pendulum (LIP) for both
walking pattern generation and balance feedback control. But DCMs can be found
for other models as well! In this talk, we will discuss DCMs for the
variable-height inverted pendulum (VHIP), an extension of the LIP where the
controller can add height variations. Ideally, we would like our robot to
behave as a LIP (nominal height) unless some perturbation occurs and the robot
resorts to the height-variation strategy, if it has to. Deciding when to use or
not this strategy may seem "smart" or predictive, but we will see that it can
be implemented straightforwardly as linear feedback over a 4D DCM.</p>
</div>
<div class="section" id="videos">
<h2>Videos</h2>
<div class="section" id="chatty-presentation">
<h3>Chatty Presentation</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/icra-2020.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
<div class="section" id="tl-dw">
<h3>TL;DW</h3>
<p>
<video width="95%" controls>
<source src="https://scaron.info/videos/vhip-stabilization.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
</p></div>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/jrl-2019.pdf">Slides</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02289919/document">Feedback control of a 4D DCM for the variable-height inverted pendulum</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">Walking trajectory generation with height variations</a></td>
</tr>
</tbody>
</table>
</div>
Lower body control of a semi-autonomous avatar in Virtual Reality: Balance and Locomotion of a 3D Bipedal Model2019-10-15T00:00:00+02:002019-10-15T00:00:00+02:00Stéphane Carontag:scaron.info,2019-10-15:/publications/vrst-2019.html<p class="authors"><strong>Vincent Thomasset</strong>, <strong>Stéphane Caron</strong> and <strong>Vincent Weistroffer</strong>. ACM
Symposium on Virtual Reality Software and Technology, Parramatta, Australia,
November 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Animated virtual humans may rely on full-body tracking system to reproduce user
motions. In this paper, we reduce tracking to the upper-body and reconstruct
the lower body to follow autonomously …</p></div><p class="authors"><strong>Vincent Thomasset</strong>, <strong>Stéphane Caron</strong> and <strong>Vincent Weistroffer</strong>. ACM
Symposium on Virtual Reality Software and Technology, Parramatta, Australia,
November 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Animated virtual humans may rely on full-body tracking system to reproduce user
motions. In this paper, we reduce tracking to the upper-body and reconstruct
the lower body to follow autonomously its upper counterpart. Doing so reduces
the number of sensors required, making the application of virtual humans
simpler and cheaper. It also enable deployment in cluttered scenes where the
lower body is often hidden. The contribution here is the inversion of the
well-known capture problem for bipedal walking. It determines footsteps rather
than center-of-mass motions and yet can be solved with an off-the-shelf capture
problem solver. The quality of our method is assessed in real-time tracking
experiments on a wide variety of movements.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal-cea.archives-ouvertes.fr/cea-02385996/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1145/3359996.3364240">10.1145/3359996.3364240</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">thomasset2019vrst</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Lower body control of a semi-autonomous avatar in Virtual Reality: Balance and Locomotion of a 3D Bipedal Model}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Thomasset, Vincent and Caron, St{\'e}phane and Weistroffer, Vincent}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{ACM Symposium on Virtual Reality Software and Technology}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2019}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">nov</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1145/3359996.3364240}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Capture point2019-10-14T00:00:00+02:002019-10-14T00:00:00+02:00Stéphane Carontag:scaron.info,2019-10-14:/robot-locomotion/capture-point.html<p>The <em>capture point</em> is a characteristic quantity of the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted
pendulum model</a> used in legged
robot locomotion. It was originally 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 …</p><p>The <em>capture point</em> is a characteristic quantity of the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted
pendulum model</a> used in legged
robot locomotion. It was originally coined by <a class="reference external" href="https://doi.org/10.1109/ICHR.2006.321385">Pratt et al. (2006)</a> to address a question of push
recovery: where should the robot step <em>right now</em> to eliminate linear momentum
and come <em>asymptotically</em> to a stop?</p>
<div class="section" id="derivation">
<h2>Derivation</h2>
<p>Let us start from the equation of motion of the linear inverted pendulum, where
all the mass is concentrated at the center of mass <span class="math">\(\bfp_G\)</span>:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \omega^2 (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>We assume that the robot steps instantly at time <span class="math">\(t=0\)</span> and maintains its
<a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">ZMP</a> <span class="math">\(\bfp_Z\)</span> at a constant
location in its new foothold, so that <span class="math">\(\bfp_Z\)</span> is stationary. Since the
natural frequency <span class="math">\(\omega\)</span> of the pendulum is also a model constant, we
can solve this second-order linear differential equation as:</p>
<div class="math">
\begin{equation*}
\bfp_G(t) = \displaystyle \bfp_Z + \frac{e^{\omega t}}{2} \left[\bfp_G(0) +
\frac{\bfpd_G(0)}{\omega} - \bfp_Z\right] + \frac{e^{-\omega t}}{2}
\left[\bfp_G(0) - \frac{\bfpd_G(0)}{\omega} - \bfp_Z\right]
\end{equation*}
</div>
<p>This function is the sum of a stationary term <span class="math">\(\bfp_Z\)</span>, a convergent term
factored by <span class="math">\(e^{-\omega t}\)</span> that vanishes as <span class="math">\(t \to \infty\)</span>, and a
term factored by <span class="math">\(e^{\omega t}\)</span> that diverges as <span class="math">\(t \to \infty\)</span>.
Let us define the <em>capture point</em> as:</p>
<div class="math">
\begin{equation*}
\bfp_C \defeq \bfp_G + \frac{\bfpd_G}{\omega}
\end{equation*}
</div>
<p>The divergent term in <span class="math">\(\bfp_G(t)\)</span> is then <span class="math">\(e^{\omega t}/2
(\bfp_C(0) - \bfp_Z)\)</span>. In particular, the <em>only</em> way for the center of mass
trajectory to be bounded is for the stationary ZMP to be equal to the
instantaneous capture point:</p>
<div class="math">
\begin{equation*}
\bfp_Z = \bfp_C(0) \ \Longrightarrow \ \bfp_G(t) \underset{t \to
\infty}{\longrightarrow} \bfp_C(0)
\end{equation*}
</div>
<p>We can thus interpret the capture point as a point where the robot should step
(shift its ZMP) in order to come (asymptotically) to a stop.</p>
</div>
<div class="section" id="connection-to-balance">
<h2>Connection to balance</h2>
<p>The capture point is a <a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent component of motion</a> of the linear inverted pendulum. Shifting the
ZMP to the capture point prevents divergence from the unstable dynamics of the
model, but does not control the other (stable) component. In effect, the system
comes to a stop following its <em>natural</em> dynamics:</p>
<div class="math">
\begin{equation*}
\bfpd_G = \omega (\bfp_C - \bfp_G)
\end{equation*}
</div>
<p>This phenomenon is noticable in <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html#walking-stabilization">balance controllers</a> based on
capture point feedback such as <a class="reference external" href="https://doi.org/10.1109/IROS.2011.6094435">Englsberger et al. (2011)</a> and <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2012.6651601">Morisawa et al. (2012)</a>. Take the robot standing,
push it in a given direction and sustain your push, then suddenly release it:
the robot will come back to its reference standing position following its
natural dynamics (which only depend on <span class="math">\(\omega\)</span>, <em>i.e.</em> gravity and the
height of the center of mass), regardless of the values of the various feedback
gains used in the balance controller. You can for instance test this behavior
in dynamic simulations with the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">lipm_walking_controller</a>.</p>
<p>This behavior highlights how balance controllers based on capture-point
feedback are not trying to come to a stop as fast as possible. Rather, they
focus on preventing divergence, and leverage passive dynamics to absorb
undesired linear momentum. When using linear feedback, <a class="reference external" href="https://doi.org/10.1109/ROBOT.2009.5152284">Sugihara (2009)</a> showed that this approach
maximizes the basin of attraction of the resulting controller.</p>
</div>
<div class="section" id="boundedness-condition">
<h2>Boundedness condition</h2>
<p>The derivation above can be generalized to the case where <span class="math">\(\bfp_Z(t)\)</span> is
time-varying rather than time-invariant. Consider the equation of motion split
as follows into divergent and convergent components:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfpd_C & = & \omega (\bfp_C - \bfp_Z) \\
\bfpd_G & = & \omega (\bfp_C - \bfp_G)
\end{array}
\end{equation*}
</div>
<p>The capture point diverges away from the ZMP while the center of mass is
attracted to the capture point:</p>
<img alt="Decoupled dynamics between the ZMP, capture point and center of mass" class="center max-height-200px max-width-90pct" src="https://scaron.info/figures/zmp-cp-com.png" />
<p>As the center-of-mass dynamics are convergent, the system diverges <em>if and only
if</em> its capture point diverges. We can therefore focus on the
capture point dynamics alone.</p>
<p>The solution to a first-order linear time-varying differential equation is:</p>
<div class="math">
\begin{equation*}
\dot{\bfy}(t) - a(t) \bfy(t) = \bfb(t) \ \Longrightarrow
\ \bfy(t) = e^{A(t)} \left(\bfy(0) + \int_{\tau=0}^t \bfb(\tau) e^{-A(\tau)}
{\rm d} \tau \right)
\end{equation*}
</div>
<p>where <span class="math">\(A\)</span> is the antiderivative of <span class="math">\(a\)</span> such that <span class="math">\(A(0)=0\)</span>.
Applied to capture point dynamics, this formula becomes:</p>
<div class="math">
\begin{equation*}
\bfp_C(t) = e^{\omega t} \left(\bfp_C(0) - \omega \int_{\tau=0}^t \bfp_Z(t)
e^{-\omega \tau} {\rm d}\tau\right)
\end{equation*}
</div>
<p>We can check how, in the previous case where <span class="math">\(\bfp_Z\)</span> is stationary, this
formula becomes:</p>
<div class="math">
\begin{equation*}
\bfp_C(t) = \bfp_Z + e^{\omega t} (\bfp_C(0) - \bfp_Z)
\end{equation*}
</div>
<p>The capture point trajectory is then bounded if and only if <span class="math">\(\bfp_Z =
\bfp_C(0)\)</span>, which is indeed the result we obtained above. In the general case,
the capture point stays bounded if and only if:</p>
<div class="math">
\begin{equation*}
\bfp_C(0) = \omega \int_{\tau=0}^t \bfp_Z(t) e^{-\omega \tau} {\rm d}\tau
\end{equation*}
</div>
<p>This condition was coined <em>boundedness condition</em> by <a class="reference external" href="https://doi.org/10.1109/HUMANOIDS.2014.7041478">Lanari et al. (2014)</a>. It relates <em>future</em> system
inputs to the present state, and characterizes the subset of these inputs that
will actually stabilize the system in the long run. The boundedness condition
is, for instance, a core component of the walking trajectory generator from
<a class="reference external" href="https://arxiv.org/pdf/1901.08505.pdf">Scianca et al. (2019)</a>. It can also be
applied to more general reduced models such as the <a class="reference external" href="/publications/tro-2019.html">variable-height inverted
pendulum</a>.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The notion of <a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent component of motion</a>
behind the capture point reaches beyond the linear inverted pendulum model. For
instance, there exists a generalization of the capture point to <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02289919/document">variable
height center-of-mass trajectories</a>, from which the
<em>tiptoe</em> balancing strategy emerges in addition to the stepping strategy.</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Linear inverted pendulum model2019-10-14T00:00:00+02:002019-10-14T00:00:00+02:00Stéphane Carontag:scaron.info,2019-10-14:/robot-locomotion/linear-inverted-pendulum-model.html<p>The linear inverted pendulum is a <a class="reference external" href="https://scaron.info/robot-locomotion/point-mass-model.html">point-mass model</a> that focuses on the translational dynamics
of a legged robot's locomotion.</p>
<div class="section" id="assumptions">
<h2>Assumptions</h2>
<p>Both fixed and mobile robots are usually modeled as rigid bodies connected by
actuated joints. The general <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> for such a system are:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd …</div></div><p>The linear inverted pendulum is a <a class="reference external" href="https://scaron.info/robot-locomotion/point-mass-model.html">point-mass model</a> that focuses on the translational dynamics
of a legged robot's locomotion.</p>
<div class="section" id="assumptions">
<h2>Assumptions</h2>
<p>Both fixed and mobile robots are usually modeled as rigid bodies connected by
actuated joints. The general <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a> for such a system are:</p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \sum_{i=1}^N \bfJ_{C_i}^\top \bff_i,
\end{equation*}
</div>
<p>where <span class="math">\(\bfq\)</span> is the vector of actuated and unactuated coordinates.
Actuated coordinates correspond to joint angles directly controlled by motors.
Unactuated coordinates correspond to the six degrees of freedom for the
position and orientation of the <em>floating base</em> (a frame attached to any of the
robot's bodies) with respect to the inertial fame. The vector <span class="math">\(\bfq\)</span> is
typically high-dimensional.</p>
<div class="section" id="centroidal-dynamics">
<h3>Centroidal dynamics</h3>
<p>The first working assumption to simplify this model is (Assumption 1) that the
robot has enough joint torques to realize the actuated part of the equation,
and focus on the <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton-Euler equations</a> that correspond to the six unactuated
coordinates:</p>
<div class="math">
\begin{equation*}
\begin{bmatrix} m \bfpdd_G \\ \dot{\bfL}_G \end{bmatrix}
=
\begin{bmatrix} \bff \\ \bftau_G \end{bmatrix}
+
\begin{bmatrix} m \bfg \\ \boldsymbol{0} \end{bmatrix}
\end{equation*}
</div>
<p>where on the left-hand side <span class="math">\(\bfp_G\)</span> is the position of the center
of mass (CoM) and <span class="math">\(\bfL_G\)</span> is the net angular momentum around the CoM,
while on the right-hand side <span class="math">\(\bff\)</span> is the resultant of contact forces,
<span class="math">\(\bftau_G\)</span> is the moment of contact forces around the CoM, <span class="math">\(m\)</span> is
the robot mass and <span class="math">\(\bfg\)</span> is the gravity vector. This model is called
<em>centroidal dynamics</em>.</p>
</div>
<div class="section" id="linearized-dynamics">
<h3>Linearized dynamics</h3>
<p>Angular momentum or height variations make centroidal dynamics nonlinear. This
means for instance that, to generate a trajectory for this system, one needs to
solve a nonlinear optimization. An alternative to linearize this system is to
make two assumptions:</p>
<ul class="simple">
<li><em>Assumption 2:</em> there is no angular momentum around the center of mass
<span class="math">\((\dot{\bfL}_G=\boldsymbol{0})\)</span>. This is why the Honda P2 <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=172">walks with
locked arms</a>.</li>
<li><em>Assumption 3:</em> the center of mass keeps a constant height. This is why the
Honda P2 <a class="reference external" href="https://youtu.be/d2BUO4HEhvM?t=26">walks with bent knees</a>.</li>
</ul>
<p>These two assumptions are used to derive linearized dynamics as follows.</p>
</div>
</div>
<div class="section" id="id1">
<h2>Equations of motion</h2>
<p>Let us consider the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point (ZMP)</a> of the contact wrench. It is a
point <span class="math">\(Z\)</span> where the moment of contact forces is vertical:</p>
<div class="math">
\begin{equation*}
\bfe_Z \times \bftau_Z = \boldsymbol{0}
\end{equation*}
</div>
<p>with <span class="math">\(\bfe_Z\)</span> the unit upward vertical vector of the inertial frame. This
quantity defines an axis in general: to make <span class="math">\(Z\)</span> a unique point, let us
take it on the ground with <span class="math">\(z_Z = z_G - h\)</span>, where <span class="math">\(h\)</span> is the
constant height of the CoM. The moment <span class="math">\(\bftau_Z\)</span> of the contact wrench
at this ZMP is related to the moment at the CoM by:</p>
<div class="math">
\begin{equation*}
\bftau_Z = \bftau_G + (\bfp_G - \bfp_Z) \times \bff
\end{equation*}
</div>
<p>Since <span class="math">\(\bftau_G = \boldsymbol{0}\)</span> (Assumption 2), we have:</p>
<div class="math">
\begin{equation*}
\bfe_Z \times ((\bfp_G - \bfp_Z) \times \bff) = \boldsymbol{0}
\end{equation*}
</div>
<p>Applying the <a class="reference external" href="https://en.wikipedia.org/wiki/Triple_product#Vector_triple_product">vector triple product</a> formula,
we get:</p>
<div class="math">
\begin{equation*}
f_z (\bfp_G - \bfp_Z) - h \bff = \boldsymbol{0}
\end{equation*}
</div>
<p>From Newton's equation, <span class="math">\(\bff = m (\bfpdd_G - \bfg)\)</span> and we can rewrite
the equation above as:</p>
<div class="math">
\begin{equation*}
h (\bfpdd_G - \bfg) = (\zdd_G + g) (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>Since <span class="math">\(\zdd_G = 0\)</span> (Assumption 3), this equation is a trivial identity in
the vertical direction while its horizontal coordinates are:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \omega^2 (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>where <span class="math">\(\omega^2 = g / h\)</span>, <span class="math">\(g\)</span> is the gravity constant and <span class="math">\(h
= z_G - z_Z\)</span> is the constant height of the center of mass. The constant
<span class="math">\(\omega\)</span> is called <em>natural frequency</em> of the linear inverted pendulum.</p>
<img alt="Humanoid robot walking in the linear inverted pendulum mode" class="center max-height-450px" src="https://scaron.info/figures/lipm.png" />
<p>In this model, the robot can be seen as a point-mass concentrated at <span class="math">\(G\)</span>
resting on a mass-less leg in contact with the ground at <span class="math">\(Z\)</span>.
Intuitively, the ZMP is the point where the robot applies its weight. As a
consequence, this point needs to lie inside the contact surface <span class="math">\(\cal S\)</span>.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The linear inverted pendulum <em>mode</em> (not model) was introduced in <a class="reference external" href="https://doi.org/10.1109/IROS.2001.973365">Kajita et
al. (2001)</a> for walking trajectory
generation. The rationale for calling it a <em>mode</em> of motion is that it relies
solely on planar linear momentum, which is one among many ways to affect the
acceleration of the center of mass and thus locomote (others include <a class="reference external" href="/publications/tro-2019.html">height
variations</a> and angular momentum variations that
separate the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">ZMP</a> and <a class="reference external" href="http://ijr.sagepub.com/cgi/doi/10.1177/0278364905058363">centroidal
moment pivot</a>).</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Balance of Humanoid Robots in a Mix of Fixed and Sliding Multi-Contact Scenarios2019-09-16T00:00:00+02:002019-09-16T00:00:00+02:00Stéphane Carontag:scaron.info,2019-09-16:/publications/icra-2020-samadi.html<p class="authors"><strong>Saeid Samadi</strong>, <strong>Stéphane Caron</strong>, <strong>Arnaud Tanguy</strong> and <strong>Abderrahmane
Kheddar</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>This study deals with the balance of humanoid or multi-legged robots in a
multi-contact setting where a chosen subset of contacts is undergoing desired
sliding-task motions. One method to keep balance is to hold the center-of-mass …</p></div><p class="authors"><strong>Saeid Samadi</strong>, <strong>Stéphane Caron</strong>, <strong>Arnaud Tanguy</strong> and <strong>Abderrahmane
Kheddar</strong>. ICRA 2020, May 2020.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>This study deals with the balance of humanoid or multi-legged robots in a
multi-contact setting where a chosen subset of contacts is undergoing desired
sliding-task motions. One method to keep balance is to hold the center-of-mass
(CoM) within an admissible convex area. This area is calculated based on the
contact positions and forces. We introduce a methodology to compute this CoM
support area (CSA) for multiple fixed and intentionally sliding contacts. To
select the most appropriate CoM position within CSA, we account for (i)
constraints of multiple fixed and sliding contacts, (ii) desired wrench
distribution for contacts, and (iii) desired CoM position (eventually dictated
by other tasks). These are formulated as a quadratic programming (QP)
optimization problems. We illustrate our approach with pushing against a wall
and wiping, and conducted experiments using the HRP-4 humanoid robot.</p>
</div>
<div class="section" id="video">
<h2>Video</h2>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/Wai-Lp4e5FE" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-02297879/document">Paper</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA40945.2020.9197253">10.1109/ICRA40945.2020.9197253</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">samadi2020icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Balance of Humanoid Robots in a Mix of Fixed and Sliding Multi-Contact Scenarios}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Samadi, Saeid and Caron, St{\'e}phane and Tanguy, Arnaud and Kheddar, Abderrahmane}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-02297879}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA40945.2020.9197253}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Parametric recurrence quantification analysis of autoregressive processes for pattern recognition in multichannel electroencephalographic data2019-07-05T00:00:00+02:002019-07-05T00:00:00+02:00Stéphane Carontag:scaron.info,2019-07-05:/publications/patcog-2020.html<p class="authors"><strong>Sofiane Ramdani</strong>, <strong>Anthony Boyer</strong>, <strong>Stéphane Caron</strong>, <strong>François Bonnetblanc</strong>, <strong>Frédéric Bouchara</strong>, <strong>HuguesDuffau</strong> and <strong>Annick Lesne</strong>. Pattern Recognition. Submitted July 2019. Accepted August 2020. To appear in January 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Recurrence quantification analysis (RQA) is an acknowledged method for the
characterization of experimental time series. We propose a parametric version
of RQA …</p></div><p class="authors"><strong>Sofiane Ramdani</strong>, <strong>Anthony Boyer</strong>, <strong>Stéphane Caron</strong>, <strong>François Bonnetblanc</strong>, <strong>Frédéric Bouchara</strong>, <strong>HuguesDuffau</strong> and <strong>Annick Lesne</strong>. Pattern Recognition. Submitted July 2019. Accepted August 2020. To appear in January 2021.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>Recurrence quantification analysis (RQA) is an acknowledged method for the
characterization of experimental time series. We propose a parametric version
of RQA, pRQA, allowing a fast processing of spatial arrays of time series, once
each is modeled by an autoregressive stochastic process. This method relies on
the analytical derivation of asymptotic expressions for five current RQA
measures as a function of the model parameters. By avoiding the construction of
the recurrence plot of the time series, pRQA is computationally efficient. As a
proof of principle, we apply pRQA to pattern recognition in multichannel
electroencephalographic (EEG) data from a patient with a brain tumor.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1016/J.PATCOG.2020.107572">10.1016/J.PATCOG.2020.107572</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">ramdani2021patcog</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Parametric recurrence quantification analysis of autoregressive processes for pattern recognition in multichannel electroencephalographic data}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Ramdani, Sofiane and Boyer, Anthony and Caron, St{\'e}phane and Bonnetblanc, Fran{\c{c}}ois and Bouchara, Frederic and Duffau, Hugues and Lesne, Annick}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{Pattern Recognition}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{107572}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2021}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">jan</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{109}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{Elsevier}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1016/j.patcog.2020.107572}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Walking and stair climbing controller for locomotion in an aircraft factory2019-06-05T00:00:00+02:002019-06-05T00:00:00+02:00Stéphane Carontag:scaron.info,2019-06-05:/talks/jpl-2019.html<p class="authors">Talk given at <a class="reference external" href="https://deepmind.com/">DeepMind</a> on 10 May 2019 and at the 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 …</p></div><p class="authors">Talk given at <a class="reference external" href="https://deepmind.com/">DeepMind</a> on 10 May 2019 and at the the
NASA-Caltech <a class="reference external" href="https://www.jpl.nasa.gov/">Jet Propulsion Laboratory</a> on 5 June
2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>The task was initially phrased for humans: go upstairs, walk into the aircraft,
pick up the object and place it on the fuselage. Yet, repetition and the
disposition of target locations turned out to cause chronic back pain, and the
question of automation was raised: can a robot do that? In this talk, we will
see how the HRP-4 humanoid can do that, and discuss the components we developed
for the <a class="reference external" href="https://cordis.europa.eu/article/id/386867-humanoids-may-soon-conquer-airplane-assembly">final demonstrator of the COMANOID project</a>
that ran at the Airbus Saint-Nazaire factory on 21 February 2019: self
localization, locomotion, object localization and manipulation. We will focus
on walking and stair climbing control. The audience is encouraged to bring
plastic bottles.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/jpl-2019.pdf">Slides</a></td>
</tr>
<tr><td><img alt="youtube" class="icon" src="https://scaron.info/images/icons/youtube.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=vFCFKAunsYM&t=22">Stair climbing experiment</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Stair climbing stabilization of the HRP-4 humanoid robot using whole-body admittance control</a></td>
</tr>
</tbody>
</table>
</div>
Stair Climbing Stabilization of the HRP-4 Humanoid Robot using Whole-body Admittance Control2019-05-20T00:00:00+02:002021-10-08T00:00:00+02:00Stéphane Carontag:scaron.info,2019-05-20:/publications/icra-2019.html<p class="authors"><strong>Stéphane Caron</strong>, <strong>Abderrahmane Kheddar</strong> and <strong>Olivier Tempier</strong>. ICRA
2019, Montreal, Canada, May 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We consider dynamic stair climbing with the HRP-4 humanoid robot as part of an
Airbus manufacturing use-case demonstrator. We share experimental knowledge
gathered so as to achieve this task, which HRP-4 had never been challenged to …</p></div><p class="authors"><strong>Stéphane Caron</strong>, <strong>Abderrahmane Kheddar</strong> and <strong>Olivier Tempier</strong>. ICRA
2019, Montreal, Canada, May 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>We consider dynamic stair climbing with the HRP-4 humanoid robot as part of an
Airbus manufacturing use-case demonstrator. We share experimental knowledge
gathered so as to achieve this task, which HRP-4 had never been challenged to
before. In particular, we extend walking stabilization based on <a class="reference external" href="https://doi.org/10.1109/IROS.2010.5651082">linear
inverted pendulum tracking</a> by
quadratic programming-based wrench distribution and a whole-body admittance
controller that applies both end-effector and CoM strategies. While existing
stabilizers tend to use either one or the other, our experience suggests that
the combination of these two approaches improves tracking performance. We
demonstrate this solution in an on-site experiment where HRP-4 climbs an
industrial staircase with 18.5 cm high steps, and release <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">our walking
controller</a> as
open source software.</p>
</div>
<div class="section" id="videos">
<h2>Videos</h2>
<div class="section" id="climbing-stairs">
<h3>Climbing stairs</h3>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/vFCFKAunsYM?start=22" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="standing-on-mobile-ground">
<h3>Standing on mobile ground</h3>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/MxSJtEKkZE0?start=7" allowfullscreen seamless frameBorder="0"></iframe></div></div>
</div>
<div class="section" id="content">
<h2>Content</h2>
<p>The paper on HAL is a <em>post-print</em> I have kept updating after the conference to
keep up with the later additions and fixes implemented in the <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">walking
controller</a>. See
the release notes on GitHub for an overview of the major changes since the
conference version (v1.1).</p>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Post-print</a> paper</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/files/icra-2019/poster.pdf">Poster</a> presented at ICRA 2019</td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="/slides/jpl-2019.pdf">Presentation given at the Jet Propulsion Laboratory</a> on 5 June 2019</td>
</tr>
<tr><td><img alt="mp4" class="icon" src="https://scaron.info/images/icons/video.png" /></td>
<td><a class="reference external" href="https://ieeexplore.ieee.org/document/8794348/media">Stair climbing at the Airbus Saint-Nazaire factory</a></td>
</tr>
<tr><td><img alt="github" class="icon" src="https://scaron.info/images/icons/github.png" /></td>
<td><a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">Walking controller (C++)</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/ICRA.2019.8794348">10.1109/ICRA.2019.8794348</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="supplementary-material">
<h2>Supplementary material</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="/doc/lipm_walking_controller/">Controller documentation</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/robot-locomotion/floating-base-estimation.html">Floating base observer</a> used in this controller</td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">Overview</a> of the controller's principles</td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html">Python prototype</a> of the walking trajectory generator</td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@inproceedings</span><span class="p">{</span><span class="nl">caron2019icra</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Stair Climbing Stabilization of the {HRP}-4 Humanoid Robot using Whole-body Admittance Control}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane and Kheddar, Abderrahmane and Tempier, Olivier}</span><span class="p">,</span>
<span class="na">booktitle</span> <span class="p">=</span> <span class="s">{IEEE International Conference on Robotics and Automation}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01875387}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2019}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">may</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/ICRA.2019.8794348}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
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 trajectory generator (Python)</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td>Walking trajectory generator manual: <a class="reference external" href="https://scaron.info/doc/capture-walkgen/">HTML</a> or <a class="reference external" href="https://scaron.info/doc/capture-walkgen/capture-walkgen.pdf">PDF</a></td>
</tr>
<tr><td><img alt="doi" class="icon" src="https://scaron.info/images/icons/doi.png" /></td>
<td><a class="reference external" href="https://doi.org/10.1109/TRO.2019.2923971">10.1109/TRO.2019.2923971</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="bibtex">
<h2>BibTeX</h2>
<div class="highlight"><pre><span></span><span class="nc">@article</span><span class="p">{</span><span class="nl">caron2019tro</span><span class="p">,</span>
<span class="na">title</span> <span class="p">=</span> <span class="s">{Capturability-based Pattern Generation for Walking with Variable Height}</span><span class="p">,</span>
<span class="na">author</span> <span class="p">=</span> <span class="s">{Caron, St{\'e}phane and Escande, Adrien and Lanari, Leonardo and Mallein, Bastien}</span><span class="p">,</span>
<span class="na">journal</span> <span class="p">=</span> <span class="s">{IEEE Transactions on Robotics}</span><span class="p">,</span>
<span class="na">year</span> <span class="p">=</span> <span class="s">{2020}</span><span class="p">,</span>
<span class="na">month</span> <span class="p">=</span> <span class="nv">apr</span><span class="p">,</span>
<span class="na">volume</span> <span class="p">=</span> <span class="s">{36}</span><span class="p">,</span>
<span class="na">number</span> <span class="p">=</span> <span class="s">{2}</span><span class="p">,</span>
<span class="na">pages</span> <span class="p">=</span> <span class="s">{517--536}</span><span class="p">,</span>
<span class="na">url</span> <span class="p">=</span> <span class="s">{https://hal.archives-ouvertes.fr/hal-01689331}</span><span class="p">,</span>
<span class="na">publisher</span> <span class="p">=</span> <span class="s">{IEEE}</span><span class="p">,</span>
<span class="na">doi</span> <span class="p">=</span> <span class="s">{10.1109/TRO.2019.2923971}</span><span class="p">,</span>
<span class="p">}</span>
</pre></div>
</div>
Prototyping a walking pattern generator2019-04-30T00:00:00+02:002019-04-30T00:00:00+02:00Stéphane Carontag:scaron.info,2019-04-30:/robot-locomotion/prototyping-a-walking-pattern-generator.html<p>Walking trajectory generation is the problem of computing an ideal locomotion
trajectory, sometimes called a <em>walking pattern</em>. It is the first sub-problem
in the <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">traditional walking control scheme</a> for legged robots. A walking
trajectory is dynamically consistent (it respects the laws of physics) and
unperturbed (it assumes all controls are …</p><p>Walking trajectory generation is the problem of computing an ideal locomotion
trajectory, sometimes called a <em>walking pattern</em>. It is the first sub-problem
in the <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html">traditional walking control scheme</a> for legged robots. A walking
trajectory 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 trajectory 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 trajectory generator will be to make the robot walk
through a given sequence of contacts (in Python, a list of <a class="reference external" href="https://scaron.info/doc/pymanoid/contact-stability.html#pymanoid.contact.Contact">pymanoid.Contact</a>
objects). For walking forward, we can generate this sequence simply by placing
footsteps to the left and right of a line segment:</p>
<pre class="code python literal-block">
<span class="kn">import</span> <span class="nn">pymanoid</span>
<span class="k">def</span> <span class="nf">generate_footsteps</span><span class="p">(</span><span class="n">distance</span><span class="p">,</span> <span class="n">step_length</span><span class="p">,</span> <span class="n">foot_spread</span><span class="p">):</span>
<span class="n">contacts</span> <span class="o">=</span> <span class="p">[]</span>
<span class="k">def</span> <span class="nf">append_contact</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
<span class="n">contacts</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">pymanoid</span><span class="o">.</span><span class="n">Contact</span><span class="p">(</span>
<span class="n">shape</span><span class="o">=</span><span class="p">(</span><span class="mf">0.11</span><span class="p">,</span> <span class="mf">0.05</span><span class="p">),</span> <span class="n">pos</span><span class="o">=</span><span class="p">[</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="mf">0.</span><span class="p">],</span> <span class="n">friction</span><span class="o">=</span><span class="mf">0.7</span><span class="p">))</span>
<span class="n">append_contact</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="o">+</span><span class="n">foot_spread</span><span class="p">)</span>
<span class="n">append_contact</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="o">-</span><span class="n">foot_spread</span><span class="p">)</span>
<span class="n">x</span> <span class="o">=</span> <span class="mf">0.</span>
<span class="n">y</span> <span class="o">=</span> <span class="n">foot_spread</span>
<span class="k">while</span> <span class="n">x</span> <span class="o"><</span> <span class="n">distance</span><span class="p">:</span>
<span class="k">if</span> <span class="n">distance</span> <span class="o">-</span> <span class="n">x</span> <span class="o"><=</span> <span class="n">step_length</span><span class="p">:</span>
<span class="n">x</span> <span class="o">+=</span> <span class="nb">min</span><span class="p">(</span><span class="n">distance</span> <span class="o">-</span> <span class="n">x</span><span class="p">,</span> <span class="mf">0.5</span> <span class="o">*</span> <span class="n">step_length</span><span class="p">)</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># still some way to go</span>
<span class="n">x</span> <span class="o">+=</span> <span class="n">step_length</span>
<span class="n">y</span> <span class="o">=</span> <span class="o">-</span><span class="n">y</span>
<span class="n">append_contact</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
<span class="n">append_contact</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="n">y</span><span class="p">)</span> <span class="c1"># now x == distance</span>
<span class="k">return</span> <span class="n">contacts</span>
</pre>
<p>The function depends on three parameters: <tt class="docutils literal">distance</tt>, the total distance in
meters to walk, <tt class="docutils literal">step_length</tt>, which is by definition the distance between
right and left heel in double support, and the lateral distance <tt class="docutils literal">foot_spread</tt>
between left and right foot centers. The first and last steps have half step
length so as to accelerate and decelerate more progressively. In what follows,
the robot will walk seven steps forward with a step length of 30 cm:</p>
<pre class="code python literal-block">
<span class="n">footsteps</span> <span class="o">=</span> <span class="n">generate_footsteps</span><span class="p">(</span>
<span class="n">distance</span><span class="o">=</span><span class="mf">2.1</span><span class="p">,</span>
<span class="n">step_length</span><span class="o">=</span><span class="mf">0.3</span><span class="p">,</span>
<span class="n">foot_spread</span><span class="o">=</span><span class="mf">0.1</span><span class="p">,</span>
<span class="n">friction</span><span class="o">=</span><span class="mf">0.7</span><span class="p">)</span>
</pre>
<p>The robot starts from a standing posture with its center of mass (COM) 85 cm
above ground, which is a comfortable height for the default JVRC-1 model. We
put the robot in this initial posture by solving the inverse kinematics (IK) of
our desired stance (a <a class="reference external" href="https://scaron.info/doc/pymanoid/inverse-kinematics.html#stance">pymanoid.Stance</a>):</p>
<pre class="code python literal-block">
<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s2">"__main__"</span><span class="p">:</span>
<span class="n">dt</span> <span class="o">=</span> <span class="mf">0.03</span> <span class="c1"># simulation time step, in [s]</span>
<span class="n">sim</span> <span class="o">=</span> <span class="n">pymanoid</span><span class="o">.</span><span class="n">Simulation</span><span class="p">(</span><span class="n">dt</span><span class="o">=</span><span class="n">dt</span><span class="p">)</span>
<span class="n">robot</span> <span class="o">=</span> <span class="n">pymanoid</span><span class="o">.</span><span class="n">robots</span><span class="o">.</span><span class="n">JVRC1</span><span class="p">(</span><span class="n">download_if_needed</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">set_viewer</span><span class="p">()</span>
<span class="n">stance</span> <span class="o">=</span> <span class="n">pymanoid</span><span class="o">.</span><span class="n">Stance</span><span class="p">(</span>
<span class="n">com</span><span class="o">=</span><span class="n">pymanoid</span><span class="o">.</span><span class="n">PointMass</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mf">0.85</span><span class="p">],</span> <span class="n">robot</span><span class="o">.</span><span class="n">mass</span><span class="p">),</span>
<span class="n">left_foot</span><span class="o">=</span><span class="n">footsteps</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>
<span class="n">right_foot</span><span class="o">=</span><span class="n">footsteps</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
<span class="n">stance</span><span class="o">.</span><span class="n">bind</span><span class="p">(</span><span class="n">robot</span><span class="p">)</span>
<span class="n">robot</span><span class="o">.</span><span class="n">ik</span><span class="o">.</span><span class="n">solve</span><span class="p">(</span><span class="n">max_it</span><span class="o">=</span><span class="mi">42</span><span class="p">)</span>
</pre>
<p>Our robot is now ready to walk:</p>
<img alt="Humanoid model JVRC-1 is now ready to walk" class="center max-height-400px" src="https://scaron.info/figures/walkgen-ready.png" />
</div>
<div class="section" id="walking-state-machine">
<h2>Walking state machine</h2>
<p>Walking alternates <em>double support phases</em> (DSP), where both feet are in
contact with the ground, and <em>single support phases</em> (SSP), where one foot (the
<em>swing foot</em>) is in the air while the robot supports its weight on the other
one:</p>
<img alt="Single and double support phases while walking" class="center" src="https://scaron.info/figures/walkgen-phases.png" />
<p>We can represent these phases in a <a class="reference external" href="https://en.wikipedia.org/wiki/Finite-state_machine">finite-state machine (FSM)</a> with three states,
standing, double support and single support, implemented as a <a class="reference external" href="https://scaron.info/doc/pymanoid/simulation-environment.html#processes">pymanoid.Process</a>:</p>
<pre class="code python literal-block">
<span class="k">class</span> <span class="nc">WalkingFSM</span><span class="p">(</span><span class="n">pymanoid</span><span class="o">.</span><span class="n">Process</span><span class="p">):</span>
<span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ssp_duration</span><span class="p">,</span> <span class="n">dsp_duration</span><span class="p">):</span>
<span class="nb">super</span><span class="p">(</span><span class="n">WalkingFSM</span><span class="p">,</span> <span class="bp">self</span><span class="p">)</span><span class="o">.</span><span class="fm">__init__</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span> <span class="o">=</span> <span class="n">dsp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">mpc_timestep</span> <span class="o">=</span> <span class="mi">3</span> <span class="o">*</span> <span class="n">dt</span> <span class="c1"># update MPC every 90 [ms] (see below)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">=</span> <span class="mi">2</span>
<span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span> <span class="o">=</span> <span class="n">ssp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="kc">None</span>
<span class="c1">#</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_standing</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">on_tick</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">sim</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"Standing"</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_standing</span><span class="p">()</span>
<span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"DoubleSupport"</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_double_support</span><span class="p">()</span>
<span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"SingleSupport"</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_single_support</span><span class="p">()</span>
<span class="k">raise</span> <span class="ne">Exception</span><span class="p">(</span><span class="s2">"Unknown state: "</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
</pre>
<p>The state machine will switch between single and double support phases based on
the pre-defined phase timings <tt class="docutils literal">ssp_duration</tt> and <tt class="docutils literal">dsp_duration</tt>.</p>
<div class="section" id="standing-state">
<h3>Standing state</h3>
<p>For each state, we define a "start" and "run" function. The standing state
simply waits for the user to set the <tt class="docutils literal">start_walking</tt> boolean attribute to
<tt class="docutils literal">True</tt>:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_standing</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_walking</span> <span class="o">=</span> <span class="kc">False</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="s2">"Standing"</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_standing</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">run_standing</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_walking</span><span class="p">:</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_walking</span> <span class="o">=</span> <span class="kc">False</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o"><</span> <span class="nb">len</span><span class="p">(</span><span class="n">footsteps</span><span class="p">):</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_double_support</span><span class="p">()</span>
</pre>
</div>
<div class="section" id="double-support-state">
<h3>Double support state</h3>
<p>For starters, the double support state simply waits for its phase duration, then
switches to single support:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_double_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">%</span> <span class="mi">2</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span> <span class="c1"># left foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">right_foot</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># right foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">left_foot</span>
<span class="n">dsp_duration</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">==</span> <span class="mi">2</span> <span class="ow">or</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">==</span> <span class="nb">len</span><span class="p">(</span><span class="n">footsteps</span><span class="p">)</span> <span class="o">-</span> <span class="mi">1</span><span class="p">:</span>
<span class="c1"># double support is a bit longer for the first and last steps</span>
<span class="n">dsp_duration</span> <span class="o">=</span> <span class="mi">4</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span> <span class="o">=</span> <span class="n">footsteps</span><span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span><span class="p">]</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">=</span> <span class="n">dsp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="s2">"DoubleSupport"</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_com_mpc_dsp</span><span class="p">()</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">run_double_support</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">run_double_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o"><=</span> <span class="mf">0.</span><span class="p">:</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_single_support</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_com_mpc</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">-=</span> <span class="n">dt</span>
</pre>
<p>We use the start function of the state to save two important contact locations,
namely the stance foot and swing target of the next SSP. The reason for saving
them here rather than in the start function of the SSP itself will become clear
later on, when we implement the <tt class="docutils literal">start_com_mpc_dsp()</tt> and <tt class="docutils literal">run_com_mpc()</tt>
functions. For now, let us complete them with a dummy linear interpolation to
debug the development of our ongoing FSM:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_com_mpc_dsp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">pass</span> <span class="c1"># to be implemented later</span>
<span class="k">def</span> <span class="nf">run_com_mpc</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">set_x</span><span class="p">(</span><span class="mf">0.5</span> <span class="o">*</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span><span class="o">.</span><span class="n">x</span><span class="p">))</span>
</pre>
</div>
<div class="section" id="single-support-state">
<h3>Single support state</h3>
<p>The single support phase interpolates both COM and swing foot trajectories, and
updates stance targets accordingly:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_single_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">%</span> <span class="mi">2</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span> <span class="c1"># left foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">free_contact</span><span class="p">(</span><span class="s1">'left_foot'</span><span class="p">)</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># right foot swings</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">free_contact</span><span class="p">(</span><span class="s1">'right_foot'</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o">+=</span> <span class="mi">1</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span>
<span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">=</span> <span class="s2">"SingleSupport"</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_swing_foot</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">start_com_mpc_ssp</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_single_support</span><span class="p">()</span>
<span class="k">def</span> <span class="nf">run_single_support</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o"><=</span> <span class="mf">0.</span><span class="p">:</span>
<span class="n">stance</span><span class="o">.</span><span class="n">set_contact</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="p">)</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">next_footstep</span> <span class="o"><</span> <span class="nb">len</span><span class="p">(</span><span class="n">footsteps</span><span class="p">):</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_double_support</span><span class="p">()</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># footstep sequence is over</span>
<span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">start_standing</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_swing_foot</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">run_com_mpc</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span> <span class="o">-=</span> <span class="n">dt</span>
</pre>
<p>For now, we apply dummy linear interpolations for both the COM and swing foot
trajectories:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_start</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">pose</span>
<span class="k">def</span> <span class="nf">start_com_mpc_ssp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">pass</span> <span class="c1"># to be implemented later</span>
<span class="k">def</span> <span class="nf">run_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="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">pymanoid</span><span class="o">.</span><span class="n">interp</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="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">set_pose</span><span class="p">(</span><span class="n">new_pose</span><span class="p">)</span>
</pre>
<p>When the phase time reaches the desired SSP duration, the state machine
switches back to double support and the process repeats until the robot has
traversed all footsteps in the sequence, finally switching back to the standing
state.</p>
</div>
<div class="section" id="scheduling-simulation-processes">
<h3>Scheduling simulation processes</h3>
<p>Although we haven't implemented neither swing foot nor COM trajectories yet, we
can already validate this code in simulation. Let us create an FSM process with
phase durations roughly 0.7/0.1 seconds for single and double support
respectively:</p>
<pre class="code python literal-block">
<span class="n">ssp_duration</span> <span class="o">=</span> <span class="mi">24</span> <span class="o">*</span> <span class="n">dt</span> <span class="c1"># 720 [ms]</span>
<span class="n">dsp_duration</span> <span class="o">=</span> <span class="mi">3</span> <span class="o">*</span> <span class="n">dt</span> <span class="c1"># 90 [ms]</span>
<span class="n">fsm</span> <span class="o">=</span> <span class="n">WalkingFSM</span><span class="p">(</span><span class="n">ssp_duration</span><span class="p">,</span> <span class="n">dsp_duration</span><span class="p">)</span>
</pre>
<p>As the FSM updates targets, we schedule it before inverse kinematics in the
simulation:</p>
<pre class="code python literal-block">
<span class="n">sim</span><span class="o">.</span><span class="n">schedule</span><span class="p">(</span><span class="n">fsm</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">ik</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">start</span><span class="p">()</span>
</pre>
<p>Let us throw in some extra drawer processes to keep track of the COM and swing
foot position targets while the robot moves:</p>
<pre class="code python literal-block">
<span class="n">com_traj_drawer</span> <span class="o">=</span> <span class="n">TrajectoryDrawer</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="p">,</span> <span class="s1">'b-'</span><span class="p">)</span>
<span class="n">lf_traj_drawer</span> <span class="o">=</span> <span class="n">TrajectoryDrawer</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">left_foot</span><span class="p">,</span> <span class="s1">'g-'</span><span class="p">)</span>
<span class="n">rf_traj_drawer</span> <span class="o">=</span> <span class="n">TrajectoryDrawer</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">right_foot</span><span class="p">,</span> <span class="s1">'r-'</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">com_traj_drawer</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">lf_traj_drawer</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">rf_traj_drawer</span><span class="p">)</span>
</pre>
<p>Now, the only thing left to do to start walking is:</p>
<pre class="code python literal-block">
<span class="n">In</span> <span class="p">[</span><span class="mi">1</span><span class="p">]:</span> <span class="n">fsm</span><span class="o">.</span><span class="n">start_walking</span> <span class="o">=</span> <span class="kc">True</span>
</pre>
<img alt="This simple finite-state machine makes the legged robot shuffle its feet" class="center max-height-400px" src="https://scaron.info/figures/walkgen-shuffle.png" />
<p>The robot shuffles its feet in a motion that is kinematically but not
dynamically consistent. We can see that by scheduling an extra wrench drawer to
check the dynamics of the simulation:</p>
<pre class="code python literal-block">
<span class="n">wrench_drawer</span> <span class="o">=</span> <span class="n">PointMassWrenchDrawer</span><span class="p">(</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="p">,</span> <span class="n">stance</span><span class="p">)</span>
<span class="n">sim</span><span class="o">.</span><span class="n">schedule_extra</span><span class="p">(</span><span class="n">wrench_drawer</span><span class="p">)</span>
</pre>
<p>This process draws feasible contact forces when it finds some, otherwise it
colors the background in red to show that the motion is not dynamically
feasible. This is what happens most of the time with our controller at this
point.</p>
</div>
</div>
<div class="section" id="swing-foot-interpolation">
<h2>Swing foot interpolation</h2>
<p>Our fixed footstep sequence provides initial and target configurations of the
swing foot for each SSP. For this first prototype, let us implement a "flat
foot" walk where the swing foot stays parallel to the ground. This strategy is
easier to implement, but on real robots it has a tendency to yield early
contacts and larger impacts than the heel-strike toe-off walk commonly applied
by humans.</p>
<p>The orientation of the swing foot being given, the main parameter to tune is
the vertical clearance of the trajectory. The default <a class="reference external" href="https://scaron.info/doc/pymanoid/walking-pattern-generation.html#pymanoid.swing_foot.SwingFoot">pymanoid.SwingFoot</a>
interpolates a <a class="reference external" href="https://en.wikipedia.org/wiki/Cubic_Hermite_spline">cubic Hermite spline</a> parameterized by a
takeoff clearance height <span class="math">\(a\)</span> and a landing clearance height <span class="math">\(b\)</span>:</p>
<img alt="Swing foot trajectory interpolation" class="center max-height-200px" src="https://scaron.info/images/swing-foot.png" />
<p>For us the ground contact normal <span class="math">\(\bfn\)</span> coincides with the world vertical
<span class="math">\(\bfe_z\)</span> in the figure above. The swing foot interpolator computes a
polynomial <span class="math">\(P(t)\)</span> such that:</p>
<ul class="simple">
<li><span class="math">\(P(0)\)</span> is the initial foot position,</li>
<li><span class="math">\(P(T_\textit{SSP})\)</span> is the target foot position, with <span class="math">\(T_\textit{SSP}\)</span> the duration of the single support phase,</li>
<li><span class="math">\(P(\frac{1}{4} T_\textit{SSP})\)</span> is at height greater than <span class="math">\(a\)</span> from the initial contact, and</li>
<li><span class="math">\(P(\frac{3}{4} T_\textit{SSP})\)</span> is at height greater than <span class="math">\(b\)</span> from the target contact.</li>
</ul>
<p>We can choose <span class="math">\(a = b = 5~\text{cm}\)</span> so that the foot detaches neatly from
the ground without causing too much vertical height variations of the COM.
First, let us instantiate the interpolator by:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_start</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">pose</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_interp</span> <span class="o">=</span> <span class="n">SwingFoot</span><span class="p">(</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="p">,</span> <span class="n">ssp_duration</span><span class="p">,</span>
<span class="n">takeoff_clearance</span><span class="o">=</span><span class="mf">0.05</span><span class="p">,</span> <span class="n">landing_clearance</span><span class="o">=</span><span class="mf">0.05</span><span class="p">)</span>
</pre>
<p>Second, we replace the dummy linear interpolation by the output from the swing
foot interpolator:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">run_swing_foot</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="n">new_pose</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_interp</span><span class="o">.</span><span class="n">integrate</span><span class="p">(</span><span class="n">dt</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">swing_foot</span><span class="o">.</span><span class="n">set_pose</span><span class="p">(</span><span class="n">new_pose</span><span class="p">)</span>
</pre>
<img alt="Now the legged robot swings its feet properly between footsteps" class="center max-height-400px" src="https://scaron.info/figures/walkgen-swing.png" />
<p>Now the robot properly lifts its feet in a motion that is kinematically but
still not dynamically consistent.</p>
</div>
<div class="section" id="center-of-mass-trajectory">
<h2>Center of mass trajectory</h2>
<p>The main challenge in walking trajectory generation is to make the motion
dynamically consistent, that is, to make sure that the <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html">equations of motion</a></p>
<div class="math">
\begin{equation*}
\bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau +
\bftau_g(\bfq) + \sum_{i=1}^N \calJ_{C_i}^{\top} \bfw_{C_i}
\end{equation*}
</div>
<p>have physically feasible joint torques <span class="math">\(\bftau\)</span> and contact wrenches
<span class="math">\(\bfw_{C_i} = [\bftau_{C_i}\ \bff_i]\)</span>.</p>
<div class="section" id="linear-inverted-pendulum-mode">
<h3>Linear inverted pendulum mode</h3>
<p>Assuming that the robot is powerful enough to execute the walking motion, we
can omit the lines that contain <span class="math">\(\bftau\)</span> and focus on the <a class="reference external" href="https://scaron.info/robot-locomotion/newton-euler-equations.html">Newton-Euler
equations</a> that govern the
floating-base dynamics of the robot:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
m \bfpdd_G & = & m \bfg + \sum_i \bff_i \\
\dot{\bfL}_G & = & \sum_i (\bfp_{C_i} - \bfp_G) \times \bff_i + \bftau_{C_i}
\end{array}
\end{equation*}
</div>
<p>For this first walking trajectory generator, we won't use rotations of the
upper-body and focus on producing the linear horizontal motion of the center of
mass. We rely for this purpose on the <a class="reference external" href="https://scaron.info/robot-locomotion/linear-inverted-pendulum-model.html">linear inverted pendulum model</a>, keeping the COM at a constant
height <span class="math">\(h\)</span> as well as a constant angular momentum <span class="math">\(\bfL_G =
\boldsymbol{0}\)</span>. As a consequence, the Euler equation reduces to having the
resultant of contact forces going from the <a class="reference external" href="https://scaron.info/robot-locomotion/zero-tilting-moment-point.html">zero-tilting moment point (ZMP)</a> to the COM, and the equation of
motion becomes:</p>
<div class="math">
\begin{equation*}
\bfpdd_G = \frac{g}{h} (\bfp_G - \bfp_Z)
\end{equation*}
</div>
<p>where <span class="math">\(Z\)</span> denotes the ZMP.</p>
</div>
<div class="section" id="feasibility-of-contact-forces">
<h3>Feasibility of contact forces</h3>
<p>We want to make sure that, at every time instant, there exists feasible contact
wrenches <span class="math">\(\bfw_{C_i} = [\bftau_{C_i}\ \bff_i]\)</span> that lie in their <a class="reference external" href="https://scaron.info/robot-locomotion/wrench-friction-cones.html">wrench
friction cone</a>. In the linear inverted
pendulum mode, this is equivalent to having the ZMP inside its <a class="reference external" href="/publications/tro-2016.html">ZMP support
area</a>. This area is a convex polytope projection
in general, fortunately when walking on a flat floor with large friction it
reduces to the convex hull <span class="math">\(\cal C\)</span> of ground contact points:</p>
<div class="math">
\begin{equation*}
\bfp_Z = \bfp_G - \frac{h}{g} \bfpdd_G \in {\cal C}
\end{equation*}
</div>
</div>
<div class="section" id="linear-model-predictive-control">
<h3>Linear model predictive control</h3>
<p>Walking involves falling forward during single support phases, which is
instantaneously unstable and only becomes "stable" when considering the future
of the system (for instance, the next double support phase where the biped may
slow down to a stop if needed). As such, walking trajectory generation is
commonly solved by <em>model predictive control</em> (MPC) where the COM trajectory is
computed over a <em>preview horizon</em> of one or two steps. This approach was
initially known as <a class="reference external" href="https://doai.io/10.1109/ROBOT.2003.1241826">preview control</a> and reformulated with inequality
constraints as <a class="reference external" href="https://doai.io/10.1109/ICHR.2006.321375">linear model predictive control</a>.</p>
<p>Let us focus on lateral COM motions (sagittal motions can be derived
similarly), and define the <em>state</em> of our system as <span class="math">\(\bfx = [y_G\
\dot{y}_G\ \ddot{y}_G]\)</span>. The MPC problem is then to generate a trajectory
<span class="math">\(y_G(t)\)</span> such that:</p>
<ul class="simple">
<li>at the current time <span class="math">\(t_0\)</span>, <span class="math">\(\bfx(t_0)\)</span> is equal to the current
state of the COM <span class="math">\([y_G(t_0)\ \dot{y}_G(t_0)\ \ddot{y}_G(t_0)]\)</span>;</li>
<li>at all times, the ZMP <span class="math">\(y_\textit{min}(t) \leq y_Z(\bfx(t)) \leq
y_\textit{max}(t)\)</span> where <span class="math">\(y_\textit{min}(t)\)</span> and
<span class="math">\(y_\textit{max}(t)\)</span> define the edges of the support area at time
<span class="math">\(t\)</span>;</li>
<li>at the end of the preview horizon <span class="math">\(t_f\)</span>, <span class="math">\(\bfx(t_f)\)</span> is equal to
a desired terminal state, for instance <span class="math">\([y_f\ 0\ 0]\)</span> where the robot
has stopped walking over the last footstep.</li>
</ul>
<p>We define <em>control</em> input as the COM jerk <span class="math">\(u = \dddot{y}_G\)</span>, and
discretize our system as:</p>
<div class="math">
\begin{equation*}
\bfx_{k+1} = \bfA \bfx_k + \bfB u_k
\end{equation*}
</div>
<p>where the state matrix <span class="math">\(\bfA\)</span> and control matrix <span class="math">\(\bfB\)</span> are defined
by:</p>
<div class="math">
\begin{equation*}
\bfA = \begin{bmatrix}
1 & T & \frac{T^2}{2} \\
0 & 1 & T \\
0 & 0 & 1
\end{bmatrix}
\quad
\bfB = \begin{bmatrix}
\frac{T^3}{6} \\
\frac{T^2}{2} \\
T
\end{bmatrix}
\end{equation*}
</div>
<p>The ZMP is obtained from the state as:</p>
<div class="math">
\begin{equation*}
y_Z(x_k) = \begin{bmatrix} 1 & 0 & -\frac{h}{g} \end{bmatrix} \bfx_k
\end{equation*}
</div>
<p>At every discretized time <span class="math">\(t_k\)</span>, we want the ZMP <span class="math">\(y_Z\)</span> to lie
between <span class="math">\(y_{\textit{min},k}\)</span> and <span class="math">\(y_{\textit{max},k}\)</span>. This can be
written as a linear matrix inequality on the state:</p>
<div class="math">
\begin{equation*}
\bfC \bfx_k \leq \bfe_k
\end{equation*}
</div>
<p>where:</p>
<div class="math">
\begin{equation*}
\bfC = \begin{bmatrix}
+1 & 0 & -\frac{h}{g} \\
-1 & 0 & +\frac{h}{g}
\end{bmatrix}
\quad
\bfe_k = \begin{bmatrix}
+y_{\textit{max},k} \\
-y_{\textit{min},k}
\end{bmatrix}
\end{equation*}
</div>
<p>We now have all the ingredients to build a linear model predictive control
problem. The discretized time step of our problem is <span class="math">\(T = 90\)</span> ms, and our
preview window will have 16 such steps:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">update_mpc</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">dsp_duration</span><span class="p">,</span> <span class="n">ssp_duration</span><span class="p">):</span>
<span class="n">nb_preview_steps</span> <span class="o">=</span> <span class="mi">16</span>
<span class="n">T</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">mpc_timestep</span>
<span class="n">nb_init_dsp_steps</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="nb">round</span><span class="p">(</span><span class="n">dsp_duration</span> <span class="o">/</span> <span class="n">T</span><span class="p">))</span>
<span class="n">nb_init_ssp_steps</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="nb">round</span><span class="p">(</span><span class="n">ssp_duration</span> <span class="o">/</span> <span class="n">T</span><span class="p">))</span>
<span class="n">nb_dsp_steps</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="nb">round</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">dsp_duration</span> <span class="o">/</span> <span class="n">T</span><span class="p">))</span>
</pre>
<p>Our preview consists of an initial DSP of duration <tt class="docutils literal">dsp_duration</tt> (can be
zero), followed by an SSP of duration <tt class="docutils literal">ssp_duration</tt>, followed by a DSP of
regular duration <tt class="docutils literal">self.dsp_duration</tt>, followed by a second SSP lasting until
the preview horizon. We build the matrices of the problem following the
derivation above:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="c1"># def update_mpc(self, dsp_duration, ssp_duration):</span>
<span class="n">A</span> <span class="o">=</span> <span class="n">array</span><span class="p">([[</span><span class="mf">1.</span><span class="p">,</span> <span class="n">T</span><span class="p">,</span> <span class="n">T</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">/</span> <span class="mf">2.</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">,</span> <span class="n">T</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">]])</span>
<span class="n">B</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="n">T</span> <span class="o">**</span> <span class="mi">3</span> <span class="o">/</span> <span class="mf">6.</span><span class="p">,</span> <span class="n">T</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">/</span> <span class="mf">2.</span><span class="p">,</span> <span class="n">T</span><span class="p">])</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">1</span><span class="p">))</span>
<span class="n">h</span> <span class="o">=</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">z</span>
<span class="n">g</span> <span class="o">=</span> <span class="o">-</span><span class="n">sim</span><span class="o">.</span><span class="n">gravity</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span>
<span class="n">zmp_from_state</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="o">-</span><span class="n">h</span> <span class="o">/</span> <span class="n">g</span><span class="p">])</span>
<span class="n">C</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="n">zmp_from_state</span><span class="p">,</span> <span class="o">-</span><span class="n">zmp_from_state</span><span class="p">])</span>
<span class="n">D</span> <span class="o">=</span> <span class="kc">None</span>
<span class="n">e</span> <span class="o">=</span> <span class="p">[[],</span> <span class="p">[]]</span>
<span class="n">cur_vertices</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">stance_foot</span><span class="o">.</span><span class="n">get_scaled_contact_area</span><span class="p">(</span><span class="mf">0.8</span><span class="p">)</span>
<span class="n">next_vertices</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">get_scaled_contact_area</span><span class="p">(</span><span class="mf">0.8</span><span class="p">)</span>
<span class="k">for</span> <span class="n">coord</span> <span class="ow">in</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]:</span>
<span class="n">cur_max</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">cur_vertices</span><span class="p">)</span>
<span class="n">cur_min</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">cur_vertices</span><span class="p">)</span>
<span class="n">next_max</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">next_vertices</span><span class="p">)</span>
<span class="n">next_min</span> <span class="o">=</span> <span class="nb">min</span><span class="p">(</span><span class="n">v</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="k">for</span> <span class="n">v</span> <span class="ow">in</span> <span class="n">next_vertices</span><span class="p">)</span>
<span class="n">e</span><span class="p">[</span><span class="n">coord</span><span class="p">]</span> <span class="o">=</span> <span class="p">[</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="mf">1000.</span><span class="p">,</span> <span class="o">+</span><span class="mf">1000.</span><span class="p">])</span> <span class="k">if</span> <span class="n">i</span> <span class="o"><</span> <span class="n">nb_init_dsp_steps</span> <span class="k">else</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="n">cur_max</span><span class="p">,</span> <span class="o">-</span><span class="n">cur_min</span><span class="p">])</span> <span class="k">if</span> <span class="n">i</span> <span class="o">-</span> <span class="n">nb_init_dsp_steps</span> <span class="o"><=</span> <span class="n">nb_init_ssp_steps</span> <span class="k">else</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="mf">1000.</span><span class="p">,</span> <span class="o">+</span><span class="mf">1000.</span><span class="p">])</span> <span class="k">if</span> <span class="n">i</span> <span class="o">-</span> <span class="n">nb_init_dsp_steps</span> <span class="o">-</span> <span class="n">nb_init_ssp_steps</span> <span class="o"><</span> <span class="n">nb_dsp_steps</span> <span class="k">else</span>
<span class="n">array</span><span class="p">([</span><span class="o">+</span><span class="n">next_max</span><span class="p">,</span> <span class="o">-</span><span class="n">next_min</span><span class="p">])</span>
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">nb_preview_steps</span><span class="p">)]</span>
</pre>
<p>Here, we construct two lists of vectors <span class="math">\(\bfe = [\bfe_0\ \bfe_1\ \ldots]\)</span>
in parallel: <tt class="docutils literal">e[0]</tt> for the sagittal direction and <tt class="docutils literal">e[1]</tt> for the lateral
direction. We put constraints on the ZMP during single support phases based on
the vertices of the support area (the area is scaled by <tt class="docutils literal">0.9</tt> to make sure
that the ZMP stays well inside). Finally, we build and solve MPC problems for
both directions using the <a class="reference external" href="https://scaron.info/doc/pymanoid/walking-pattern-generation.html#pymanoid.mpc.LinearPredictiveControl">pymanoid.LinearPredictiveControl class</a>:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="c1"># def update_mpc(self, dsp_duration, ssp_duration):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">x_mpc</span> <span class="o">=</span> <span class="n">LinearPredictiveControl</span><span class="p">(</span>
<span class="n">A</span><span class="p">,</span> <span class="n">B</span><span class="p">,</span> <span class="n">C</span><span class="p">,</span> <span class="n">D</span><span class="p">,</span> <span class="n">e</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>
<span class="n">x_init</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">xd</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">xdd</span><span class="p">]),</span>
<span class="n">x_goal</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">]),</span>
<span class="n">nb_steps</span><span class="o">=</span><span class="n">nb_preview_steps</span><span class="p">,</span>
<span class="n">wxt</span><span class="o">=</span><span class="mf">1.</span><span class="p">,</span> <span class="n">wu</span><span class="o">=</span><span class="mf">0.01</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">y_mpc</span> <span class="o">=</span> <span class="n">LinearPredictiveControl</span><span class="p">(</span>
<span class="n">A</span><span class="p">,</span> <span class="n">B</span><span class="p">,</span> <span class="n">C</span><span class="p">,</span> <span class="n">D</span><span class="p">,</span> <span class="n">e</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
<span class="n">x_init</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">yd</span><span class="p">,</span> <span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">ydd</span><span class="p">]),</span>
<span class="n">x_goal</span><span class="o">=</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">swing_target</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">]),</span>
<span class="n">nb_steps</span><span class="o">=</span><span class="n">nb_preview_steps</span><span class="p">,</span>
<span class="n">wxt</span><span class="o">=</span><span class="mf">1.</span><span class="p">,</span> <span class="n">wu</span><span class="o">=</span><span class="mf">0.01</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">x_mpc</span><span class="o">.</span><span class="n">solve</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">y_mpc</span><span class="o">.</span><span class="n">solve</span><span class="p">()</span>
<span class="bp">self</span><span class="o">.</span><span class="n">preview_time</span> <span class="o">=</span> <span class="mf">0.</span>
</pre>
<p>We use this <tt class="docutils literal">update_mpc()</tt> function to initialize these problems at the
beginning of double and single support phases:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">start_com_mpc_dsp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span><span class="p">)</span>
<span class="k">def</span> <span class="nf">start_com_mpc_ssp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">)</span>
</pre>
<p>Finally, in the <tt class="docutils literal">run_com_mpc()</tt> function we update the COM by integrating
jerk outputs from model predictive control:</p>
<pre class="code python literal-block">
<span class="c1"># class WalkingFSM(pymanoid.Process):</span>
<span class="k">def</span> <span class="nf">run_com_mpc</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">preview_time</span> <span class="o">>=</span> <span class="bp">self</span><span class="o">.</span><span class="n">mpc_timestep</span><span class="p">:</span>
<span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">state</span> <span class="o">==</span> <span class="s2">"DoubleSupport"</span><span class="p">:</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">ssp_duration</span><span class="p">)</span>
<span class="k">else</span><span class="p">:</span> <span class="c1"># self.state == "SingleSupport":</span>
<span class="bp">self</span><span class="o">.</span><span class="n">update_mpc</span><span class="p">(</span><span class="mf">0.</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rem_time</span><span class="p">)</span>
<span class="n">com_jerk</span> <span class="o">=</span> <span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">x_mpc</span><span class="o">.</span><span class="n">U</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">y_mpc</span><span class="o">.</span><span class="n">U</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="mf">0.</span><span class="p">])</span>
<span class="n">stance</span><span class="o">.</span><span class="n">com</span><span class="o">.</span><span class="n">integrate_constant_jerk</span><span class="p">(</span><span class="n">com_jerk</span><span class="p">,</span> <span class="n">dt</span><span class="p">)</span>
<span class="bp">self</span><span class="o">.</span><span class="n">preview_time</span> <span class="o">+=</span> <span class="n">dt</span>
</pre>
<p>The <tt class="docutils literal">preview_time</tt> variable is used to compute new predictive solutions after
we are done integrating their first timestep. This is one of the key concepts
in MPC: we only execute the first control of the output trajectory, and
recompute that trajectory when this is done.</p>
<p>Dynamic constraints on the ZMP naturally produce a center of mass motion that
sways laterally:</p>
<img alt="Humanoid model JVRC-1 walks dynamically" class="center max-height-400px" src="https://scaron.info/figures/walkgen-com.png" />
<p>Now our robot model is properly walking with a motion that is both
kinematically and dynamically consistent. You can check out the complete
walking controller in the <a class="reference external" href="https://github.com/stephane-caron/pymanoid/blob/18b75807d679630752ea038536226fd46d6e8691/examples/horizontal_walking.py">horizontal_walking.py</a>
example in pymanoid.</p>
</div>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>The concepts we have seen in this introduction are used in a <a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/">full-fledged
walking controller</a> developed for
walking and stair climbing with the HRP-4 humanoid.</p>
<p>The model predictive control problem we solved here to generate center of mass
trajectories was 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 trajectory generation method in the
<a class="reference external" href="https://github.com/stephane-caron/lipm_walking_controller/blob/58c08d1cb36697cc487de620beb6efe59d40857f/src/ModelPredictiveControl.cpp">ModelPredictiveControl.cpp</a>
source of the controller: it adds general footstep orientations, velocity and
jerk terms to the cost function, as well as a (strict) terminal condition on a
<a class="reference external" href="https://scaron.info/talks/jrl-2019.html">divergent component of motion</a>. Those are
implemented using the <a class="reference external" href="https://github.com/vsamy/copra/">Copra</a> linear MPC
library.</p>
<p>There are two ways model predictive control has been applied to legged
locomotion so far: open-loop and closed-loop MPC. In open-loop MPC is a motion
planning approach where MPC outputs at the current step are fed to an integrator, which in turn determines MPC inputs for the next step. the plan is "unrolled" progressively</p>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>Floating base estimation2019-04-13T00:00:00+02:002019-04-13T00:00:00+02:00Stéphane Carontag:scaron.info,2019-04-13:/robot-locomotion/floating-base-estimation.html<p>For a legged system, locomotion is about controlling the <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#case-of-mobile-robots">unactuated floating
base</a> to a desired
location in the world. Before a control action can be applied, the first thing
is to estimate the position and orientation of this floating base. In what
follows, we will see the simple <em>anchor point …</em></p><p>For a legged system, locomotion is about controlling the <a class="reference external" href="https://scaron.info/robot-locomotion/equations-of-motion.html#case-of-mobile-robots">unactuated floating
base</a> to a desired
location in the world. Before a control action can be applied, the first thing
is to estimate the position and orientation of this floating base. In what
follows, we will see the simple <em>anchor point</em> solution, which is a good way to
get started and works well in practice. For example, it is the one used in
<a class="reference external" href="https://www.youtube.com/watch?v=vFCFKAunsYM&t=22">this stair climbing experiment</a>.</p>
<div class="section" id="orientation-of-the-floating-base">
<h2>Orientation of the floating base</h2>
<p>We assume that our legged robot is equipped with an inertial measurement unit
(IMU) in its pelvis link, and attach the base frame <span class="math">\(B\)</span> of our floating
base to the same link. The IMU contains gyroscopes, providing angular velocity
measurements <span class="math">\({}^B\bfomega_B\)</span>, and accelerometers, providing linear
acceleration measurements <span class="math">\({}^B\bfa\)</span> that include both gravity and linear
accelerations caused by robot motions. These measurements can be used to
estimate the orientation <span class="math">\({}^W\bfR_B\)</span> of the floating base with respect
to an inertial world frame <span class="math">\(W\)</span>, but only partially. Let us outline how
this process works.</p>
<div class="section" id="integration-of-angular-velocities">
<h3>Integration of angular velocities</h3>
<p>Define the world frame as the initial floating base of the system, so that
<span class="math">\(\bfR^i_0 := {}^W\bfR^i_B = \bfI_3\)</span> with <span class="math">\(\bfI_3\)</span> the <span class="math">\(3
\times 3\)</span> identity matrix. At discrete time <span class="math">\(t_k\)</span>, gyroscopes measure the
angular velocity <span class="math">\({}^B \bfomega_B(t_k)\)</span> of the floating base <span class="math">\(B\)</span>
relative to the inertial frame, expressed in <span class="math">\(B\)</span>. The discrete series
<span class="math">\(\bfomega_k := {}^B \bfomega_B(t_k)\)</span> of angular velocity measurements can
be integrated to get:</p>
<div class="math">
\begin{equation*}
\bfR^i_{k+1} = \bfR^i_{k} \exp([\bfomega_k \times] \Delta t)
\end{equation*}
</div>
<p>with <span class="math">\(\Delta t = t_k - t_{k-1}\)</span> the duration between two measurements and
<span class="math">\([\bfomega_k \times]\)</span> the skew-symmetric matrix formed from the angular
velocity vector. In practice the matrix exponential can be expanded by
<a class="reference external" href="https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula">Rodrigues' rotation formula</a>:</p>
<div class="math">
\begin{equation*}
\bfR^i_{k+1} = \bfR^i_{k} \left(\bfI_3 + \frac{\sin \theta_k}{\theta_k} [\bfomega_k \times] + \frac{1 - \cos \theta_k}{\theta_k^2} [\bfomega_k \times]^2 \right)
\end{equation*}
</div>
<p>with <span class="math">\(\theta_k = \| \omega_k \| \Delta t\)</span>. This integration scheme gives
us a first estimate of the orientation of the floating base over time, however
it quickly drifts in practice because gyroscopes have a (time-varying)
measurement bias.</p>
</div>
<div class="section" id="accelerometer-based-estimation">
<h3>Accelerometer-based estimation</h3>
<p>Accelerometers can be used to provide an alternative estimate of the floating
base orientation. When the robot is not moving, what these accelerometers see
on average is the gravity vector <span class="math">\({}^B\bfg\)</span> expressed in <span class="math">\(B\)</span>. As
the gravity vector <span class="math">\({}^W\bfg \approx [0 \ 0 \ {-9.81}]\)</span> in the inertial
frame is known, we can estimate <span class="math">\(\bfR^g =
{}^W \bfR_B\)</span> as <a class="reference external" href="https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/476311#476311">the rotation matrix that sends</a>
<span class="math">\({}^B \bfg\)</span> onto <span class="math">\({}^W \bfg\)</span>. Let us denote by <span class="math">\({}^B \bfu\)</span> and
<span class="math">\({}^W \bfu\)</span> the normalized vectors from <span class="math">\({}^B \bfg\)</span> and <span class="math">\({}^W
\bfg\)</span> respectively:</p>
<div class="math">
\begin{equation*}
\begin{array}{rcl}
\bfv & = & {}^B \bfu \times {}^W \bfu \\
c & = & {}^B \bfu \cdot {}^W \bfu \\
\bfR^g & = & \bfI_3 + [\bfv \times] + \frac{1}{1 + c} [\bfv \times]^2
\end{array}
\end{equation*}
</div>
<p>This formula performs well to estimate the <em>roll</em> and <em>pitch</em> angles of the
rotation <span class="math">\({}^W \bfR_B\)</span> of the floating base, however it cannot estimate
the third <em>yaw</em> component because the gravity vector is invariant by rotation
around the yaw axis. Basically, at our levels of perception, gravity is felt
the same whether you face north, east or south.</p>
</div>
<div class="section" id="drift-compensation-of-the-gyroscope-bias">
<h3>Drift compensation of the gyroscope bias</h3>
<p>The drawbacks of these two estimates fortunately appear at different
frequencies: angular-velocity integration drifts in the long run,
accelerometer-based estimation is perturbed in the short run as the robot
moves. A common solution is then to rely mostly on the former, compensating its
drift by low-frequency feedback of the latter. On the HRP-4 humanoid, this
solution is implemented in an extended Kalman filter that runs on the robot's
embedded computer. It can also be realized by a complementary filter such as
<a class="reference external" href="https://forums.parallax.com/uploads/attachments/41167/106661.pdf">(Madgwick, 2010)</a>, which
provides remarkable C source code in its Appendix.</p>
</div>
</div>
<div class="section" id="translation-of-the-floating-base">
<h2>Translation of the floating base</h2>
<p>Now that we have estimated the orientation <span class="math">\({}^W\bfR_B\)</span> of the floating
base with respect to the world frame, let us estimate its translation
<span class="math">\({}^W \bfp_B\)</span> using the anchor point strategy. Its idea is summed up in
the following figure:</p>
<img alt="Illustration of anchor-point floating-base translation estimation" class="align-center max-height-500px" src="https://scaron.info/figures/anchor-point.png" />
<p>We select an anchor point <span class="math">\(A\)</span> at a fixed location on the foot sole, for
instance the point closest to the ankle frame (a pertinent choice for Honda and
HRP robots to limit deflection errors caused by their rubber foot flexibility),
and assume that this point is in contact at the position <span class="math">\({}^W \bfp_A\)</span> on
the ground where it is planned to be in the world frame. In the floating-base
frame, the position <span class="math">\({}^B \bfp_A\)</span> of this point is known by forward
kinematics. From the anchor-point assumption, we can then compute the
floating-base translation by:</p>
<div class="math">
\begin{equation*}
{}^W \bfp_A = {}^W \bfp_B + {}^W \bfR_B {}^B \bfp_A \ \Longrightarrow
\ {}^W \bfp_B = {}^W \bfp_A - {}^W \bfR_B {}^B \bfp_A
\end{equation*}
</div>
<p>This estimator is simple to implement and does not require additional
parameters. While walking, the anchor point can be chosen (1) closest to the
ankle frame during single-support phases and (2) continuously varying from one
ankle frame to the other during double-support phases. Continuous variations
help avoid discontinuities, which are generally undesirable in a controller.</p>
<p>While simple to implement, this estimator performs quite well in practice. Here
is for example a comparison to more precise VICON measurements ran while HRP-4
was stepping in place:</p>
<img alt="Precision of the observer compared to more precise VICON measurements" class="align-center max-height-450px" src="https://scaron.info/figures/floating-base-vicon.png" />
<p>In this example, the estimation error in the lateral direction was at most 0.8
cm.</p>
</div>
<div class="section" id="divergent-component-of-motion">
<h2>Divergent component of motion</h2>
<p>For <a class="reference external" href="https://scaron.info/robot-locomotion/how-do-biped-robots-walk.html#walking-stabilization">walking stabilization</a>, the
controller may need to estimate not only the floating base position but also
the divergent component of motion (DCM) <span class="math">\(\bfxi\)</span>, which depends on the
center of mass (CoM) position <span class="math">\(\bfc\)</span> and velocity <span class="math">\(\bfcd\)</span> by:</p>
<div class="math">
\begin{equation*}
\bfxi = \bfc + \frac{\bfcd}{\omega}
\end{equation*}
</div>
<p>where <span class="math">\(\omega = \sqrt{g / h}\)</span> is a known constant. For a straightforward
solution, the CoM position can be derived from the floating base and joint
encoder measurements <a class="reference external" href="#comment-reply-center-of-mass">by forward kinematics</a>.
Its velocity is then obtained via a first-order low-pass filter with cutoff
period <span class="math">\(T_\textit{cutoff} = 10~\text{ms}\)</span>:</p>
<div class="math">
\begin{equation*}
\alpha = \frac{\Delta t}{T_\textit{cutoff}}
\ \Longrightarrow \
\bfcd_{k+1} = \alpha \frac{\bfc_{k+1} - \bfc_k}{\Delta t} + (1 - \alpha) \bfcd_k
\end{equation*}
</div>
<p>where <span class="math">\(\Delta t\)</span> is the duration of a control cycle. The DCM is finally
computed from the CoM position <span class="math">\(\bfc_k\)</span> and velocity <span class="math">\(\bfcd_k\)</span>.
While simple to implement, this scheme is the one we applied in a <a class="reference external" href="/publications/icra-2019.html">walking and
stair climbing controller</a> for the HRP-4
humanoid.</p>
</div>
<div class="section" id="to-go-further">
<h2>To go further</h2>
<p>Estimating the floating base orientation from IMU measurements is a key
component in the above process, of which we only gave the outline. To go
further, you can take a look at Sections III and V of <a class="reference external" href="http://rpg.ifi.uzh.ch/docs/TRO16_forster.pdf">(Forster et al., 2016)</a>.</p>
<div class="section" id="kalman-and-complementary-filters">
<h3>Kalman and complementary filters</h3>
<p>There is significant literature on the topic of floating base estimation, and
more generally <em>state estimation</em> for legged robots. One way is to design a
Kalman or complementary filter. The following open source libraries follow this
approach:</p>
<ul class="simple">
<li><a class="reference external" href="https://github.com/ori-drs/pronto-distro">Pronto</a>: an extended Kalman
filter for floating base estimation, used by the MIT DRC team during the
DARPA Robotics Challenge.</li>
<li><a class="reference external" href="https://github.com/mrsp/serow">SEROW</a>: a Kalman filter for floating base
and CoM position and velocity estimation.</li>
<li><a class="reference external" href="https://github.com/mehdi-benallegue/state-observation">state-observation</a>:
a collection of Kalman filters based on different contact assumptions.</li>
</ul>
</div>
<div class="section" id="numerical-optimization">
<h3>Numerical optimization</h3>
<p>Another approach relies on <a class="reference external" href="https://doi.org/10.1109/IROS.2014.6942679">quadratic programming to minimize model and
measurement errors</a>. Quadratic
programming can also be used to <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01574819/document">fuse simple estimators with time-varying
weights</a>. The
<a class="reference external" href="https://github.com/jrl-umi3218/SpringEstimator">SpringEstimator</a> library
follows this approach. It applies a nonlinear hierarchical optimization that
can estimate multiple compliant links in contact. Technical details are
provided <a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01276931/document">around Fig. 8 of this paper</a>.</p>
</div>
</div>
<script type='text/javascript'>if (!document.getElementById('mathjaxscript_pelican_#%@#$@#')) {
var align = "center",
indent = "0em",
linebreak = "false";
if (false) {
align = (screen.width < 768) ? "left" : align;
indent = (screen.width < 768) ? "0em" : indent;
linebreak = (screen.width < 768) ? 'true' : linebreak;
}
var mathjaxscript = document.createElement('script');
mathjaxscript.id = 'mathjaxscript_pelican_#%@#$@#';
mathjaxscript.type = 'text/javascript';
mathjaxscript.src = '/MathJax/MathJax.js?config=TeX-AMS-MML_HTMLorMML';
var configscript = document.createElement('script');
configscript.type = 'text/x-mathjax-config';
configscript[(window.opera ? "innerHTML" : "text")] =
"MathJax.Hub.Config({" +
" config: ['MMLorHTML.js']," +
" TeX: { extensions: ['AMSmath.js','AMSsymbols.js','noErrors.js','noUndefined.js'], equationNumbers: { autoNumber: 'none' } }," +
" jax: ['input/TeX','input/MathML','output/HTML-CSS']," +
" extensions: ['tex2jax.js','mml2jax.js','MathMenu.js','MathZoom.js']," +
" displayAlign: '"+ align +"'," +
" displayIndent: '"+ indent +"'," +
" showMathMenu: true," +
" messageStyle: 'normal'," +
" tex2jax: { " +
" inlineMath: [ ['\\\\(','\\\\)'] ], " +
" displayMath: [ ['$$','$$'] ]," +
" processEscapes: true," +
" preview: 'TeX'," +
" }, " +
" 'HTML-CSS': { " +
" availableFonts: ['STIX', 'TeX']," +
" preferredFont: 'STIX'," +
" styles: { '.MathJax_Display, .MathJax .mo, .MathJax .mi, .MathJax .mn': {color: '#333 ! important'} }," +
" linebreaks: { automatic: "+ linebreak +", width: '90% container' }," +
" }, " +
" 'TeX': { " +
" Macros: { " +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{=}}\"," +
" Ld: \"{\\\\dot{L}}\"," +
" LdG: \"{\\\\dot{L}_G}\"," +
" bfA: \"{\\\\boldsymbol{A}}\"," +
" bfB: \"{\\\\boldsymbol{B}}\"," +
" bfC: \"{\\\\boldsymbol{C}}\"," +
" bfD: \"{\\\\boldsymbol{D}}\"," +
" bfE: \"{\\\\boldsymbol{E}}\"," +
" bfF: \"{\\\\boldsymbol{F}}\"," +
" bfG: \"{\\\\boldsymbol{G}}\"," +
" bfH: \"{\\\\boldsymbol{H}}\"," +
" bfI: \"{\\\\boldsymbol{I}}\"," +
" bfJ: \"{\\\\boldsymbol{J}}\"," +
" bfK: \"{\\\\boldsymbol{K}}\"," +
" bfL: \"{\\\\boldsymbol{L}}\"," +
" bfM: \"{\\\\boldsymbol{M}}\"," +
" bfN: \"{\\\\boldsymbol{N}}\"," +
" bfO: \"{\\\\boldsymbol{O}}\"," +
" bfP: \"{\\\\boldsymbol{P}}\"," +
" bfQ: \"{\\\\boldsymbol{Q}}\"," +
" bfR: \"{\\\\boldsymbol{R}}\"," +
" bfS: \"{\\\\boldsymbol{S}}\"," +
" bfT: \"{\\\\boldsymbol{T}}\"," +
" bfU: \"{\\\\boldsymbol{U}}\"," +
" bfV: \"{\\\\boldsymbol{V}}\"," +
" bfW: \"{\\\\boldsymbol{W}}\"," +
" bfX: \"{\\\\boldsymbol{X}}\"," +
" bfY: \"{\\\\boldsymbol{Y}}\"," +
" bfZ: \"{\\\\boldsymbol{Z}}\"," +
" bfa: \"{\\\\boldsymbol{a}}\"," +
" bfb: \"{\\\\boldsymbol{b}}\"," +
" bfc: \"{\\\\boldsymbol{c}}\"," +
" bfd: \"{\\\\boldsymbol{d}}\"," +
" bfe: \"{\\\\boldsymbol{e}}\"," +
" bff: \"{\\\\boldsymbol{f}}\"," +
" bfg: \"{\\\\boldsymbol{g}}\"," +
" bfh: \"{\\\\boldsymbol{h}}\"," +
" bfi: \"{\\\\boldsymbol{i}}\"," +
" bfj: \"{\\\\boldsymbol{j}}\"," +
" bfk: \"{\\\\boldsymbol{k}}\"," +
" bfl: \"{\\\\boldsymbol{l}}\"," +
" bfm: \"{\\\\boldsymbol{m}}\"," +
" bfn: \"{\\\\boldsymbol{n}}\"," +
" bfo: \"{\\\\boldsymbol{o}}\"," +
" bfp: \"{\\\\boldsymbol{p}}\"," +
" bfq: \"{\\\\boldsymbol{q}}\"," +
" bfr: \"{\\\\boldsymbol{r}}\"," +
" bfs: \"{\\\\boldsymbol{s}}\"," +
" bft: \"{\\\\boldsymbol{t}}\"," +
" bfu: \"{\\\\boldsymbol{u}}\"," +
" bfv: \"{\\\\boldsymbol{v}}\"," +
" bfw: \"{\\\\boldsymbol{w}}\"," +
" bfx: \"{\\\\boldsymbol{x}}\"," +
" bfy: \"{\\\\boldsymbol{y}}\"," +
" bfz: \"{\\\\boldsymbol{z}}\"," +
" bfalpha: \"{\\\\boldsymbol{\\\\alpha}}\"," +
" bfbeta: \"{\\\\boldsymbol{\\\\beta}}\"," +
" bfchi: \"{\\\\boldsymbol{\\\\chi}}\"," +
" bfgamma: \"{\\\\boldsymbol{\\\\gamma}}\"," +
" bflambda: \"{\\\\boldsymbol{\\\\lambda}}\"," +
" bfomega: \"{\\\\boldsymbol{\\\\omega}}\"," +
" bfone: \"{\\\\boldsymbol{1}}\"," +
" bfsigma: \"{\\\\boldsymbol{\\\\sigma}}\"," +
" bftau: \"{\\\\boldsymbol{\\\\tau}}\"," +
" bfxi: \"{\\\\boldsymbol{\\\\xi}}\"," +
" bfzero: \"{\\\\boldsymbol{0}}\"," +
" calA: \"{\\\\cal A}\"," +
" calB: \"{\\\\cal B}\"," +
" calC: \"{\\\\cal C}\"," +
" calD: \"{\\\\cal D}\"," +
" calE: \"{\\\\cal E}\"," +
" calF: \"{\\\\cal F}\"," +
" calG: \"{\\\\cal G}\"," +
" calH: \"{\\\\cal H}\"," +
" calI: \"{\\\\cal I}\"," +
" calJ: \"{\\\\cal J}\"," +
" calK: \"{\\\\cal K}\"," +
" calL: \"{\\\\cal L}\"," +
" calM: \"{\\\\cal M}\"," +
" calN: \"{\\\\cal N}\"," +
" calO: \"{\\\\cal O}\"," +
" calP: \"{\\\\cal P}\"," +
" calQ: \"{\\\\cal Q}\"," +
" calR: \"{\\\\cal R}\"," +
" calS: \"{\\\\cal S}\"," +
" calT: \"{\\\\cal T}\"," +
" calU: \"{\\\\cal U}\"," +
" calV: \"{\\\\cal V}\"," +
" calW: \"{\\\\cal W}\"," +
" calX: \"{\\\\cal X}\"," +
" calY: \"{\\\\cal Y}\"," +
" calZ: \"{\\\\cal Z}\"," +
" d: [\"{\\\\rm d}{#1}\", 1]," +
" bfcd: \"{\\\\dot{\\\\bfc}}\"," +
" bfpd: \"{\\\\dot{\\\\bfp}}\"," +
" bfpdd: \"{\\\\ddot{\\\\bfp}}\"," +
" dim: \"{\\\\rm dim}\"," +
" p: \"{\\\\boldsymbol{p}}\"," +
" q: \"{\\\\boldsymbol{q}}\"," +
" qd: \"{\\\\dot{\\\\bfq}}\"," +
" qdd: \"{\\\\ddot{\\\\bfq}}\"," +
" xd: \"{\\\\dot{x}}\"," +
" xdd: \"{\\\\ddot{x}}\"," +
" yd: \"{\\\\dot{y}}\"," +
" ydd: \"{\\\\ddot{y}}\"," +
" zd: \"{\\\\dot{z}}\"," +
" zdd: \"{\\\\ddot{z}}\"," +
" defeq: \"{\\\\stackrel{\\\\mathrm{def}}{\\\\ =\\\\ }}\"," +
" } " +
" } " +
"}); " +
"if ('default' !== 'default') {" +
"MathJax.Hub.Register.StartupHook('HTML-CSS Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax['HTML-CSS'].FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"MathJax.Hub.Register.StartupHook('SVG Jax Ready',function () {" +
"var VARIANT = MathJax.OutputJax.SVG.FONTDATA.VARIANT;" +
"VARIANT['normal'].fonts.unshift('MathJax_default');" +
"VARIANT['bold'].fonts.unshift('MathJax_default-bold');" +
"VARIANT['italic'].fonts.unshift('MathJax_default-italic');" +
"VARIANT['-tex-mathit'].fonts.unshift('MathJax_default-italic');" +
"});" +
"}";
(document.body || document.getElementsByTagName('head')[0]).appendChild(configscript);
(document.body || document.getElementsByTagName('head')[0]).appendChild(mathjaxscript);
}
</script>How do biped robots walk?2019-03-27T00:00:00+01:002019-03-27T00:00:00+01:00Stéphane Carontag:scaron.info,2019-03-27:/talks/willow-2019.html<p class="authors">Talk given at the <a class="reference external" href="https://www.di.ens.fr/willow/">Willow team seminar</a>,
Computer Science Department of the École Normale Supérieure de Paris (DI ENS),
on 26 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this introductory talk, we will see the general methods used nowadays to
make humanoid robots walk or climb stairs. Predictive and feedback controls
will be …</p></div><p class="authors">Talk given at the <a class="reference external" href="https://www.di.ens.fr/willow/">Willow team seminar</a>,
Computer Science Department of the École Normale Supérieure de Paris (DI ENS),
on 26 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this introductory talk, we will see the general methods used nowadays to
make humanoid robots walk or climb stairs. Predictive and feedback controls
will be presented from a high-level perspective, with more videos than
equations. At its end, our bird-eye trip will leave us with open questions, for
instance: what is missing in our robots' brains today? The audience is
encouraged to bring lightweight plastic bottles.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/wandercraft-2019.pdf">Slides</a></td>
</tr>
<tr><td><img alt="html" class="icon" src="https://scaron.info/images/icons/html5.png" /></td>
<td><a class="reference external" href="/teaching/how-do-biped-robots-walk.html">Introduction to bipedal walking control</a></td>
</tr>
<tr><td><img alt="youtube" class="icon" src="https://scaron.info/images/icons/youtube.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=T7SEyvPtWFY">Documentary from the NHK on ASIMO</a></td>
</tr>
</tbody>
</table>
</div>
Walking and stair climbing stabilization for position-controlled biped robots2019-03-19T00:00:00+01:002019-03-19T00:00:00+01:00Stéphane Carontag:scaron.info,2019-03-19:/talks/wandercraft-2019.html<p class="authors">Talk given at <a class="reference external" href="https://www.wandercraft.eu/">Wandercraft</a> on 19 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will see the walking and stair climbing stabilizer implemented
on the HRP-4 humanoid robot as part of the final demonstrator for an Airbus
manufacturing use case. Our solution is based on DCM feedback control,
quadratic-programming wrench distribution …</p></div><p class="authors">Talk given at <a class="reference external" href="https://www.wandercraft.eu/">Wandercraft</a> on 19 March 2019.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will see the walking and stair climbing stabilizer implemented
on the HRP-4 humanoid robot as part of the final demonstrator for an Airbus
manufacturing use case. Our solution is based on DCM feedback control,
quadratic-programming wrench distribution and a whole-body admittance
controller that combines three kinds of tasks: single-effector CoP control at
each foot, multi-effector foot pressure control during double-support phases,
and CoM admittance control.</p>
</div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/wandercraft-2019.pdf">Slides</a></td>
</tr>
<tr><td><img alt="youtube" class="icon" src="https://scaron.info/images/icons/youtube.png" /></td>
<td><a class="reference external" href="https://www.youtube.com/watch?v=vFCFKAunsYM">Stair climbing experiment</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Stair climbing stabilization of the HRP-4 humanoid robot using whole-body admittance control</a></td>
</tr>
</tbody>
</table>
</div>
Stair Climbing Stabilization of the HRP-4 Humanoid Robot2018-12-11T00:00:00+01:002018-12-11T00:00:00+01:00Stéphane Carontag:scaron.info,2018-12-11:/talks/jrl-2018.html<p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 11 December 2018 and at the <strong>University of Tokyo</strong> on 14 December 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will discuss stair climbing experiments carried out with the
HRP-4 humanoid as part of an Airbus manufacturing use case. Our solution is …</p></div><p class="authors">Talk given at the <a class="reference external" href="http://jrl-umi3218.github.io/">CNRS-AIST Joint Robotics Laboratory (JRL)</a> on 11 December 2018 and at the <strong>University of Tokyo</strong> on 14 December 2018.</p>
<div class="section" id="abstract">
<h2>Abstract</h2>
<p>In this talk, we will discuss stair climbing experiments carried out with the
HRP-4 humanoid as part of an Airbus manufacturing use case. Our solution is
based on linear inverted pendulum tracking, quadratic-programming wrench
distribution and a whole-body admittance controller that relies on both
end-effector and CoM strategies. After presenting these components, we will
open up the discussion on the interplay between end-effector and CoM
strategies, as well as their potential applications in multi-contact.</p>
<div class="youtube youtube-16x9"><iframe src="https://www.youtube.com/embed/vFCFKAunsYM?start=22" allowfullscreen seamless frameBorder="0"></iframe></div></div>
<div class="section" id="content">
<h2>Content</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://scaron.info/slides/jrl-2018.pdf">Slides</a></td>
</tr>
</tbody>
</table>
</div>
<div class="section" id="references">
<h2>References</h2>
<table border="1" class="colwidths-given files docutils">
<colgroup>
<col width="10%" />
<col width="90%" />
</colgroup>
<tbody valign="top">
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01875387/document">Stair climbing experiments with HRP-4</a></td>
</tr>
<tr><td><img alt="pdf" class="icon" src="https://scaron.info/images/icons/pdf.png" /></td>
<td><a class="reference external" href="https://hal.archives-ouvertes.fr/hal-01689331/document">Walking trajectory generation with height variations</a></td>
</tr>
</tbody>
</table>
</div>
Converting robot models to OpenRAVE2018-11-03T00:00:00+01:002018-11-03T00:00:00+01:00Stéphane Carontag:scaron.info,2018-11-03:/robot-locomotion/converting-robot-models-to-openrave.html<p>OpenRAVE robot models can be written in <a class="reference external" href="http://openrave.programmingvision.com/wiki/index.php/Format:XML">OpenRAVE Custom XML Format</a>, if you
write them from scratch, but are most of the time written in the <a class="reference external" href="http://openrave.org/docs/latest_stable/collada_robot_extensions/">COLLADA 1.5
Robot Extensions</a>. This
format is not so widely used and converting to it can be tricky. In this post,
we will …</p><p>OpenRAVE robot models can be written in <a class="reference external" href="http://openrave.programmingvision.com/wiki/index.php/Format:XML">OpenRAVE Custom XML Format</a>, if you
write them from scratch, but are most of the time written in the <a class="reference external" href="http://openrave.org/docs/latest_stable/collada_robot_extensions/">COLLADA 1.5
Robot Extensions</a>. This
format is not so widely used and converting to it can be tricky. In this post,
we will review the conversion paths I've found to generate the humanoid robot
models in the <a class="reference external" href="https://github.com/stephane-caron/openrave_models">openrave_models</a> repository.</p>
<div class="section" id="from-vrml-to-collada-1-5">
<h2>From VRML to COLLADA 1.5</h2>
<p>The <a class="reference external" href="https://github.com/jvrc/model">JVRC-1 model</a>, similarly to robot models
from the HRP series, is provided in VRML format. To convert it to COLLADA, you
can use the <tt class="docutils literal"><span class="pre">openhrp-export-collada</span></tt> script provided in <a class="reference external" href="https://github.com/fkanehiro/openhrp3">OpenHRP 3</a>. After the regular CMake build
process, you will find it in the <tt class="docutils literal">bin</tt> folder of your build directory:</p>
<pre class="code bash literal-block">
<span class="nb">cd</span> <openhrp3-folder>
./build/bin/openhrp-export-collada -i main.wrl -o JVRC-1.dae
</pre>
<p>Textures are not converted in this process. To palliate this, you can open the
COLLADA file in a text editor (COLLADA is still XML so it can be read and
edited by human beings) and change link colors by hand. You can check out
<a class="reference external" href="https://github.com/stephane-caron/openrave_models/blob/master/JVRC-1/JVRC-1.dae">JVRC-1.dae</a>
to see how this is done.</p>
<img alt="JVRC-1 humanoid model in OpenRAVE" class="noborder gui align-center" src="https://scaron.info/images/openrave/jvrc-1.png" />
<p>More recently, the authors of OpenHRP have also developed <a class="reference external" href="https://github.com/fkanehiro/simtrans">simtrans</a> which is a more general set of
conversion scripts.</p>
</div>
<div class="section" id="from-urdf-to-collada-1-5">
<h2>From URDF to COLLADA 1.5</h2>
<p>Many robot models are provided in another XML dialect called Unified Robot
Description Format (URDF). This was the case for instance with the <a class="reference external" href="https://github.com/ros-aldebaran/romeo_robot">Romeo</a> model. To convert it to
COLLADA, you can use the <tt class="docutils literal">urdf_to_collada</tt> tool provided in <a class="reference external" href="http://wiki.ros.org/collada_urdf">collada_urdf</a>. After the package is installed in your
ROS distribution, run:</p>
<pre class="code bash literal-block">
rosrun collada_urdf urdf_to_collada romeo.urdf Romeo.dae
</pre>
<p>Textures are not converted in this process. To palliate this, you can open the
COLLADA file in a text editor (COLLADA is still XML so it can be read and
edited by human beings) and change link colors by hand. You can check out
<a class="reference external" href="https://github.com/stephane-caron/openrave_models/blob/master/Romeo/Romeo.dae">Romeo.dae</a>
to see how this is done.</p>
<img alt="Romeo humanoid model in OpenRAVE" class="noborder gui align-center" src="https://scaron.info/images/openrave/romeo.png" />
<p>For a more recent tool, the <a class="reference external" href="https://github.com/fkanehiro/simtrans">simtrans</a>
conversion scripts provide a less burdensome alternative</p>
</div>