Flash and JavaScript are required for this feature.
Download the video from iTunes U or the Internet Archive.
Topics covered: Feasible motion planning
Instructors: Russell Tedrake

Lecture 14: Feasible Motion...
The following content is provided under a Creative Commons license. Your support will help MIT OpenCourseWare continue to offer high quality educational resources for free. To make a donation or view additional materials from hundreds of MIT courses, visit MIT OpenCourseWare at ocw.mit.edu.
RUSS TEDRAKE: OK. Welcome back. Well, sorry, I wasn't here. It's more dramatic for me, I guess, than if I had been here on Tuesday. I hope John did a good job covering for me. I'm sure he did.
So you've learned about walking robots. You've learned about lots of robots. You learned a handful of very powerful techniques, I think, for designing motions for these robots.
Some of my favorite these days are things like the dir call type methods. So let's consider the problem of taking some robot described by our standard, some non-linear dynamics form, and taking that robot from an initial condition x0 and getting it to some final condition. I don't even care about the time. So you're just getting it to some other state, x goal.
So what tools have we already got that could help us drive this system from some initial condition to some goal state? The answer is a lot of tools, right? If the state space is small enough, you could imagine designing an optimal control cost function that we could do dynamic programming on. That would get us to that goal if we only rewarded ourselves from being away from the goal.
We could do a shooting method, right? If we're using our SQP, our SNOPT type interface to the shooting methods, then we could provide a final value constraint, which says I want the endpoint of my trajectory to be exactly at the goal. And that would solve this problem in some cases.
But both of those methods have problems, right? The DP doesn't scale. The shooting methods scale nicely. But if you ask your shooting method to drive you from x0 to x goal, there's some chance it's going to just say, I can't do it, right? I can't satisfy that constraint because it's based on a local gradient optimization.
And if there's local minima, SNOPT will just say, I can't satisfy the constraint x at time final equals x goal, happens a lot. If you're looking at hard enough, interesting enough, problems, it'll happen to you, OK? So what do you do? Do you just say, oh, find a new research problem, tell my advisor I've got to find something else?
OK, there's more things you can try. The methods I want to talk about today are the feasible motion planning algorithms. So motion planning is a very broad term. It's used all the time in robotics.
I'd say roughly everything we've done to date has been a motion planning algorithm. It's like saying it's a control algorithm. It's a very general term.
Some people debate whether an algorithm is a motion planning algorithm or an optimal control algorithm or whatever. I think that's sort of a waste of time. I would say they're all motion planning algorithms.
Most of the things we've talked about so far I would call optimal motion planning, where we actually had a cost function flying around to do it. Oftentimes, motion plans go from a start to a goal. Oftentimes, they're open loop, but that's not really defining. You can do feedback motion planning. Motion planing is just a very general term saying I'm designing the motions of my machine.
The reason I want to use motion planning in the title for this lecture and next lecture is because there's an important class of motion planning algorithms that are not covered by the optimal control algorithms. And that's these feasible motion planning algorithms.
So the optimal motion planning algorithms try to get me from x0 to x goal in some way that's good as scored by some cost function. Feasible motion planning algorithms aspire to do less. They just say, I'll get any trajectory that gets me from the start to the goal. Because they surface their claims on optimality, oftentimes these algorithms will work for more complicated systems.
So the message I want to sort of deliver today is that there are a lot of good feasible motion planning algorithms out there in the world. We'll talk about some of the most exciting ones, I think. And actually, even if you care about optimality, even if deep down in your core you say this is a cost function that I must optimize for my life to be complete, you still should care about feasible motion planning algorithms because they can do something like seed a dir call method, let's say, and get it out of local minima.
So let's say I have an Acrobot that I'm trying to swing up. And I've got a table right here, so I can't swing that way. And whatever my initial guess at the trajectory, the system keeps banging up against this. It can't figure out a way to turn around and go back the opposite way to get to the top. Or maybe there's a table that you have to go through just at a certain way.
I would say, if you're shooting method is having trouble finding its way to the goal, maybe you should back off on optimality. Just find any old trajectory that gets to the goal. And then take that result of that motion planning algorithm, hand it to dir call, and let it then do the rest of the optimization.
We'll say more about this. I just wanted to give you some context about why we want to talk about this extra set of algorithms. In general, we're going to expect these to scale to slightly higher dimensional systems than the optimal control algorithms we've talked about so far and to be more global than the shooting methods.
There's a great book on all of this stuff called-- I guess it should be underlined-- Planning Algorithms. So I just spelled planning wrong. So I'm completely sleep deprived.
If I do stupid things like that, call them out. I'll fix them. Something about kid number two, the first kid stays awake while the other kid sleeps and vice versa. It just makes it that much more interesting. Yeah.
And Planning Algorithms, the book by Steve LaValle is actually-- Steve's got it on his website, the full version. You can download for free. So it's nice accessible text that you can check out if you like it.
OK. So before we get into our first feasible motion planning algorithm, let me give you just a little sort of culture. The motion planning algorithms, I think, actually grew up not so much in the controls community, but in more the artificial intelligence community. And there's a reason for that.
There are serious people out there that think the only thing there is to intelligence is the ability to do search. So I think that artificial intelligence via search, to some extent, sounds ridiculous. But it's pretty hard to disprove and some people really think it. But that's the way it happened.
So let me give you some cultural examples of where that came from. Some of the original efforts in artificial intelligence, they thought, my computer program will be intelligent if it can play a game like checkers or chess. And as early as the '50s, Art Samuel was building checker playing robots that are basically doing search in order to try to figure out how to beat their opponent in chess-- or in checkers, sorry.
Checkers-- Samuel in the '50s. Chess, we can leap forward to IBM's Deep Blue, let's say in the last decade. These are computer programs that, at times, can play with the sophistication of a trained human.
And they're based on simply having the rules of the game programmed, some databases of moves that they've stored up, a lot of them in the Deep Blue case, and search. Finding the way to move my robot from x0 to x goal you can imagine being very, very analogous to taking my computer game and finding from an initial board configuration to a winning board configuration. They're exactly the same problem.
So because of things like computer games, the artificial intelligence community really started building up these search algorithms. And they were very fundamental. The other big push, I think, from the AI community was in ideas like theorem proving.
So an AI zealot of 4 years ago might have told you that mathematics would be obsolete by now. Because all you have to do is program the basic rules of mathematics into a set of rules that your computer program could evaluate, and then proving whether something was true or not was just a matter of finding the right chain of small logical deductions to put together to be able to prove that p equals np or something like that.
OK. And actually, I joke, but that's actually a very powerful thing that works today. If you've use Mathematica or Maple and you've asked it to simplify, that's a result of theorem proving work that people did in artificial intelligence a long time ago. And again, it's a matter of designing the rules of the game, which is a collection, in that case, of mathematical primitives, and finding a path from your initial knowledge to some proof, let's say.
But even going beyond that, there's people out there that-- my favorite instance of this is there's a guy, David Lenat, who's somewhere in Texas right now typing-- well, he's got teams of people typing random factoids into computers. He's got this project called Cyc. How many people have heard of Cyc? Yeah?
So I think this guy believes that, if I can get enough people to type in things like, dogs chase cats and balloons go up or something-- I don't know, enough random factoids into this computer-- and he builds up an ontology of knowledge to some big collection of facts and a good search algorithm to back it up, that his computer will achieve intelligence. It'll be able to answer any query.
It'll be completely indistinguishable from a human. You could ask it any query. And as long as it's got the right chain of factoids stored away and a good search algorithm behind it, then this thing is intelligent.
And people have been typing things in for a really long time. You can go, and you can get the student version or whatever and ask it questions. And you can get a research version and ask it questions.
And people use it. And I mean, Google, I think, works better because it's accessing WordNet, which is basically an ontology of synonyms and things like this. These sound crazy, but they're the backbone of some of the things that you use every day.
OK. So for whatever reason, for these kind of reasons, the search algorithms, the motion planning algorithms that we're going to talk about, which are today used a lot in robotics, grew up under the computer science umbrella under artificial intelligence.
So the term motion planning typically implies that you've got some dynamical system you're shoving around, let's say, but it's clearly just an instance of this search problem. When people talk about motion planning in robotics, there are problems that people care about a lot.
A majority of the motion planning literature in robotics is concerned with kinematic motion planning, often with obstacles or something like this. So if I have a robotic arm with 10 degrees of freedom and I want to reach through some complicated geometry to turn a doorknob or to reach down and pick up some part from assembly, then that's a complicated geometric problem.
Typically, you assume that the systems are fully actuated. You don't worry about actually handling the trajectory once you get there. That's normally assumed away. And they assume that all trajectories are feasible. They're just trying to find some path through the obstacle field.
So one of the key problems that you can think about in that world is the piano movers problem. How many people know the piano movers problem? OK. How many people have ever moved a couch into a dorm room?
You all know the piano movers problem then, right? Basic task is you have some 3D shape. You have a bunch of obstacles. You have to find a position and orientation trajectory that will maneuver your big piano through a world of obstacles.
It's just like going up the stairs with your couch, you know, marking the walls and bending your couch as you go. And that's sort some of the driving problems in the motion planning space. So it's things like that, finding a collision-free path for a complicated geometry through an obstacle based environment, just trying to give you a little culture here.
And in that world, one of the most important concepts, I think, is the configuration space concept, which actually-- you know who came up with configuration spaces? He's sitting right upstairs. It's Tomás Lozano-Pérez is, I think, credited with the configuration space idea.
And the idea is that these problems can get a lot simpler. Let's say I have a square robot going through some complicated obstacle field. And I want to find a path from some start to the goal.
Well, instead of reasoning about the geometry of this relative to the geometry of this all the time, the configuration space tells you you should actually add the volume of your robot to your obstacles coming up with configuration space obstacles. If you actually add the volume of your robot to the obstacles-- I'm doing this very hand-waving here.
But then you can turn the problem of finding a big geometry through another geometric field to taking a point and driving it through the geometric field. And that's a critical, critical idea in the world of kinematic motion planning. We don't really care about kinematic motion planning in this class.
We've got enough troubles without obstacles. We just care about having an interesting dynamic system to move around. So the things we're talking about in this class fall under the heading kinodynamic motion planning.
And of course, the nonholonomic motion planning ideas are very related, too. We care about the dynamics. We care about things that are forced to follow some trajectories, like nonholonomic systems are, too.
So if you're out there seeing papers and talks and everything by motion planning, I just want you to sort of see that they're certainly related to the optimal control things we've talked about. But sometimes they let go of optimality. They're often about kinematics, but there's a good subset of them which is thinking about exactly the problems we care about with this kinodynamic motion planning.
Excellent. OK. So now, let's do some motion planning. Culture is there.
When we did dynamic programming, I already sort of forced upon you the idea that we could potentially, at great cost, discretize our system and state and actions potentially. So let's do the same thing when we start off with motion planning and start with discrete planning algorithms.
Let's say we've got our phase plot of our simple pendulum. For kicks right now, let's just start off with the trivial discretization, let's say. We're going to bin up our space like this, call each one of those a state and try to start talking about a graph-based representation of the dynamics of the system that tries to find a path, let's say, from our initial state to some goal state.
This isn't the only way to do discretization, but it's a good way to start thinking about it. So that turns the problem into some graphical model, where the control actions determine our state transitions. I like to actually call them s and a in the discrete world.
So people know a lot about doing search on a graph. I don't want to bore you with it, but I want you to at least know the algorithms that are out there and be able to call upon them if they become useful. So let's see we've butchered up our system like this, and we want to find a path from some starting discrete state to some goal state with a discrete set of actions. How can we do that?
AUDIENCE: [INAUDIBLE] search algorithm, like A star.
RUSS TEDRAKE: A star is a perfect example. Yeah. Dynamic programming is on the table, too. Dynamic programming, that's actually the way I-- the reason I drew that picture before is because we talked about that as a way to think about dynamic programming.
In dynamic programming is incredibly efficient if what you care about is knowing the optimal feedback policy, how to get to the goal from every possible initial state. I mean, the problem with that is, if you have to look at every single initial state, then it's not going to scale the high dimensions because there's going to be a lot of states.
If you have a slightly higher dimensional system and you want to find a start to the goal, but just a single path-- you don't care about every possible state-- then you can do not better than dynamic programming. But you can be more selective with your dynamic programming algorithm by doing these Dijkstra type and A star type algorithms. So let me just sort of do that quickly, so you know that that's there.
So dynamic programming, again, it's very efficient. It goes to a global optima. Typically, DP is used to describe the version where you're trying to solve for every state simultaneously.
You can do a bit better, you can be more selective, if you just care about from a start to a goal, like 0 to xg. It's actually a little bit surprising to say that I can actually find my path from a start to a goal and know that I'm at a global optima without ever expanding every single node.
And then sometimes we don't even care about optimality at all. Like I said, some of these algorithms are just trying to find any old path to the goal, and then you can certainly imagine finding your path to the goal without expanding every possible node.
They all have a very simple recipe. Basically, you keep a prioritized Q. How many people know what a prioritized Q is? I should say it's often called Q, but it means queue. It's another sleep deprivation thing there.
So a prioritized queue is a data structure that we use in computer science that I'll describe quickly here. So we basically have a list of nodes that we want to consider expanding next stored in some list, but they're stored in that list in an ordered fashion, ordered by how likely it is that we want to check that node next.
Depending on the ways that you add nodes into the prioritized queue, you can do any number of the standard algorithms, the breadth first search, the depth first search. Dijkstra's, let me make sure I spell it right. D-J-I, right? What is it?
AUDIENCE: D-I-J.
RUSS TEDRAKE: D-I-J? OK, good. Thanks. And you could do A star. There's more. There's iterative deepening, things like this.
They all go basically like this. I have a queue of things I'm about to explore. I start off with the starting state. I put that in my queue.
Next step of the algorithm-- take something out of the queue. Consider its children. And then take the first one out of my priority queue, repeat.
If I consider the children in a first in, first out kind of way, if I add nodes and just say, as soon as I pick up a node, I'm going to go ahead and stick those nodes into my queue in a sort of a first in, first out kind of way, then what I'm going to do is I'm going to proceed to look for the goal by going here, then here, then here, then here, then here, right down the tree. That's a breadth first search.
If I do a prioritized queue where, when I add the children, I just do a last in, first out, then I'll go here. Then I'll go here. Then I'll go here, and I'll go as deep down the tree as I could possibly do before I come back and go down to the other nodes.
If I keep along, let's call it, a cost to come to differentiate it from the cost to go, if I keep a record of how much cost I incurred for my cost function getting to that node and I always select to expand the node that would be the cheapest, then that's called Dijkstra's algorithm. It's exactly equivalent to dynamic programming, but it's in the forward sense.
And then A star is an even nicer way to do that, which combines a cost to come plus some heuristic.
My goal is to make sure you guys know these are out there. There's lots of good easy places to read about these algorithms, especially Steve LaValle's book. The only surprising thing really about these algorithms is, first of all, that you can often very efficiently find your way to the goal without expanding as many nodes as you might think.
A star, in particular, if you can find a good heuristic-- a heuristic, in this case, is an estimate of the cost to go, a guess at your value function. If you can guess your value function in a way where you always underestimate it-- again, I'm not going to bore you with all the details here-- then actually you can sometimes expand very few nodes and find your way right to the goal. They're very efficient algorithms.
People out here use A star all the time. For the Grand Challenge, I think half the teams had A star planners running on their vehicle. For LittleDog, we're picking footholds to get from the start to the goal. We have an A star planner in there, discrete time planners.
These things are real. They're good. They're fast. If you can find a good heuristic, they can be very, very efficient.
If you care about a continuous time, a continuous state, continuous action plan that gets me from the goal, then they're not as satisfying because they relied on this discretization. So we know, when we're discretizing this thing, we're going to have some problems. I'm going to just assume that there's a control action that gets me sort of squarely in the center of this next discretized cell and then go like this.
If I were to execute out my actions from my discrete planning algorithm on the continuous time system, I'm going to quickly deviate from my plan. But these things are an excellent way to start to seed something like a direct co-location method. Find yourself a feasible optimal plan. And then stabilize it, let's say, with the LTV feedback.
Good. So that world of planning algorithms is out there. But about, I don't know, 10 years ago now, something big happened in the robotic motion planning world. People started using sample-based motion planning algorithms. And there was sort of a revolution in people using motion planning in robotics.
It's also sometimes called randomized motion planning. And these are we are going to dig a little deeper in because we use them all the time. I think they're an important class of algorithms.
So two of the most notable sample-based motion planning ideas are the rapidly exploring randomized trees and the probabilistic roadmap.
Another poll, how many people know about RRTs? Awesome. How many people know about PRMs? OK. They're pretty related to each other.
Let's talk about rapidly exploring randomized trees first. So both of these are an attempt to get away from these very coarse discretizations like this and sort of embrace the fact that we're working in a continuous state space. Instead of discretizing it at some known sites, we're going to discretize sort of at randomly chosen samples.
And as we add more and more samples, we're going to worry less and less about the discretization. And eventually, we're going to have something nice to say about the continuous space. So the rapidly exploring randomized trees, the RRTs, are a very simple algorithm to explain.
Let's think about it in terms of moving a 2D piano through a 2D world in configuration space. So we've got a point moving through a world. It should really be 3D, actually, if there's an orientation to the piano. But let's just look at the 2D problem here.
So I've got a point, an official state, an initial goal in a 2D world. Let's call it xy. And I've got some obstacles.
I'd like to find a path from the start to the goal without explicitly discretizing the space. The RRTs do it like this. Pick a point from random in the space. Try to connect your current tree, which in the initial state is just this x0, to that new point.
Let's make it exactly clear that I can't do that. My attempt to directly connect this is going to fail because it goes through an obstacle. So I can't add that node.
Sample again randomly. At some point, I'll get a node, like, here. I'm going to make a branch of my tree that connects those two points.
Pick a new random point. Let's say you get something here. Now, you have to choose between these two random points, decide which is the closest. The closest isn't always an easy thing to decide if you have complicated dynamics.
In this problem, it's pretty easy. We could just use something like a Euclidean distance. And I'll connect it up like this.
I get another random point. I connect it up like this. I get some random points that go whatever way.
If they end up here, I just throw them out. If they end up something that I can't connect, I'll just throw it out. But at some point, I'll start filling the space with these trees.
Lo and behold, if I do it enough, it doesn't actually necessarily take that long until I get up here and get close in the vicinity of that goal, very simple random algorithm. Do you understand how it's sort of avoiding the curse of discretization? It's just picking points at random from this continuous space.
So there's no constraints, no explicit constraints that it lies in the center of some bucket or something like that. Nodes could be arbitrarily close to each other. I can get arbitrarily fine sort of discretization.
What's really cool, what makes these things tick, is that the idea is so simple. The code is so easy to write. And they really, really work fast. Let me just show you that basic example.
This time, I'm going to do it even without any obstacles, but just to show you the basic growth of these kind of planners. Awesome. OK. So my initial condition is just that blue point.
And I'm going to start growing a tree in every random direction. There's not even a goal in this problem. This is just to show you the basic growth of the tree.
Every point I grow at random, every time I pick a point from a uniform distribution covering the space, I'm going to find the closest point in the algorithm, a closest point on the tree. And I'm going to try to grow my tree towards that node. Yeah. Sort of understand what's happening there? It's pretty faint, I think, but-- all right. So watch what happens if I just let that run.
OK. Trivial algorithm, it has what a lot of people like to call a fractal expansion of the coverage area. Of course, if there's obstacles, it'll take a little longer to get around there, but the algorithm probabilistically covers the state space as I add more and more points.
Some people call them dense trees because they do this. If you noticed something about them, they have sort of surprisingly nice qualities. It filled the space pretty uniformly pretty quickly.
It didn't just go off in one corner and start adding nodes on one side or something like that. It actually has a property that people call the Voronoi bias.
The Voronoi bias implies that the tree has a bias for always growing into the biggest open regions of the graph. And that just comes from sampling uniformly. If I have some tree that has a big, wide open area for whatever region, then with very high probability I'm going to pick a point that's inside the biggest region.
And the biggest regions in the space are the ones that have the highest probability of being chosen. So probabilistically, this thing fills the biggest open spaces in the search tree just by the virtue of sampling uniformly.
So you get these very, very dense, fast trees. And you can imagine, if there's obstacles or something like that inside there, it's going to work its way just around the obstacles. Yeah.
AUDIENCE: Does it slow down as you get more and more nodes in there?
RUSS TEDRAKE: Good. So what is the computation that goes into it? What's the expensive part of the computation would you guess?
AUDIENCE: Finding the nearest neighbor.
RUSS TEDRAKE: Nearest neighbor-- good. So, yeah, it slows down with the nearest neighbor, but not as much as you expect. Actually, there's two expensive parts. Only the nearest neighbor one shows up here.
The other one, potentially, is the collision checker. And in some complicated systems, checking if you're inside a region, especially if you're a big robotic arm or something like that, can be actually the dominant, expensive thing.
So these things, in MATLAB, everything's vectorized. It actually doesn't grow that badly with the number of nodes, but certainly the nearest neighbor calculation gets more and more expensive. By default, it's just checking the sample point at every possible node.
AUDIENCE: You could use a k-d tree or something.
RUSS TEDRAKE: Yeah, good. Yeah. So there are different sort of structures that you can use to make that look-up more efficient. So RRTs have kind of come in and done things to robotics that people hadn't done before. So this is a two-dimensional example.
People would have said before, I can do sort of motion planning effectively and maybe 5 or 10 dimensions, a little bit more than I could do with just DP, but not a lot more. Because I'm still doing these sort of random factorizations. The guys that came out and started doing these RRTs-- LaValle being one of them, Kuffner being another one-- started showing examples of robots with 32 degrees of freedom doing pretty complicated plans. See if I have that Kuffner video there.
It's an animated GIF. This is H7 at the time. This was the University of Tokyo humanoid entry before everybody started giving out ASIMOs. A little before Honda started giving out ASIMOs this is.
And it's doing things like taking that 32 degree of freedom robot, finding a plan to have it bend down and pick up a flashlight under the table. And that sort of shocked people. Now, people have gone off and used RRTs to do things like protein folding. It's was a pretty hot topic in the motion planning community and these other very high complexity geometric planning problems that people just haven't done before.
But even this guy, even though it's a robot, his feet are flat on the ground, yeah? And they're still just assuming that you can search in position space and then find some controller that'll stabilize it. So they're not doing the underactuated planning example.
All right, so I wouldn't be talking about it if I didn't think it was relevant for underactuated planning. So let's show you the basic story here. What happens if we do it on a simple pendulum?
There we go. So you see, I'm plotting only every 30 nodes or something like that. I pick a point at random. I start growing the tree.
The way I grew the tree, now, is I don't go all the way towards the goal because I can't necessarily go all the way towards the goal. I just take a step in the direction of the goal. How do I take a step in the direction of the goal?
I just try five random actions, let's say, and pick the one that got closest to the goal. So if you look closely, every time it expands a node, you'll see a bunch of different arrows coming out. And it picked the one that got closest to that random sample point.
And it's still just using the Euclidean distance to try to find the closest neighbor. This one I think I wasn't careful about doing the terminal condition. But I want you to see what the most important thing about this is, what do you see in that plot besides a mess as it gets adding more and more?
What I see is I see the fundamental structure of the pendulum. I see these spiral trajectories. It's amazing to me that trivial randomized algorithms like this can probe the basic dynamics of my underactuated system.
All right, so if you see the algorithm is, now, very simple. So I pick a point at random. That's actually a good example I just happened upon.
Pick a point at random. The red is my random sample plane. The blue is the closest plane in the tree in the Euclidean sense.
It happens to be a horrible choice for the closest point on the tree in the dynamic sense because I know, in state space, my dynamics can only go that way. So really, I'd like to have picked a point back there that was farther and Euclidean distance, but closer sort of in dynamic distance. And I'll actually talk more next lecture about how you could do that.
But from that note, I try 10 different actions, see where it would have taken me, and pick the one that ended up closest in Euclidean distance, which happens to have been the one that happened horizontally. That's enough to let this thing just sort of spread itself out into the world like a disease or something, just covering the space and probing the dynamics of the system. And you wait until you get close to the goal.
RRTs are very, very powerful way, now, to start trying to find reasonable plans on a robot. That's the vanilla RRT algorithm. There's a lot of ways you can quickly improve them. And actually, any of the graph-based search algorithms do this.
So if I wanted to do some simple things to try to speed up that algorithm, what might you suggest? There's a couple of reasons why it felt inefficient. One of them is because it's not doing a very nice job of chasing these sort of intermediate goals, like that example showed. The other thing is it seems like it's not really making any effort to go towards the goal.
So there's a handful of heuristics that people use to make these things tick. One of them is a goal bias.
I mean, there's lots of ways to implement it. The standard way to implement it is with, let's say, probability of 0.5 or something like that, 0.05, choose my random sample to be the goal. Otherwise, my random sample is just the uniform distribution.
That would certainly encourage the system to find its way to the goal a lot more efficiently. What's another possible way to speed this up?
AUDIENCE: Bidirectional.
RUSS TEDRAKE: Bidirectional, yeah. In fact, all of the search techniques, the breadth versus depth versus A star on discrete graph searches, a lot of times one of the standard things people do is they'll do a backward search. While you're growing out from the start towards the goal, you might as well start from the goal and go backwards.
The problem with this is it feels like you're trying to find a needle in a haystack. If you can grow a tree in both directions, then at least you got a lot of needles to look for. And you just wait till the trees come fairly close.
And even backward search sometimes by itself can be faster. It depends on the system.
AUDIENCE: Is that because, if your goal is an unstable point [INAUDIBLE], it's more likely to find it? Is that intuitively what happens?
RUSS TEDRAKE: Why sometimes a backward search would work better?
AUDIENCE: In cases like underactuated.
RUSS TEDRAKE: It's a really good question.
AUDIENCE: Because you might have trouble going to some type of fixed point, but that for sure already starts you out there.
RUSS TEDRAKE: I won't put my weight behind it, but that sounds like a pretty good explanation. In practice, we always do bidirectional trees.
AUDIENCE: Even when you want to do both, you never just go backwards.
RUSS TEDRAKE: Yeah.
AUDIENCE: OK.
RUSS TEDRAKE: But certainly, in the kinematic case, you could definitely imagine cases where the backwards could be faster. Let's say it's hiding in some little island, and you just want to get out. And that's much easier than finding your way into the island, let's say.
So certainly, we've noticed sensitivities to growing RRTs around very unstable fixed points. And it might be exactly what you said. It might be. Yeah.
AUDIENCE: So is the Euclidean norm the best measure for this?
RUSS TEDRAKE: Good. The Euclidean norm is a horrible metric for this. What do you think would be a better metric?
AUDIENCE: Energy or times [INAUDIBLE].
RUSS TEDRAKE: Awesome. So energy's a good candidate. In fact, I think, depending on what we put on the problem set, we might ask you to try an energy metric to do it. So almost everybody says this would work better with a better distance metric. And almost nobody can tell you a better distance metric for these kind of systems.
So we actually have a couple of ideas that we've been working on in lab. Elena has been working on some LQR-based distance metrics, for instance. So what did you just say? You said time to arrival. How would you compute a time to arrival?
AUDIENCE: You have to do a moment in time.
RUSS TEDRAKE: Right.
AUDIENCE: Muscle control.
RUSS TEDRAKE: Exactly.
AUDIENCE: [INAUDIBLE]
RUSS TEDRAKE: So you just guessed the one that's one of my favorites right now. So one idea here, not fully proven yet, but it's still in the oven here is that, every time, I pick a random sample point I'll linearize the system around that point. Say that's a cartoon of a linearization.
Compute a LQR stabilizer around this. And then use the value function for a minimum time problem. So compute a min time LQR actually around this point. And then use the value function as the distance metric.
So I like that idea a lot. There's a couple subtleties that go into it. So when you pick a random point in state space, if it's got a velocity or-- most points you pick randomly in states space are not stabilizable.
So you can't do an infinite horizon minimum time LQR. You have to do a finite horizon and then potentially search over horizon times, but you can do that, actually, pretty efficiently. The other issue about it is that it's actually not a proper distance metric. Why is it not a proper distance metric? Anybody?
AUDIENCE: It doesn't [INAUDIBLE].
RUSS TEDRAKE: So I think--
[INTERPOSING VOICES]
It probably doesn't follow the triangle inequality. It doesn't even have symmetry. If I were to linearize around this point and compute its distance, linearize around this point, there just absolutely no guarantees that it would be the same.
But I don't care about that actually. I just have to put it in the paper. As long as it works, right? It's also more expensive a little bit, but it can make a dramatic improvement in the way it spreads.
There's another idea. I was actually planning to talk about some of the more subtle ideas next time. But since we're at it, the other way to do it is to change the sampling distribution.
So Alec Shkolnik in our group has got a very clever way to grow these trees, where it uses a sampling metric that doesn't try to go where it can't go. Let me see if I can say that more carefully. I told DARPA about it three days ago. So I just want to show you the picture.
OK. So this is Alec's algorithm for being more clever at expanding. He changes the bias to the point where this is the tree we just saw it, basically, right? And his new tree looks like that. So it looks pretty compelling.
How did he do it? He does this thing he calls reachability guided RRTs. Every time he expands a node, he also expands what would happen if I had applied maximum torque and minimum torque.
So he expands a couple nodes that represent a cone of feasibility. The algorithm turns out to be trivial. So now, I take my Euclidean distance metric or whatever it is, pick my random sample point.
If the closest point is one of these boundary feasible things, that means there's room to grow the tree. If the closest point, however, is one of the nodes in my actual graph, that means I'm not feasible. I said that quickly. But long story short, there's a trivial check here, which effectively changes your sampling distribution just by saying sample a point. If the closest point on there is not one of my boundary regions, but one of my normal tree elements, then I know I can't grow there.
And if I get that, I just throw that point away, pick another point. What that does is it effectively changes the sampling distribution to be the green region. It just throws out all these unnecessary points that you can't get towards and changes your sampling distribution, so it's only trying to grow in places that it can actually go, to where the trees got a chance of growing towards.
So I think the world is still sort of small enough that people are still finding pretty clever little tricks to the RRTs that make them grow a lot better in constrained systems like this. So that happens to be, I think, a very good one for our kind of problems. But it also points out what RRTs are bad at.
So what kind of problems would you expect an RRT to work on? And what would you not expect an RRT to work on? The classic case of something you shouldn't expect an RRT to do very well on is something that looks like this, let's say.
x start is over here. x goal is over here. And my obstacles look like this. In general, I think it's fair to say RRTs don't like tunnels and tubes.
What's going to happen if I run my RRT on this sort of a system? So you know, I got my tree here. Pick a point here, fine. Pick a point here, I'll quickly cover this area.
Pick a point here, I throw it away. Pick a point here, I throw it away. Pick a point right here, can't get to it. It doesn't do me any good.
You know, after a very long time, maybe I luck out and I pick a point here, right? But then I have to wait, and I pick point here. I can't get to it. You know, I'm just completely hopeless.
If it's in two-dimensional space, then you can imagine sort of getting lucky enough or designing a sampling distribution that does this. But let's say it's in some 14-dimensional space that you have no idea where the tunnels, the tubes, are. There are some problems where RRTs just choke.
AUDIENCE: So in general, when there's basic solutions, feasible solutions, it's small? It's that fair to say? Because this is a very small set of [INAUDIBLE] space.
RUSS TEDRAKE: It's a little hard to say that, right? Because, I mean, lots of trajectories work, right? This one works this. This works, you know? But I think, when there's narrow passages, I think that's a more clear way to say it when it chokes.
AUDIENCE: And so when you say chokes, it means it's, as times goes to infinity, it'll finally find it.
RUSS TEDRAKE: Yes.
AUDIENCE: It is [INAUDIBLE].
RUSS TEDRAKE: Yes.
AUDIENCE: OK.
RUSS TEDRAKE: Good. So these algorithms, mostly what you look for in a planning algorithm is completeness. You want to know that, as you expand all the nodes, eventually if there's a path to the goal, you'll find the path to the goal. And even if there's not a path, it'll tell you there's no path, right?
RRTs have probabilistic completeness guarantees. They say, if you expand enough nodes, then with the probability one, if there is a path, you'll find it. Probabilistic completeness doesn't do as much for disproving the existence of a path. It would take a very long time to disprove it.
But probabilistically, these things will find a path to the goal if it exists. That's good. Maybe that'll make you sleep better at night. But if you have to wait for something to be probabilistically complete, it's not going to be useful.
The reason these things are used like crazy is that, in practice, they're very fast. They can be very, very fast for finding good solutions. So really, I mean, the Grand Challenge vehicle is using RRTs all the time to consider possible future paths along the street, trying to avoid obstacles.
It's doing it fast enough sort of in a receding horizon way. It's always planning a minute ahead. I'm not sure exactly what the window is in driving time. And it's doing it every DT roughly, re-computing its plan. These things can be really, really fast in practice.
To some extent, the dynamic constraints which we're talking about on the pendulum are exactly this problem. Because the pendulum, like we said, it's coming here. We know what the phase portrait of the pendulum look like. And we know that, if we're torque limited, there's only a handful of places we can go.
To some extent, kinodynamic constraints, like this, dynamics can look exactly like this. So if you take your vanilla RRT, you saw how effectively it grew on the pendulum. It took a lot a lot of nodes to find its way out. The hope is that we know enough about this sort of dynamical system from some of our other tools, like LQR, to do distance metric from tricks like this that we're effectively guiding this thing down the differential constraint tubes.
AUDIENCE: Have you had any [INAUDIBLE] with relaxing the number? Like, for example, if you have like a 15-dimensional robot, you're going to get to [INAUDIBLE] position. Can you relax enough those dimensions and then use the rest [INAUDIBLE] under control in a very low dimensional space and then use that as a [INAUDIBLE]?
RUSS TEDRAKE: Awesome. Yes. So Alec also had-- I'll tell you what, my next slide, this is Alec's other thing about planning in a task space, which is exactly what you said I think.
There's lots of ways to do it. And I think Alec found a really good way to do it. This is just an example of a five-link robotic arm just in configuration space.
They had to find its way from that endpoint to the goal endpoint. So you see at the top, that little green arm is the resulting configuration of the robot. The standard RRT looked all over the place to try to find that solution.
It's a little subtle how he had to do it. You had to basically change the Voronoi bias to live in the task space, but still sample uniformly from the other space. So that's not enough to tell you how to do it, but that's enough to tell you it's not completely trivial.
Then he found these really, really elegant solutions to plan in task space. And the result, his sort of killer plot, is that this is the number of links in the robotic arm versus the number of nodes it took to find the goal. And the standard RRT went like that.
And after 10 links, it just was hopeless. With n going to over 1,000 dimensions, he was just planning in constant time basically. So, yeah, there's the structure in the problems.
We also, using the partial feedback linearization task space, have tried doing it on underactuated systems. And that sort of works, too. Yeah. But it's not the norm, actually. It's not the accepted version of the algorithm yet.
RRTs are beautiful things, right? They just grow out, find their way to the goal. Potentially, a complaint is, when you're done with it, you've got the sort of choppy discretized path that gets you to the goal.
But then just hand it to your trajectory optimizer as an initial condition and let it smooth it out, still satisfying obstacle constraints, torque constraints, We know how to do that and smooth it out. And you've got yourself a good trajectory that gets from the start to the goal.
It's a beautiful thing, I mean, I think to the point where Tomás Lozano-Pérez invented configuration space, did motion planning for however long. He actually stopped robotics, went off and did computational biology for a while. And he says he came back to robotics just a few years ago because these kind of algorithms made a difference to the point where it was worth doing motion planning robotics again. I don't a more dramatic thing to say than that.
OK. So next time, I'm going to go a little bit more into using these planning algorithms and also feedback motion planning applied to our model systems, but I don't want to completely segue. That's a big idea. These RRTs-- simple algorithm, big idea.
I mean, the idea that a randomized algorithm can do better than our explicit algorithms, it's all these funny things. There's classes on randomized algorithms here. Every once in a while, these things just really work nicely. Randomization can help.
And the reason, roughly, may be is that it's actually pretty cheap to pick a sample point. It's pretty cheap to evaluate if it's in a collision. And so why not just pick a lot?
Instead of trying to reason about the geometry of your obstacles, building configuration spaces, that's hard compared to just saying, OK, what if my robot was here? Was that a collision? Yeah, that's in collision. OK. Throw it out. And just sometimes randomized algorithms are better. OK, see you next week.