forked from RussTedrake/underactuated
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathhumanoids.html
595 lines (483 loc) · 29.3 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
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
<!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, 2023<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="https://underactuated.csail.mit.edu/Spring2023/">a course being taught
at MIT</a>. They will be updated throughout the Spring 2023 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>Perhaps one of the most surprising thing about the world of complex
(highly-articulated) legged robots is the dominant use of simple models for
estimation, planning, and control. These abstractions become valuable when
you have enough degrees of freedom to move your feet independently (to an
extent) from how you move your body.</p>
<p>Since our robots are getting more complicated, I'm going to need a bit
stronger notation. In Drake we use <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__notation.html">monogram
notation</a> and define its spatial algebra <a
href="https://manipulation.csail.mit.edu/pick.html#monogram">here for
kinematics</a>, <a
href="https://manipulation.csail.mit.edu/pick.html#jacobian">differential
kinematics (velocities)</a> and <a
href="https://manipulation.csail.mit.edu/clutter.html#spatial_force">here for
force</a>.</p>
<section><h1>A thought experiment</h1>
<p>Let's 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>
<subsection><h1>A spacecraft model</h1>
<figure><img width="70%" src="figures/zmp_thrusters.svg"/></figure>
<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 pose of the body in the world frame, ${}^WX^B$,
parameterized by its position $(x,z)$ and orientation $\theta$, and take
the center of mass to be at the origin of this body frame. The vector
from the center of mass to thruster $i$ in the world frame is given by
${}^Bp^{B_i}_W$, yielding the equations of motion: \begin{align*}
m\ddot{x} =& \sum_i f^{B_i}_{W_x},\\ m\ddot{z} =& \sum_i f^{B_i}_{W_z} -
mg,\\ I\ddot\theta =& \sum_i \left[ {}^Bp^{B_i}_W \times f^{B_i}_W
\right]_y, \end{align*} where I've used $f^{B_i}_{W_x}$ to represent the
$x$-component of the force applied to $B$ at the location of thruster
$i$, expressed in the world frame, and we take the $y$-axis component of
the cross product on the last line.
</p>
<p>Our goal is to move the spacecraft to a desired position/orientation
and to keep it there. If we take the inputs to be $f^{B_i}_W$, 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^{B_i}_W \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
${}^B\text{v}^{B_i} = \frac{d}{dt} {}^Bp^{B_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^{B_i}{}^B_{W_j}
\text{v}^{B_i}_{W_j} = 0$) and there is a limit to how quickly you can
move the thrusters $|{}^B\text{v}^{B_i}_{W_j}| \le \text{v}_{max}$. This
problem is now a lot more difficult, due especially to the fact that
constraints of the form $xy = 0$ are non-convex (the red area in the
illustration below represents the feasible set for $x \ge 0, y \ge
0$).</p>
<center><img width="25%"
src="figures/complementarity_constraint.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 a
few more constraints to make it relevant for legged robots. Now let's
say additionally, that the thrusters can only be turned on in certain
locations, as cartooned here:</p>
<figure id="zmp_thrusters_w_regions"><img width="70%"
src="figures/zmp_thrusters_w_regions.svg"/><figcaption>What if we can
only apply thrust when the thrusters are in a certain (world)
position?</figcaption></figure>
<p>Even if the individual regions are convex, the union of these regions
need not form a convex set. Furthermore, these locations are often
expressed in the world frame, not the robot frame, e.g. $${\bf A}\, {}^W
p^{R_i}_W \le {\bf b}.$$</p>
</subsection>
<subsection><h1>Robots with (massless) legs</h1>
<p>In my view, the spacecraft with thrusters problem above is a central
component of the walking problem. If we consider a walking robot as our
single rigid body, $B$, 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^{B_i}_{C_z} \ge 0, |f^{B_i}_{C_x}|
< \mu f^{B_i}_{C_z}$) where $C$ is the <a
href="multibody.html#contact">contact frame</a>.</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,
$B$, are actually still affine, and still simple!</p>
</subsection>
</section>
<section><h1>Centroidal dynamics</h1>
<p>It turns out the specific equations that we used in our thought
experiment aren't restricted to a single body and massless legs. If we
simply repurpose the coordinates $x,z$ to describe the location of the
center of mass (CoM) of the entire robot (not just the center of mass of a
single body, $B$), 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><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 <i>center of mass</i> at a constant velocity, $$\dot{z} = c \quad
\Rightarrow \quad \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>
<subsection><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^{B_i}_{W_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^{B_i}_{W_z} \ge 0 \Rightarrow \sum_i f^{B_i}_{W_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^{B_i}_{W_x}| \le \mu
f^{B_i}_{W_z} \Rightarrow \sum_i |f^{B_i}_{W_x}| \le \mu \sum_i
f^{B_i}_{W_z} \Rightarrow |\ddot{x}| \le \mu (\ddot{z} + g).$$ For
instance, if I keep my center of mass height at a constant velocity, 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 {}^Wp^{B_i}_{W_x}
f^{B_i}_{W_z}}{\sum_i f^{B_i}_{W_z}},$$ and since all ${}^Wp^{B_i}_{W_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: \begin{equation} (m\ddot{z} + mg) (x_{cop} - x) =
(z_{cop} - z) m\ddot{x} - I\ddot\theta. \label{eq:cop} \end{equation} If
we use the strategy proposed above for ignoring collision dynamics,
$\ddot{z} = \ddot{\theta} = 0$, and the result is the famous "ZMP
equations": \begin{equation} \ddot{x} = g \frac{(x - x_{cop})}{(z -
z_{cop})}. \label{eq:zmp} \end{equation} So the location of the center of
pressure completely determines the acceleration of the center of mass, and
vice versa! What's more, when $z - z_{cop}$ is a constant (the center of
mass remains at a constant height relative to the ground) or following a
predetermined schedule, the remaining 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>
<subsubsection><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, the position $p^A$ of an arbitrary point $A$:</p> <figure><img
width="70%" src="figures/zmp_derivation.svg"/></figure>
<p>I can re-write the equations of motion as \begin{align*} m\ddot{x} =&
\sum_i f^{B_i}_{W_x} = f^A_{net,W_x},\\ m\ddot{z} =& \sum_i f^{B_i}_{W_z}
- mg = f^A_{net,W_z} - mg,\\ I\ddot\theta =& \sum_i \left[ {}^Bp^{B_i}_W
\times f^{B_i}_W \right]_y = [{}^Bp^A_W \times f^A_{net,W}]_y +
\tau^A_{net,W}, \end{align*} where $f^A_{net} = \sum_i f^{B_i}$ and the
value of $\tau^A_{net}$ depends on the position, $p^A$. For some choices
of $p^A$, we can make $\tau^A_{net}=0$. We'll call these points
$\text{ZMP}$, for which we have \begin{equation} \left[
{}^Bp^{\text{ZMP}}_W \times f^{\text{ZMP}}_{net,W} \right]_y =
{}^Bp^{\text{ZMP}}_{W_z} f^{\text{ZMP}}_{net,W_x} -
{}^Bp^{\text{ZMP}}_{W_x} f^{\text{ZMP}}_{net,W_z} = \sum_i \left[
{}^Bp^{B_i}_W \times f^{B_i}_W \right]_y, \label{eq:zmp_def}
\end{equation} we can see that whenever we have ground reaction forces
($f^{\text{ZMP}}_{net,W_z} \ne 0$) there is an entire line of solutions,
${}^Bp^{\text{ZMP}}_{W_x} = a\, {}^Bp^{\text{ZMP}}_{W_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).</p>
<p>We can rewrite the equations of motion in terms of the ZMP.
Substituting the equations of motion into equation \ref{eq:zmp_def},
to replace the individual terms of $f^{\text{ZMP}}_{net}$, we have \[
{}^Bp^{\text{ZMP}}_{W_z} m\ddot{x} - {}^Bp^{\text{ZMP}}_{W_x}
(m\ddot{z} + mg) = I \ddot{\theta}. \] But this is precisely the
equation \ref{eq:cop} presented above.</p>
<p>Furthermore, since \[\sum_i \left[ {}^Bp^{B_i}_W \times f^{B_i}_W
\right]_y = \sum_i \left( {}^Bp^{B_i}_{W_z} f^{B_i}_{W_x} -
{}^Bp^{B_i}_{W_x} f^{B_i}_{W_z} \right), \] and for <i>flat terrain</i>
we have \[ \sum_i {}^Bp^{B_i}_{W_z} f^{B_i}_{W_x} =
{}^Bp^{\text{ZMP}}_{W_z} \sum_i f^{B_i}_{W_x} = {}^Bp^\text{ZMP}_{W_z}
f^\text{ZMP}_{net, W_x}, \] then we can see that this ZMP is exactly the
CoP: \[ {}^Bp^\text{ZMP}_{W_x} = \frac{\sum_i {}^Bp^{B_i}_{W_x}
f^{B_i}_{W_z}}{\sum_i f^{B_i}_{W_z} }. \] </p>
<p>In three dimensions, we solve for the point on the ground where
$\tau^\text{ZMP}_{net,W_y} = \tau^\text{ZMP}_{net,W_x} = 0$, but allow $\tau^\text{ZMP}_{net,W_z} \ne 0$ to
extract the analogous equations in the $y$-axis: \[
{}^Bp^{\text{ZMP}}_{W_z} m\ddot{y} - {}^Bp^{\text{ZMP}}_{W_y} (m\ddot{z}
+ mg) = I \ddot{\theta}. \] </p>
</subsubsection>
</subsection>
</section>
<section><h1>ZMP-based planning</h1>
<p>Understanding the relationship between the center of pressure and the
center of mass motion gives us a number of important tools for planning
motions of our complex robots. I'll describe one of the most common
pipeline for planning these motions, which decomposes the problem into:
<ol>
<li>planning the footstep (or more generally the contact) positions, then</li>
<li>planning the motion of the center of mass, and finally</li>
<li>calculating the individual joint motions to realize the desired
center of mass and footstep trajectories</li>
</ol>
This pipeline started with the famous ZMP walkers (like Honda's ASIMO),
with a focus on walking on flat terrain, but many of the concepts
generalize naturally to 3D and more general robot motion.</p>
<p>Most notable here is the decision to separate the complicated planning
problem into a sequence of simpler planning problems. Of course, one pays
some price for the decomposition -- footstep plans tend to be conservative
and kinematic because they do not reason directly about the dynamics of the
robot. And the centroidal planning cannot accurately account for
joint-level constraints, such as joint-position limits or effort limits.
But these approximations enable dramatically simpler planning solutions.
</p>
<subsection><h1>Heuristic footstep planning</h1>
<p>Coming soon. For a description of our approach with Atlas, see
<elib>Deits14a+Kuindersma14</elib>.</p>
</subsection>
<subsection><h1>Planning trajectories for the center of mass</h1>
<subsubsection><h1>The ZMP "Stability" Metric</h1>
</subsubsection>
<figure><img width="80%" src="figures/zmp_planning.svg"/></figure>
</subsection>
<subsection><h1>From a CoM plan to a whole-body
plan</h1></subsection>
</section>
<section><h1>Whole-Body Control</h1>
<p>Coming soon. See, for instance,
<elib>Kuindersma13+Kuindersma14</elib>.</p>
</section>
<section><h1>Footstep planning and push recovery</h1>
<p>Coming soon. See, for instance,
<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. See, for instance,
<elib>Dai14+Kuindersma14</elib>.</p>
<example><h1>LittleDog gait optimization</h1>
<script>document.write(notebook_link('humanoids', d=deepnote, link_text="", notebook="littledog"))</script>
</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 <script>document.write(notebook_link('footstep_planning', deepnote['exercises/humanoids'], link_text='this python notebook'))</script>.
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>
<exercise><h1>Understanding the zero moment point</h1>
<figure>
<img width="80%" src="figures/exercises/foot_on_ground.svg"/>
<figcaption>External forces acting on a robot pushing on a flat ground</figcaption>
</figure>
Consider an abstracted legged robot. The foot and the leg are assumed to be massless. Suppose there is no actuator at the pin joint P
<ol type = "a">
<li>
Is the system stable at $\theta = \pi/2 + 10^{-3}$?
</li>
<li>
Compute the zero moment point on the ground as a function of $\theta$ and the constants $m, h, g, l, L_{1}, L_{2}$ (if needed).
</li>
<li>
<ol type = i>
<li>
For what value of $\theta$ does the ZMP reach the toe $T$?
</li>
<li>
For what value of $\theta$ does the heel $H$ of the robot begin to lift?
</li>
</ol>
</li>
<li>
Suppose now that there is an actuator at the ankle with a controller capable of perfectly cancelling the torque around $P$ due to gravity so $\ddot{\theta} = 0$. What value of $\theta$ can you achieve without falling down? Express your answer in terms of the constants $m, h, l, L_{1}, L_{2}$ (if needed).
</li>
<li>
Suppose you are designing a foot that maximizes the postures you can assume without falling down. Suppose we can apply a torque at the ankle without torque limits.<ol type = i>
<li>
Would you want a taller or flatter foot?
</li>
<li>
Would you want a longer or shorter foot?
</li>
<li>
Where would you put the ankle with respect to the heel and toe?
</li>
</ol>
</li>
<li>
Suppose you are designing a robot designed to only walk forward. Suppose we can apply a torque at the ankle without torque limits.
<ol type = i>
<li>
Explain the trade offs in foot height.
</li>
<li>
Explain the trade offs in the design of $L_{1}$.
</li>
<li>
Explain the trade offs in the design of $L_{2}$.
</li>
</ol>
</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=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=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=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=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, 2023</td></tr>
</table>
</div>
</body>
</html>