Algorithms for Walking, Running, Swimming, Flying, and Manipulation

© Russ Tedrake, 2022

Last modified .

How to cite these notes, use annotations, and give feedback.

**Note:** These are working notes used for a course being taught
at MIT. They will be updated throughout the Spring 2022 semester. Lecture videos are available on YouTube.

Previous Chapter | Table of contents | Next Chapter |

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.

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
*combinatorial* optimization; I hope to make a few of those connections
for you here.

*motion planning* 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 *kinodynamic motion planning* is often used when the plant is subject to
dynamic constraints like underactuation, in addition to kinematic constraints
like obstacle avoidance.

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,...

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.

Online planning. $D^*$, $D^*$-Lite.

Some of the most visible success stories in deep learning today still make use of planning. For example: DeepMind's AlphaGo and AlphaZero combine the planning algorithms developed over the years in discrete games , notably Monte-Carlo Tree Search (MCTS), with learned heuristics in the form of policies and value functions.

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.

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?

One approach to avoiding this problem is to build a *search tree*
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.

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 *complete* 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?

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 *probabilistically complete*
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.

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:

Let us denote the data structure which contains the tree as ${\cal T}$. The algorithm is very simple:

- Initialize the tree with the start state: ${\cal T} \leftarrow \bx_0$.
- On each iteration:
- Select a random node, $\bx_{rand}$, from the tree, ${\cal T}$
- Select a random action, $\bu_{rand}$, from a distribution over feasible actions.
- Compute the dynamics: $\bx_{new} = f(\bx_{rand},\bu_{rand})$
- If $\bx_{new} \in {\cal G}$, then terminate. Solution found!
- Otherwise add the new node to the tree, ${\cal T} \leftarrow \bx_{new}$.

We're nowhere close to the goal yet, and it's not exactly a hard problem.

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.

Multi-query planning with PRMs, ...

RRT*, RRT-sharp, RRTx, ...

Kinodynamic-RRT*, LQR-RRT(*)

Complexity bounds and dispersion limits

Not sure yet whether randomness is fundamental here, or whether is a temporary "crutch" until we understand geometric and dynamic planning better.

Cell decomposition...

Approximate decompositions for complex environments (e.g. IRIS)

Shortest path on a graph as a linear program.

Mixed-integer planning.

In this 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.

- Implement RRT
- Implement RRT*

- "Planning Algorithms", Cambridge University Press , 2006. ,

Previous Chapter | Table of contents | Next Chapter |