forked from RussTedrake/underactuated
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathhumanoids.html
451 lines (361 loc) · 23 KB
/
humanoids.html
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
<!DOCTYPE html>
<html>
<head>
<title>Ch. 5 - Highly-articulated Legged
Robots</title>
<meta name="Ch. 5 - Highly-articulated Legged
Robots" content="text/html; charset=utf-8;" />
<link rel="canonical" href="http://underactuated.mit.edu/humanoids.html" />
<script src="https://hypothes.is/embed.js" async></script>
<script type="text/javascript" src="chapters.js"></script>
<script type="text/javascript" src="htmlbook/book.js"></script>
<script src="htmlbook/mathjax-config.js" defer></script>
<script type="text/javascript" id="MathJax-script" defer
src="htmlbook/MathJax/es5/tex-chtml.js">
</script>
<script>window.MathJax || document.write('<script type="text/javascript" src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-chtml.js" defer><\/script>')</script>
<link rel="stylesheet" href="htmlbook/highlight/styles/default.css">
<script src="htmlbook/highlight/highlight.pack.js"></script> <!-- http://highlightjs.readthedocs.io/en/latest/css-classes-reference.html#language-names-and-aliases -->
<script>hljs.initHighlightingOnLoad();</script>
<link rel="stylesheet" type="text/css" href="htmlbook/book.css" />
</head>
<body onload="loadChapter('underactuated');">
<div data-type="titlepage">
<header>
<h1><a href="index.html" style="text-decoration:none;">Underactuated Robotics</a></h1>
<p data-type="subtitle">Algorithms for Walking, Running, Swimming, Flying, and Manipulation</p>
<p style="font-size: 18px;"><a href="http://people.csail.mit.edu/russt/">Russ Tedrake</a></p>
<p style="font-size: 14px; text-align: right;">
© Russ Tedrake, 2022<br/>
Last modified <span id="last_modified"></span>.</br>
<script>
var d = new Date(document.lastModified);
document.getElementById("last_modified").innerHTML = d.getFullYear() + "-" + (d.getMonth()+1) + "-" + d.getDate();</script>
<a href="misc.html">How to cite these notes, use annotations, and give feedback.</a><br/>
</p>
</header>
</div>
<p><b>Note:</b> These are working notes used for <a
href="http://underactuated.csail.mit.edu/Spring2022/">a course being taught
at MIT</a>. They will be updated throughout the Spring 2022 semester. <a
href="https://www.youtube.com/channel/UChfUOAhz7ynELF-s_1LPpWg">Lecture videos are available on YouTube</a>.</p>
<table style="width:100%;"><tr style="width:100%">
<td style="width:33%;text-align:left;"><a class="previous_chapter" href=simple_legs.html>Previous Chapter</a></td>
<td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
<td style="width:33%;text-align:right;"><a class="next_chapter" href=stochastic.html>Next Chapter</a></td>
</tr></table>
<script type="text/javascript">document.write(notebook_header('humanoids'))
</script>
<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 4"><h1>Highly-articulated Legged
Robots</h1>
<p>The passive dynamic walkers and hopping robots described in the last
chapter capture the fundamental dynamics of legged locomotion -- dynamics
which are fundamentally nonlinear and punctuated by impulsive events due to
making and breaking contact with the environment. But if you start reading
the literature on humanoid robots, or many-legged robots like quadrupeds, then
you will find a quite different set of ideas taking center stage: ideas like
the "zero-moment point" and footstep planning. The goal of this chapter is to
penetrate that world.</p>
<p>I'd like to start the discussion with a model that might seem quite far
from the world of legged robots, but I think it's a very useful way to think
about the problem.</p>
<section><h1>Center of Mass Dynamics</h1>
<subsection><h1>A hovercraft model</h1>
<figure><img width="70%" src="figures/hovercraft.svg"/></figure>
<todo>flip the directions of the force vector in the image</todo>
<p>Imagine you have a flying vehicle modeled as a single rigid body in a
gravity field with some number of force "thrusters" attached. We'll
describe the configuration of the vehicle by its orientation, $\theta$ and
the location of its center of mass $(x,z)$. The vector from the center of
mass to thruster $i$ is given by $r_i$, yielding the equations of motion:
\begin{align*} m\ddot{x} =& \sum_i F_{i,x},\\ m\ddot{z} =& \sum_i F_{i,z}
- mg,\\ I\ddot\theta =& \sum_i \left[ r_i \times F_i \right]_y,
\end{align*} where I've used $F_{i,x}$ to represent the component of the
force in the $x$ direction and have taken just the $y$-axis component of
the cross product on the last line. </p>
<p>Our goal is to move the hovercraft to a desired position/orientation
and to keep it there. If we take the inputs to be $F_i$, then the
dynamics are affine (linear plus a constant term). As such, we can
stabilize a stabilizable fixed point using a change of coordinates plus
LQR or even plan/track a desired trajectory using time-varying LQR. If I
were to add additional linear constraints, for instance constraining
$F_{min} \le F_i \le F_{max}$, then I can still use linear
model-predictive control (MPC) to plan and stabilize a desired motion. By
most accounts, this is a relatively easy control problem. (Note that it
would be considerably harder if my control input or input constraints
depend on the orientation, $\theta$, but we don't need that here yet).</p>
<p>Now imagine that you have just a small number of thrusters (let's say
two) each with severe input constraints. To make things more
interesting, let us say that you're allowed to move the thrusters, so
$\dot{r}_i$ becomes an additional control input, but with some important
limitations: you have to turn the thrusters off when they are moving
(e.g. $|F_i||\dot{r}_i| = 0$) and there is a limit to how quickly you can
move the thrusters $|\dot{r}|_i \le v_{max}$. This problem is now a lot
more difficult, due especially to the fact that constraints of the form
$u_1 u_2 = 0$ are non-convex.</p> <center><img width="25%"
src="figures/complementarityConstraint.svg"/></center> <p>I find this a
very useful thought exercise; somehow our controller needs to make an
effectively discrete decision to turn off a thruster and move it. The
framework of optimal control should support this - you are sacrificing a
short-term loss of control authority for the long-term benefit of having
the thruster positioned where you would like, but we haven't developed
tools yet that deal well with this discrete plus continuous decision
making. We'll need to address that here.</p>
<p>Unfortunately, although the problem is already difficult, we need to
continue to add constraints. Now let's say additionally, that the
thrusters can only be turned on in certain locations, as cartooned
here:</p> <center><img width="60%"
src="figures/hovercraftCartesianConstraints.svg"/></center> <p>The
union of these regions need not form a convex set. Furthermore, these
locations could be specified in the robot frame, but may also be specified
in the world frame, which I'll call ${\bf p}_i$: $${\bf p}_i = {\bf r}_i -
\begin{bmatrix} x \\ 0 \\ z \end{bmatrix}.$$ This problem still feels
like it should be tractable, but it's definitely getting hard.</p>
</subsection>
<subsection><h1>Robots with (massless) legs</h1>
<p>In my view, the hovercraft problem above is a central component of the
walking problem. If we consider a walking robot with massless legs, then
the feet are exactly like movable thrusters. As above, they are highly
constrained - they can only produce force when they are touching the
ground, and (typically) they can only produce forces in certain
directions, for instance as described by a "friction cone" (you can push
on the ground but not pull on the ground, and with Coulomb friction the
forces tangential to the ground must be smaller than the forces normal to
the ground, as described by a coefficient of friction, e.g.
$|F_{\parallel}| < \mu |F_{\perp}|$).</p>
<figure><img width="50%" src="figures/hovercraftLegs.svg"/></figure>
<p>The constraints on where you can put your feet / thrusters will depend
on the kinematics of your leg, and the speed at which you can move them
will depend on the full dynamics of the leg -- these are difficult
constraints to deal with. But the actual dynamics of the rigid body are
actually still affine, and still simple!</p>
</subsection>
<subsection><h1>Capturing the full robot dynamics</h1>
<p>We don't actually need to have massless legs for this discussion to
make sense. If we use the coordinates $x,z$ to describe the location of
the center of mass (CoM) of the entire robot, and $m$ to represent the
entire mass of the robot, then the first two equations remain unchanged.
The center of mass is a configuration dependent point, and does not have
an orientation, but one important generalization of the orientation
dynamics is given by the centroidal momentum matrix, $A(\bq)$, where
$A(\bq)\dot{\bq}$ captures the linear and angular momentum of the robot
around the center of mass <elib>Orin13</elib>. Note that the center of
mass dynamics are still affine -- even for a big complicated humanoid --
but the centroidal momentum dynamics are nonlinear.</p>
</subsection>
<subsection><h1>Impact dynamics</h1>
<p>In the previous chapter we devoted relatively a lot of attention to
dynamics of impact, characterized for instance by a guard that resets
dynamics in a hybrid dynamical system. In those models we used impulsive
ground-reaction forces (these forces are instantaneously infinite, but
doing finite work) in order to explain the discontinuous change in
velocity required to avoid penetration with the ground. This story can be
extended naturally to the dynamics of the center of mass.<p>
<p>For an articulated robot, though, there are a number of possible
strategies for the legs which can affect the dynamics of the center of
mass. For example, if the robot hits the ground with a stiff leg like the
rimless wheel, then the angular momentum about the point of collision will
be conserved, but any momentum going into the ground will be lost.
However, if the robot has a spring leg and a massless toe like the SLIP
model, then no energy need be lost.</p>
<p>One strategy that many highly-articulated legged robots use is to keep
their center of mass at a constant height, $$z = c \quad \Rightarrow \quad
\dot{z} = \ddot{z} = 0,$$ and minimize their angular momentum about the
center of mass (here $\ddot\theta=0$). Using this strategy, if the
swinging foot lands roughly below the center of mass, then even with a
stiff leg there is no energy dissipated in the collision - all of the
momentum is conserved. This often (but does not always) justify ignoring
the impacts in the center of mass dynamics of the system.</p>
</subsection>
</section> <!-- end COM dynamics -->
<section><h1>The special case of flat terrain</h1>
<p>While not the only important case, it is extremely common for our robots
to be walking over flat, or nearly flat terrain. In this situation, even if
the robot is touching the ground in a number of places (e.g., two heels and
two toes), the constraints on the center of mass dynamics can be summarized
very efficiently.</p>
<figure>
<img width="80%" src="figures/flat_terrain.svg"/>
<figcaption>External forces acting on a robot pushing on a flat ground</figcaption>
</figure>
<p>First, on flat terrain $F_{i,z}$ represents the force that is normal to
the surface at contact point $i$. If we assume that the robot can only push
on the ground (and not pull), then this implies $$\forall i, F_{i,z} \ge 0
\Rightarrow \sum_i F_{i,z} \ge 0 \Rightarrow \ddot{z} \ge -g.$$ In other
words, if I cannot pull on the ground, then my center of mass cannot
accelerate towards the ground faster than gravity.</p>
<p>Furthermore, if we use a Coulomb model of friction on the ground, with
friction coefficient $\mu$, then $$\forall i, |F_{i,x}| \le \mu F_{i,z}
\Rightarrow \sum_i |F_{i,x}| \le \mu \sum_i F_z \Rightarrow |\ddot{x}| \le
\mu (\ddot{z} + g).$$ For instance, if I keep my center of mass at a
constant height, then $\ddot{z}=0$ and $|\ddot{x}| \le \mu g$; this is a
nice reminder of just how important friction is if you want to be able to
move around in the world.</p>
<p>Even better, let us define the "center of pressure" (CoP) as the point on
the ground where $$x_{cop} = \frac{\sum_i p_{i,x} F_{i,z}}{\sum_i
F_{i,z}},$$ and since all $p_{i,z}$ are equal on flat terrain, $z_{cop}$ is
just the height of the terrain. It turns out that the center of pressure is
a "zero-moment point" (ZMP) -- a property that we will demonstrate below --
and moment-balance equation gives us a very important relationship between
the location of the CoP and the dynamics of the CoM: \[ (m\ddot{z} + mg)
(x_{cop} - x) = (z_{cop} - z) m\ddot{x} - I\ddot\theta. \] If we use the
strategy proposed above for ignoring collision dynamics, $\ddot{z} =
\ddot{\theta} = 0$, then we have $z - z_{cop}$ is a constant height $h$, and
the result is the famous "ZMP equations": \[ \ddot{x} = -\frac{g}{h}
(x_{cop}-x). \] So the location of the center of pressure completely
determines the acceleration of the center of mass, and vice versa! What's
more, this relationship is affine -- a property that we can exploit in a
number of ways. </p>
<p> As an example, we can easily relate constraints on the CoP to
constraints on $\ddot{x}$. It is easy to verify from the definition that the
CoP must live inside the <a
href="http://en.wikipedia.org/wiki/Convex_hull">convex hull</a> of the
ground contact points. Therefore, if we use the $\ddot{z}=\ddot\theta=0$
strategy, then this directly implies strict bounds on the possible
accelerations of the center of mass given the locations of the ground
contacts.</p>
<subsection><h1>An aside: the zero-moment point
derivation</h1>
<p>The zero-moment point (ZMP) is discussed very frequently in the current
literature on legged robots. It also has an unfortunate tendency to be
surrounded by some confusion; a number of people have defined the ZMP is
slightly different ways (see e.g. <elib>Goswami99</elib> for a summary).
Therefore, it makes sense to provide a simple derivation here.</p>
<p>First let us recall that for rigid body systems I can always summarize
the contributions from many external forces as a single <i>wrench</i>
(force and torque) on the object -- this is simply because the $F_i$ terms
enter our equations of motion linearly. Specifically, given any point in
space, $r$, in coordinates relative to $(x,z)$:</p> <figure><img
width="70%" src="figures/zmp_derivation.svg"/></figure> <todo>update
diagram to have r instead of p</todo>
<p>I can re-write the equations of motion as \begin{align*} m\ddot{x} =&
\sum_i F_{i,x} = F_{net,x},\\ m\ddot{z} =& \sum_i F_{i,z} - mg = F_{net,z}
- mg,\\ I\ddot\theta =& \sum_i \left[ r_i \times F_i \right]_y = ({\bf r}
\times {\bf F}_{net})_y + \tau_{net}, \end{align*} where ${\bf F}_{net} =
\sum_i {\bf F}_i$ and the value of $\tau_{net}$ depends on the location
${\bf r}$. For some choices of ${\bf r}$, we can make $\tau_{net}=0$.
Examining \[ ({\bf r} \times {\bf F}_{net})_y = r_z F_{net,x} - r_x
F_{net,z} = \left[ r_i \times F_i \right]_y, \] we can see that when
$F_{net,z} > 0$ there is an entire line of solutions, $r_x = a r_z + b$,
including one that will intercept the terrain. For walking robots, it is
this point on the ground from which the external wrench can be described
by a single force vector (and no moment) that is the famous "zero-moment
point" (ZMP). Substituting the back in to replace $F_{net}$, we can
see that \[ r_x = \frac{r_z m \ddot{x} - I \ddot\theta}{m\ddot{z} + mg}.
\] If we assume that $\ddot{z}=\ddot{\theta}=0$ and replace the relative
coordinates with the global coordinates ${\bf r} = {\bf p} - [x,0,z]^T$,
then we arrive at precisely the equation presented above.</p>
<p>Furthermore, since \[\left[ r_i \times F_i \right]_y = \sum_i \left(
r_{i,z} F_{i,x} - r_{i,x} F_{i,z} \right), \] and for <i>flat terrain</i>
we have \[ r_z F_{net,x} = \sum_i r_{i,z} F_{i,x}, \] then we can see that
this ZMP is exactly the CoP: \[ r_x = \frac{\sum_i r_{i,x} F_{i,z}}{
F_{net,z} }. \] </p>
<p>In three dimensions, we solve for the point on the ground where
$\tau_{net,y} = \tau_{net,x} = 0$, but allow $\tau_{net,z} \ne 0$ to
extract the analogous equations in the $y$-axis: \[ r_y = \frac{r_z m
\ddot{y} - I \ddot\theta}{m\ddot{z} + mg}. \] </p>
</subsection>
</section>
<section><h1>ZMP planning</h1>
<figure><img width="80%" src="figures/zmp_planning.svg"/></figure>
<subsection><h1>From a CoM plan to a whole-body
plan</h1></subsection>
</section>
<section><h1>Whole-Body Control</h1>
<p>Coming soon. For a description of our approach with Atlas, see
<elib>Kuindersma13+Kuindersma14</elib>.</p>
</section>
<section><h1>Footstep planning and push recovery</h1>
<p>Coming soon. For a description of our approach with Atlas, see
<elib>Deits14a+Kuindersma14</elib>. For nice geometric insights on push
recovery, see <elib>Koolen12</elib>.</p>
</section>
<section><h1>Beyond ZMP planning</h1>
<p>Coming soon. For a description of our approach with Atlas, see
<elib>Dai14+Kuindersma14</elib>.</p>
<example><h1>LittleDog gait optimization</h1>
<p><a target="littledog"
href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/examples/littledog.ipynb"><img
src="https://colab.research.google.com/assets/colab-badge.svg"
alt="Open In Colab"/></a>
</p>
</example>
</section>
<section><h1>Exercises</h1>
<exercise><h1>Footstep Planning via Mixed-Integer Optimization</h1>
<p>In this exercise we implement a simplified version of the footstep
planning method proposed in <elib>Deits14a</elib>. You will find all the
details <a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/humanoids/footstep_planning/footstep_planning.ipynb" target="_blank">in this notebook</a>.
Your goal is to code most of the components of the mixed-integer program
at the core of the footstep planner:</p>
<ol type="a">
<li>The constraint that limits the maximum step length.</li>
<li>The constraint for which a foot cannot be in two stepping stones at
the same time.</li>
<li>The constraint that assigns each foot to a stepping stone, for each
step of the robot.</li>
<li>The objective function that minimizes the sum of the squares of the
step lengths.</li>
</ol>
</exercise>
</chapter>
<!-- EVERYTHING BELOW THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<div id="references"><section><h1>References</h1>
<ol>
<li id=Orin13>
<span class="author">David E. Orin and Ambarish Goswami and Sung-Hee Lee</span>,
<span class="title">"Centroidal dynamics of a humanoid robot"</span>,
<span class="publisher">Autonomous Robots</span>, no. September 2012, pp. 1--16, jun, <span class="year">2013</span>.
</li><br>
<li id=Goswami99>
<span class="author">A. Goswami</span>,
<span class="title">"Postural stability of biped robots and the foot rotation indicator ({FRI}) point"</span>,
<span class="publisher">International Journal of Robotics Research</span>, vol. 18, no. 6, <span class="year">1999</span>.
</li><br>
<li id=Kuindersma13>
<span class="author">Scott Kuindersma and Frank Permenter and Russ Tedrake</span>,
<span class="title">"An Efficiently Solvable Quadratic Program for Stabilizing Dynamic Locomotion"</span>,
<span class="publisher">Proceedings of the International Conference on Robotics and Automation</span> , May, <span class="year">2014</span>.
[ <a href="http://groups.csail.mit.edu/robotics-center/public_papers/Kuindersma13.pdf">link</a> ]
</li><br>
<li id=Kuindersma14>
<span class="author">Scott Kuindersma and Robin Deits and Maurice Fallon and Andr\'{e}s Valenzuela and Hongkai Dai and Frank Permenter and Twan Koolen and Pat Marion and Russ Tedrake</span>,
<span class="title">"Optimization-based Locomotion Planning, Estimation, and Control Design for the {A}tlas Humanoid Robot"</span>,
<span class="publisher">Autonomous Robots</span>, vol. 40, no. 3, pp. 429--455, <span class="year">2016</span>.
[ <a href="http://groups.csail.mit.edu/robotics-center/public_papers/Kuindersma14.pdf">link</a> ]
</li><br>
<li id=Deits14a>
<span class="author">Robin Deits and Russ Tedrake</span>,
<span class="title">"Footstep Planning on Uneven Terrain with Mixed-Integer Convex Optimization"</span>,
<span class="publisher">Proceedings of the 2014 IEEE/RAS International Conference on Humanoid Robots (Humanoids 2014)</span> , <span class="year">2014</span>.
[ <a href="http://groups.csail.mit.edu/robotics-center/public_papers/Deits14a.pdf">link</a> ]
</li><br>
<li id=Koolen12>
<span class="author">Twan Koolen and Tomas de Boer and John Rebula and Ambarish Goswami and Jerry Pratt</span>,
<span class="title">"Capturability-based analysis and control of legged locomotion, Part 1: Theory and application to three simple gait models"</span>,
<span class="publisher">The International Journal of Robotics Research</span>, vol. 31, no. 9, pp. 1094-1113, <span class="year">2012</span>.
</li><br>
<li id=Dai14>
<span class="author">Hongkai Dai and Andr\'es Valenzuela and Russ Tedrake</span>,
<span class="title">"Whole-body Motion Planning with Centroidal Dynamics and Full Kinematics"</span>,
<span class="publisher">IEEE-RAS International Conference on Humanoid Robots</span>, <span class="year">2014</span>.
[ <a href="http://groups.csail.mit.edu/robotics-center/public_papers/Dai14.pdf">link</a> ]
</li><br>
</ol>
</section><p/>
</div>
<table style="width:100%;"><tr style="width:100%">
<td style="width:33%;text-align:left;"><a class="previous_chapter" href=simple_legs.html>Previous Chapter</a></td>
<td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
<td style="width:33%;text-align:right;"><a class="next_chapter" href=stochastic.html>Next Chapter</a></td>
</tr></table>
<div id="footer">
<hr>
<table style="width:100%;">
<tr><td><a href="https://accessibility.mit.edu/">Accessibility</a></td><td style="text-align:right">© Russ
Tedrake, 2022</td></tr>
</table>
</div>
</body>
</html>