forked from RussTedrake/underactuated
-
Notifications
You must be signed in to change notification settings - Fork 0
/
planning.html
355 lines (275 loc) · 15.3 KB
/
planning.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
<!DOCTYPE html>
<html>
<head>
<title>Ch. 12 - Motion Planning as
Search</title>
<meta name="Ch. 12 - Motion Planning as
Search" content="text/html; charset=utf-8;" />
<link rel="canonical" href="http://underactuated.mit.edu/planning.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=policy_search.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=feedback_motion_planning.html>Next Chapter</a></td>
</tr></table>
<script type="text/javascript">document.write(notebook_header('planning'))
</script>
<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 11"><h1>Motion Planning as
Search</h1>
<p>The term "motion planning" is a quite general term which almost
certainly encompasses the dynamic programming, feedback design, and
trajectory optimization algorithms that we have already discussed. However,
there are a number of algorithms and ideas that we have not yet discussed
which have grown from the idea of formulating motion planning as a search
problem -- for instance searching for a path from a start to a goal in a
graph which is too large solve completely with dynamic programming. <!--Some, but
certainly not all, of these algorithms sacrifice optimality in order to find
any path if it exists, and the notion of a planner being "complete" --
guaranteed to find a path if one exists -- is highly valued.--> </p>
<p>My goal for this chapter is to introduce these additional tools based on
search into our toolkit. For robotics, they will play a particularly
valuable role when the planning problem is geometrically complex (e.g. a
robot moving around obstacles in 3D) or the optimization landscape is very
nonconvex, because these are the problems where the trajectory optimization
formulation we've studied before will potentially suffer badly from local
minima. Many of these algorithms were developed initially for discrete or
purely kinematic planning problems; a major theme of this chapter will be the
adaptations that allow them to work for underactuated systems. There are also
many deep mathematical connections between discrete search and
<i>combinatorial</i> optimization; I hope to make a few of those connections
for you here.</p>
<p><elib>LaValle06</elib> is a very nice book on planning algorithms in
general and on motion planning algorithms in particular. Compared to other
planning problems, <em>motion planning</em> typically refers to problems
where the planning domain is continuous (e.g. continuous state space,
continuous action space), but many motion planning algorithms trace their
origins back to ideas in discrete domains. The term <em>kinodynamic motion planning</em> is often used when the plant is subject to
dynamic constraints like underactuation, in addition to kinematic constraints
like obstacle avoidance.</p>
<!--
<p>For this chapter, we will consider the following problem formulation:
given a system defined by the nonlinear dynamics (in continuous- or
discrete-time) $$\dot{\bx} = f(\bx,\bu) \quad \text{or} \quad \bx[n+1] =
f(\bx[n],\bu[n]),$$ and given a start state $\bx(0) = \bx_0$ and a goal
region ${\cal G}$, find any finite-time trajectory from $\bx_0$ to to $\bx
\in {\cal G}$ if such a trajectory exists.</p>-->
<section><h1>Artificial Intelligence as Search</h1>
<p>A long history... some people feel that the route to creating
intelligent machines is to collect large ontologies of knowledge, and then
perform very efficient search. (The more modern view of AI is focused
instead on machine learning, the right answer probably involves pieces of
both.) Samuels checker players, Deep Blue playing chess, theorem proving,
Cyc, IBM Watson,...</p>
<p>One of the key ideas is the use of "heuristics" to guide the search.
"Is it possible to find and optimal path from the start to a goal without
visiting every node?". $A^*$. Admissible heuristics. Example: google
maps.</p>
<p>Online planning. $D^*$, $D^*$-Lite.</p>
<p>Some of the most visible success stories in deep learning today still make use of planning. For example: DeepMind's
<a href="https://en.wikipedia.org/wiki/AlphaGo">AlphaGo</a> and <a
href="https://en.wikipedia.org/wiki/AlphaZero">AlphaZero</a> combine the planning algorithms developed over the
years in discrete games , notably <a href="https://en.wikipedia.org/wiki/Monte_Carlo_tree_search">Monte-Carlo Tree
Search (MCTS)</a>, with learned heuristics in the form of policies and value functions.</p>
</section>
<section><h1>Sampling-based motion planning</h1>
<p>If you remember how we introduced dynamic programming initially as a
graph search, you'll remember that there were some challenges in
discretizing the state space. Let's assume that we have discretized the
continuous space into some finite set of discrete nodes in our graph. Even
if we are willing to discretize the action space for the robot (this might
be even be acceptable in practice), we had a problem where discrete actions
from one node in the graph, integrated over some finite interval $h$, are
extremely unlikely to land exactly on top of another node in the graph. To
combat this, we had to start working on methods for interpolating the value
function estimate between nodes.</p>
<todo>add figure illustrating the interpolation here</todo>
<p>Interpolation can work well if you are trying to solve for the
cost-to-go function over the entire state space, but it's less compatible
with search methods which are trying to find just a single path through the
space. If I start in node 1, and land between node 2 and node 3, then
which node to I continue to expand from?</p>
<p>One approach to avoiding this problem is to build a <em>search tree</em>
as the search executes, instead of relying on a predefined mesh
discretization. This tree will contains nodes rooted in the continuous
space at exactly the points where system can reach.</p>
<p>Another other problem with any fixed mesh discretization of a continuous
space, or even a fixed discretization of the action space, is that unless
we have specific geometric / dynamic insights into our continuous system,
it very difficult to provide a <em>complete</em> planning algorithm. Even
if we can show that no path to the goal exists on the tree/graph, how can
we be certain that there is no path for the continuous system? Perhaps a
solution would have emerged if we had discretized the system differently,
or more finely?</p>
<p>One approach to addressing this second challenge is to toss out the
notion of fixed discretizations, and replace them with random sampling
(another approach would be to adaptively add resolution to the
discretization as the algorithm runs). Random sampling, e.g. of the action
space, can yield algorithms that are <em>probabilistically complete</em>
for the continuous space -- if a solution to the problem exists, then a
probabilistically complete algorithm will find that solution with
probability 1 as the number of samples goes to infinity.</p>
<p>With these motivations in mind, we can build what is perhaps the
simplest probabilistically complete algorithm for finding a path from the a
starting state to some goal region with in a continuous state and action
space:</p>
<example><h1>Planning with a Random Tree</h1>
<p>Let us denote the data structure which contains the tree as ${\cal T}$. The algorithm is very simple:
<ul>
<li>Initialize the tree with the start state: ${\cal T} \leftarrow \bx_0$.</li>
<li>On each iteration:
<ul>
<li>Select a random node, $\bx_{rand}$, from the tree, ${\cal T}$</li>
<li>Select a random action, $\bu_{rand}$, from a distribution over feasible actions.</li>
<li>Compute the dynamics: $\bx_{new} = f(\bx_{rand},\bu_{rand})$</li>
<li>If $\bx_{new} \in {\cal G}$, then terminate. Solution found!</li>
<li>Otherwise add the new node to the tree, ${\cal T} \leftarrow \bx_{new}$.</li>
</ul>
</li>
</ul>
It can be shown that this algorithm is, in fact, probabilistically complete. However,
without strong heuristics to guide the selection of the nodes scheduled for expansion,
it can be extremely inefficient. For a simple example, consider the system $\bx[n] = \bu[n]$ with $\bx \in \Re^2$ and $\bu_i \in [-1,1]$.
We'll start at the origin and put the goal region as $\forall i, 15 \le x_i \le 20$.
<!--
Try it yourself:
<pre><code class="matlab" testfile="testRandomTree">
T = struct('parent',zeros(1,1000),'node',zeros(2,1000)); % pre-allocate memory for the "tree"
for i=2:size(T.parent,2)
T.parent(i) = randi(i-1);
x_rand = T.node(:,T.parent(i));
u_rand = 2*rand(2,1)-1;
x_new = x_rand+u_rand;
if (15<=x_new(1) && x_new(1)<=20 && 15<=x_new(2) && x_new(2)<=20)
disp('Success!'); break;
end
T.node(:,i) = x_new;
end
clf;
line([T.node(1,T.parent(2:end));T.node(1,2:end)],[T.node(2,T.parent(2:end));T.node(2,2:end)],'Color','k');
patch([15,15,20,20],[15,20,20,15],'g')
axis([-10,25,-10,25]);
</code></pre>
-->
Again, this algorithm <em>is</em> probabilistically complete. But after expanding 1000 nodes, the tree is basically a mess of node points all right on top of
each other:
<figure>
<img width="70%" src="figures/random_tree.svg"/>
</figure>
<p>We're nowhere close to the goal yet, and it's not exactly a hard problem.
</p>
</example>
<p>
While the idea of generating a tree of feasible points has clear advantages, we have lost
the ability to cross off a node (and therefore a region of space) once it has been explored.
It seems that, to make randomized algorithms effective, we are going to at the very least need some
form of heuristic for encouraging the nodes to spread out and explore the space.
</p>
<subsection><h1>Rapidly-Exploring Random Trees (RRTs)</h1>
<figure>
<img width="70%" src="figures/rrt.svg"/>
</figure>
<figure>
<img width="90%" src="figures/rrt_basic.svg"/>
<figcaption>(<a href="figures/rrt_basic.swf">Click here to watch the animation</a>)</figcaption>
</figure>
<figure>
<img width="90%" src="figures/rrt_voronoi.svg"/>
<figcaption>(<a href="figures/rrt_voronoi.swf">Click here to watch the animation</a>)</figcaption>
</figure>
</subsection>
<subsection><h1>RRTs for robots with dynamics</h1>
</subsection>
<subsection><h1>Variations and extensions</h1>
<p>Multi-query planning with PRMs, ...
</p>
<p>RRT*, RRT-sharp, RRTx, ... </p>
<p>Kinodynamic-RRT*, LQR-RRT(*)</p>
<p>Complexity bounds and dispersion limits</p>
</subsection>
<subsection><h1>Discussion</h1>
<p>Not sure yet whether randomness is fundamental here, or whether is a temporary "crutch"
until we understand geometric and dynamic planning better.</p>
</subsection>
</section>
<section><h1>Decomposition methods</h1>
<p>Cell decomposition...</p>
<p>Approximate decompositions for complex environments (e.g. IRIS)</p>
</section>
<section><h1>Planning as Combinatorial + Continuous Optimization</h1>
<p>Shortest path on a graph as a linear program.</p>
<p>Mixed-integer planning.</p>
<subsection>
<h1>Motion Planning on Graphs of Convex Sets (GCS)</h1>
</subsection>
</section>
<section><h1>Exercises</h1>
<exercise><h1>RRT Planning</h1>
<p>In this <a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/planning/rrt_planning/rrt_planning.ipynb" target="_blank">notebook</a>
we will write code for the Rapidly-Exploring Random
Tree (RRT). Building on this implementation we will also implement RRT*,
a variant of RRT that converges towards an optimal solution.</p>
<ol type="a">
<li>Implement RRT</li>
<li>Implement RRT*</li>
</ol>
</exercise>
</section>
</chapter>
<!-- EVERYTHING BELOW THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<div id="references"><section><h1>References</h1>
<ol>
<li id=LaValle06>
<span class="author">Steven M. LaValle</span>,
<span class="title">"Planning Algorithms"</span>, Cambridge University Press
, <span class="year">2006</span>.
</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=policy_search.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=feedback_motion_planning.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>