Stanford's Pavone Rules Out Exact Planners Above Four Dimensions

斯坦福大学Pavone指出精确规划器无法应用于四维以上空间 经典组合运动规划在实际机器人应用中失效，原因是在超过四个自由度后，计算显式构型空间障碍物在数学上无法求解，迫使自主系统依赖启发式网格搜索和采样算法。 Marco Pavone教授介绍了机器人运动规划的形式化基础，将其阐述为非凸障碍物约束下的最优控制问题，并对比了势场法与离散图搜索方法。他详细讲解了工作空间离散化、标签修正算法、队列扩展策略（包括DFS、BFS、Dijkstra和A*搜索），以及可采纳启发式在剪枝搜索空间中的作用。最后，讲座将网格搜索延伸至连续构型空间，解释了Minkowski和、梯形分解等路线图方法，并阐明了高维环境下的计算瓶颈为何必然要求采用基于采样的规划方法。 机器人运动规划被形式化为非凸障碍物约束下的最优控制问题，涵盖势场法与离散图搜索的对比。 深入探讨了工作空间离散化、标签修正算法，以及结合可采纳启发式进行搜索剪枝的DFS、BFS、Dijkstra和A*队列扩展策略。 阐述了Minkowski和与梯形分解等连续构型空间路线图方法，指出四维以上维度的计算瓶颈使基于采样的规划成为必然选择。

Professor Marco Pavone frames motion planning as an optimal control problem with non-convex state constraints, tracing how classical potential fields, grid discretizations, and combinatorial geometry lead to modern search methods. While exact geometric methods become computationally intractable in higher dimensions, grid-based algorithms like A* provide practical, resolution-dependent paths for low-dimensional systems. Motion Planning Formulation and Algorithmic Paradigms (1:15) Motion planning extends optimal control by requiring autonomous systems to transition between states while strictly adhering to non-convex obstacle avoidance constraints and physical dynamics. Early approaches relied on artificial potential fields, treating the goal as an attractive potential valley and obstacles as repulsive hills. While potential fields allow fast computation via single-step gradient evaluations, Pavone notes they suffer from a fundamental defect: robots frequently become trapped in local minima before reaching the target. This limitation historically divided motion planning into four core paradigms: potential fields, grid-based graph search, combinatorial planning, and sampling-based planning. While combinatorial planning seeks exact, lossless continuous representations and sampling-based methods use adaptive continuous sampling, grid-based methods discretize the continuous workspace into vertices and edges, balancing computational tractability against approximation resolution. Grid Discretization and Label-Correcting Search (15:43) Converting a continuous workspace into a graph $G = (V, E)$ introduces trade-offs between cell resolution and computational cost. Finer grids yield closer approximations to the true continuous optimum but trigger exponential growth in vertex count. Furthermore, grid topology induces metric artifacts; for example, a 4-connected grid constrains diagonal traversal between $(0,0)$ and $(1,1)$ to a distance of 2 rather than the true Euclidean distance of $\sqrt{2}$, an error that persists regardless of grid refinement. To simplify geometric collision queries, practitioners inflate workspace obstacles by the radius of a bounding shape (such as a circumscribed circle) and collapse the physical robot to a dimensionless point. Graph traversal relies on label-correcting algorithms that track a tentative cost-to-arrive $C(q)$ initialized to infinity, with the start node set to zero. At each iteration, candidate nodes are popped from an open queue to relax adjacent neighbors: if routing through node $q$ provides a cheaper path $C(q) + c(q, q') < C(q')$, the neighbor's label is updated. Search behavior depends on queue discipline: depth-first search uses a last-in, first-out queue with a small memory footprint; breadth-first search operates as a first-in, first-out queue (historically corresponding to Bellman-Ford); and Dijkstra's algorithm prioritizes the minimum cost-to-arrive. Pavone emphasizes that under Dijkstra's monotonic expansion, each node enters the frontier queue exactly once. Informed Heuristics and the Efficiency of A* (45:53) Because Dijkstra's algorithm prioritizes nodes solely by cost-to-arrive $C(q)$, it performs an uninformed search that expands isotropically across the graph like a circular wavefront, expending equal effort in directions opposite the goal. Informed search resolves this inefficiency by incorporating a heuristic estimate of the cost-to-go, evaluating candidate nodes by $\text{Priority}(q) = C(q) + h(q)$. To preserve the guarantee of finding the optimal path on the discretized graph, the heuristic $h(q)$ must be an admissible lower bound that never overestimates the true remaining cost. Straight-line Euclidean distance is standard in robotics because obstacles can only increase the actual path length. Incorporating this lower bound directs node expansion toward the target and strengthens pruning: any candidate satisfying $C(q') + h(q') > U$ (where $U$ is the best complete path cost found so far) is discarded immediately. While A* substantially reduces queue expansions compared to Dijkstra, Pavone highlights that uniform grid methods still suffer from the curse of dimensionality, scaling on the order of $\mathcal{O}(k^d)$ and becoming computationally intractable beyond four dimensions. Configuration Space and Combinatorial Intractability (53:30) Continuous motion planning formalizes robot motion within configuration space ($\mathcal{C}$-space), reducing the physical robot to a point navigating free space $\mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}$. For a planar rigid body, the space is formalized as a subset of $\mathbb{R}^2 \times S^1$, requiring algorithms to account for angular equivalence ($0 \equiv 2\pi$) to avoid spurious full-circle rotations. Combinatorial planning attempts to construct discrete roadmaps that satisfy accessibility (collision-free paths from start and goal to roadmap nodes) and connectivity (preserving path existence) via techniques like trapezoidal cell decomposition. However, exact combinatorial approaches break down in practical robotics because constructing analytical representations of $\mathcal{C}_{\text{obs}}$ is extraordinarily difficult. When planar translation is augmented with continuous rotation, planar obstacle boundaries morph into complex, spiraling three-dimensional geometric volumes. For systems with five or more degrees of freedom, exact semi-algebraic formulations exhibit double-exponential time complexity in the dimension of the configuration space. Pavone stresses that because explicit mathematical boundaries of $\mathcal{C}_{\text{obs}}$ are almost never accessible for high-degree-of-freedom systems, modern robotics relies on sampling-based methods that evaluate collision queries point-by-point in the workspace. Grid-based graph search—specifically A*—remains an essential workhorse for low-dimensional autonomous navigation, balancing simplicity against resolution limits. Because exact combinatorial methods scale poorly and require explicit algebraic representations of configuration space obstacles, modern robotics predominantly turns to sampling-based algorithms when planning in high-dimensional state spaces.

[0:04] Prof. Marco Pavone: The hardest part of this course is over. [25:44] Prof. Marco Pavone: I can't tell you the radius. For whatever reason, that is considered secret. But you can assume what the radius would be. [51:26] Prof. Marco Pavone: reference planner for the rest of the quarter. [51:26] Prof. Marco Pavone: Yep. By the way, there is no notion of most optimal path, the optimal path. [58:00] Prof. Marco Pavone: And the answer is no. That's why motion planning is a hard problem. [58:00] Prof. Marco Pavone: is almost always not available. [58:00] Prof. Marco Pavone: I would say that a good fraction of roboticists missed this point. I routinely review papers where this point is missed, and the claims that people make are completely nonsensical. [68:03] Prof. Marco Pavone: really pain [70:09] Prof. Marco Pavone: No way you can do it. [72:20] Prof. Marco Pavone: really a pain in the neck, [74:18] Prof. Marco Pavone: bonus material [74:18] Prof. Marco Pavone: practically, not that useful

[0:04] Correct. [0:05] So it's a good news for you. [0:08] The hardest part of this course is over. [0:10] The hardest part of this course are the first two weeks [0:12] for two reasons. [0:14] One, because the topic of trajectory optimization [0:16] is a little bit complicated because it [0:18] entails quite sophisticated concepts in mathematics, [0:24] differential equations, and so on. [0:26] From now on, the difficulty of the class [0:28] is going to stay the same level or lower. [0:32] Second of all, another reason why, typically, the first two [0:35] weeks are the hardest for this course [0:36] is that, at the same time, you also [0:38] need to get used to the tools of the trade. [0:42] So basically, how to use a terminal, [0:45] how to revisit your knowledge of Python and so on. [0:52] So expect that by now, you've done all of that. [0:55] So that is also going to be settled [0:58] for the rest of the quarter. [0:59] So if you've been fine for the first couple of weeks, [1:02] you'll be enjoying the rest of this class for sure. [1:05] Any questions or concerns? [1:11] Any questions about the midterm? [1:14] All right. [1:15] So today, we're going to start the first lecture of the two [1:20] lectures on the topic of motion planning. [1:23] As I referred to and mentioned many times, [1:29] motion planning can be seen as an instantiation [1:32] of optimal control problem, whereby [1:34] we account for state constraints, complicated state [1:36] constraints. [1:38] In particular, state constraints are, in general, [1:40] non-convex constraints. [1:42] Motion planning problem at a high level [1:44] can be defined as a problem of computing a sequence of actions [1:49] that drive a robot from an initial condition [1:53] to a terminal condition, while avoiding obstacle, [1:57] respecting motion constraints, and possibly optimizing [2:00] a cost function. [2:01] We have already seen techniques that [2:04] aim at planning motions to respect motion constraints, [2:07] possibly optimizing a cost function. [2:10] But so far, we have not really dealt [2:11] with the topic of how to handle state constraints. [2:16] In particular, as I said, the obstacle avoidance constraints. [2:19] It's clearly for robotic systems are of pivotal importance. [2:22] So today and on Thursday, we're going to do exactly that. [2:26] In particular, the goal of today is [2:28] to give you an introduction to the topic of motion planning, [2:31] also with a little bit of historical context, [2:33] and then to learn about the most basic techniques in motion [2:38] planning that are rooted in the notion of discretization [2:42] and graph search. [2:44] These are the very first algorithms [2:46] that were introduced and are widely used in practice. [2:49] For example, Mars rovers use a variant of graph search [2:55] algorithms to plan their motion on the surface of Mars, [2:59] variant of this algorithm that we will see later on. [3:01] Readings for today. [3:05] One of the main references for motion planning [3:07] is a book by Steve LaValle-- a little bit [3:09] dated but now is from 2006. [3:12] But there has been a lot of progress in the past 10 years [3:15] but still a very good reference. [3:16] In particular, the material covered today [3:18] is taken from chapter six. [3:20] Another very good reference is a book [3:22] from Dimitri Bertsekas on Dynamic Programming and Optimal [3:25] Control. [3:26] In particular, this book has a chapter, chapter two, [3:28] that does a very good job at covering search algorithms, [3:32] in particular, the class of search algorithms referred to as [3:36] a label-correcting methods. [3:38] That is the class that we're going to cover today. [3:43] Motion planning, obviously, is ubiquitous in robotics. [3:46] Just to give you some context and some excitement, [3:50] here I'm showing one of my students [3:52] testing his motion planning algorithms against a quadrotor. [3:59] Dispensing with the quadrotor. [4:00] The quadrotor is trying to plan its motion, obviously, [4:02] by avoiding a state constraint represented by this word. [4:07] So this is what I really mean by motion planning-- trying [4:10] to reason about motion constraints [4:12] and at the same time constraints in the environment that [4:16] can be even time varying, adversarial, [4:18] in this case, uncertain, and so on. [4:21] Motion planning, as I said, is ubiquitous. [4:23] For the sake of this class, our key application domain [4:26] will be the application of trying [4:28] to find a path for the wheeled vehicle that [4:31] avoids obstacles but can be used in many other settings. [4:36] For example, in order to control a humanoid robot [4:38] that is trying to grasp a cup, but it [4:41] has to account for the fact that the cup might be in a drawer. [4:45] So it has to be very careful in not hitting [4:47] the walls or the drawer. [4:49] Another exciting application of motion planning [4:51] is how to carefully maneuver robotic probes in order [4:56] to perform surgeries, like, for example, how [4:59] to maneuver the probe in order to get into the blood vessels [5:03] and then deliver a medicine at a very specific target location. [5:07] Or it can even be applied to domains [5:09] that have very little to do with robotics, like, for example, [5:12] protein folding. [5:14] You can think of the problem of protein folding [5:17] as a problem of sequencing a number of maneuvers [5:20] in order to get to the desired protein. [5:22] That can be cast, believe it or not, [5:25] as a motion planning problem. [5:26] So very general problem, very important. [5:29] Not surprisingly, people have been working on it [5:32] for the past several decades. [5:34] The problem was the first formulated in the '70s. [5:39] And the first exact algorithms-- we'll discuss a bit better what [5:43] they mean by exact algorithms later on-- were developed [5:47] in the '80s. [5:49] The problem with the exact algorithms [5:51] is that, in general, they do not scale well [5:54] to high-dimensional spaces. [5:56] By high-dimensional, I mean to robots. [5:59] Whereby the configuration vector has [6:03] a number of dimensions that is, say, larger than four or five. [6:08] So as long as you have a unicycle, you have XY-Theta, [6:10] three-dimensional, it's fine. [6:13] But you have kinematics, so it's a little bit more complicated. [6:16] Four is fine. [6:16] Five tend to be already complicated. [6:19] Unfortunately, this problem has inherently [6:22] an exponential scaling with respect [6:24] to the dimension of the configuration vector [6:26] in terms of complexity. [6:28] This prompted people in the '90s to look at approximation [6:31] algorithms. [6:32] So algorithms that deliver a solution, that is good enough. [6:39] But the advantage is that can be done very quickly. [6:41] And this is a recurrent theme in robotics. [6:44] Since the real-time implementation [6:47] is of such of importance, oftentimes, you're [6:49] willing to sacrifice a little bit [6:51] of the correctness of the algorithm [6:52] or the quality of the output in favor of real-time computation. [6:58] 2000 have seen widespread application [7:00] of these methodologies on real systems. [7:03] I already mentioned Mars rovers, but obviously, [7:08] self-driving cars, drones. [7:10] You name it. [7:11] Basically, every robot that moves around has to reason about [7:15] environment, meaning that the environment cannot be completely [7:19] predicted at the design stage, has to run in a form of another [7:23] at motion planning algorithm. [7:26] If you already know exactly what the robot environment [7:29] that the robot is going to operate in, then, in principle, [7:32] you can script the trajectory of every configuration, [7:35] and you don't have to run a motion planning algorithm. [7:38] But in general, that is not possible, so you have to do it. [7:41] And in particular, each one of you, every day, [7:43] runs a motion planning algorithm in your brain. [7:46] This is a very active field of research. [7:49] Here is a very partial list of topics people are currently [7:53] working on. [7:54] For example, inclusion of increasingly sophisticated [7:57] motion constraints and the motion planning process, [7:59] even logical constraints. [8:01] What I mean by logical constraints [8:02] is that as you plan the trajectory for your robot, [8:07] you account for logical constraints, [8:10] like, for example, rules of the road. [8:12] Think of a self-driving car as it plans its maneuver-- [8:15] not only has to obey collision constraints but also has to obey [8:19] rules of the road, cannot move laterally if there is a solid [8:25] lane. [8:26] Planning under uncertainty. [8:27] In many applications, you have imperfect state information, [8:31] so you don't measure directly, for the case [8:33] of unicycle, what XY-Theta are. [8:36] But you have a noisy version of XY-Theta. [8:39] So how do you plan your trajectory if, for example, you [8:41] don't know exactly what your configuration is, [8:43] for example, you don't know exactly what your position is. [8:47] Parallel implementation, of course, [8:50] there has been a lot of development from the hardware [8:52] side, for example, with the advent of GPUs. [8:56] So one active research topic is how do we [8:58] design algorithms that are specifically [9:01] targeting the platform over which these algorithms will [9:05] be run, which means thinking about parallel versions [9:13] of the algorithms that can be effectively implemented, [9:15] for example, GPUs. [9:16] And finally, motion planning, as in general, trajectory [9:20] optimization, is going to deliver [9:22] you an open-loop trajectory. [9:24] So the question is, how do you endow that open-loop trajectory [9:29] with a feedback tracking, feedback correction term? [9:32] Very similar to what we already did [9:34] in the context of trajectory optimization [9:36] through the lens of differential flatness. [9:41] So let's dig a little bit deeper into the problem [9:44] of motion planning in the simplest possible setup. [9:47] So assume that we have a two-dimensional environment, [9:50] and then we have a LEGO block that we [9:53] have to move from an initial condition to a final condition. [9:57] So the motion planning problem entails [9:59] finding a sequence of maneuvers, like for example, translations [10:05] and rotations that deliver your LEGO [10:07] block at the final destination. [10:10] This is a simple problem because it's in 2D and because you [10:14] don't have a complicated motion constraints. [10:18] You can move the LEGO block laterally. [10:20] You can rotate it as you want, and so on. [10:25] Now, there are several approaches to motion planning. [10:31] One of the very first one is referred to as potential fields. [10:36] The idea is to create potentials, whose derivative [10:40] gives the forces on the robot. [10:44] These are virtual forces on the robot [10:46] that pull it toward the goal and repel it from the obstacles. [10:56] Let's assume that this is your 2D environment. [11:01] So this is an obstacle. [11:02] This is the goal. [11:07] Actually, let me turn the perspective. [11:14] Maybe like this. [11:15] So this is your obstacle. [11:17] This is your goal. [11:19] So the idea is to create artificial potentials. [11:22] So basically, hills around the obstacles and the valleys [11:27] around the goal so that you can think then of a robot [11:34] as a sort of a ball that is moving [11:37] in this kind of a hill-shaped environment. [11:39] It is repelled from the obstacles [11:42] and is attracted toward the goal. [11:45] Now, the problem is that if you don't design these fields well, [11:48] there is a danger that your ball gets stuck in a local minima [11:52] and never reaches the goal. [11:53] So that's one of the main difficulty [11:57] with the potential field motion planning. [11:59] The advantage is that it is a very intuitive [12:02] in a different way of setting the hills. [12:04] But usually, that is very intuitive design. [12:06] So this is good. [12:07] It's extremely fast. [12:09] The only thing that you have to do in order to control the robot [12:12] is at each point in the state space, [12:15] you look at where you are. [12:19] You compute the gradient of your potential. [12:22] That basically gives you the direction with respect [12:24] to which your motion should go. [12:26] And then you just follow it for one-time step. [12:29] So from a computational standpoint, [12:32] it's extremely effective. [12:33] On the other hand, it's prone to this issue of getting [12:36] stuck in local minima. [12:38] So designing potential so that you don't have this issue [12:42] is extremely tricky. [12:45] This has prompted the design of other classes [12:48] of planning algorithms. [12:49] Another approach, and this approach is extremely popular, [12:53] is to take the problem that is naturally [12:56] defined in a continuous space, and then [13:00] discretize the environment and turn the problem on motion [13:04] planning, in the problem of searching on a graph, whereby [13:08] the graph represents the underlying discretization [13:12] of your environment. [13:14] And this is something that we're going to look in more detail to. [13:17] This class of algorithms belong the Dijkstra algorithm [13:21] for motion planning-- [13:22] A-star, D-star, and so on-- [13:25] very popular in robotics. [13:29] Third class is referred to as combinatorial planning. [13:32] So here, the idea is to construct structures [13:36] that allow you to discretize the problem, [13:39] yet retain all the information that is needed in order to solve [13:43] the motion planning problem. [13:44] So basically, the idea is to have a discretization that [13:46] is lossless. [13:47] If you're able to solve the discretized problem, [13:50] the solution that you get will be [13:52] the same as if you were solving the continuous problem-- [13:57] different from the grid-based planning approach, [13:59] whereby you have some approximation in the output [14:05] that you compute. [14:07] Now, this seems great because, basically, you're [14:09] turning the problem into something you can do computation [14:12] with without losing any information that [14:16] stems from the continuous formulation of the problem. [14:19] The disadvantage is that the computation of these data [14:25] structures is, in general, complicated. [14:28] It doesn't scale well with the dimension of the problem. [14:32] So this is the reason why, actually, this approach is not [14:35] very widely used in practice. [14:37] It doesn't really scale to most of the problems that [14:40] are of practical interest. [14:41] But in some cases, it's still useful. [14:43] And finally, a fourth class of motion planning algorithms [14:48] is represented by a sampling-based planning, which [14:51] tries to somehow merge the advantages [14:56] of combinatorial planning with the grid-based planning. [14:59] So the idea is to still try to reason [15:01] about the continuous problem, but do computation with respect [15:06] to a discretization that is a bit more [15:09] adaptive than what you would do with a standard grid-based [15:11] approach. [15:12] And we'll see this in much more detail. [15:13] In particular, in this class, we're [15:15] going to focus on these three classes of planning algorithms-- [15:22] grid-based, combinatorial, and sampling-based. [15:24] In particular, today, we're going [15:25] to talk about grid-based and combinatorial. [15:27] And on Thursday, we're going to talk about sampling-based. [15:30] And we're then going to summarize all of them [15:34] and draw a conclusion about the relative advantages [15:36] and disadvantages. [15:38] So that's a plan. [15:43] So let's start with the grid-based approaches. [15:47] Grid-based approaches are arguably the most natural one. [15:52] So what you do is you have a motion-planning problem that [15:58] is defined over a continuous space. [16:01] Say, for example, a square. [16:06] And then you have obstacles here and there. [16:13] Something like that. [16:14] So this is your start point. [16:17] This is your goal region. [16:19] So what you do is just to discretize the problem. [16:23] So you just grid your environment. [16:34] You put a representative point within each cell and so on. [16:50] You connect these points, for example, [16:54] by using four neighbors or maybe eight neighbors. [17:05] And then you clip your goal and your start position [17:10] to the closest point on the grid. [17:15] So having done that, what you're left with [17:18] is a graph, meaning a set of vertices-- [17:23] these points over here-- [17:25] and a set of edges, the links between these points. [17:33] So now the motion-planning problem-- yes. [17:36] To what level do you [INAUDIBLE]? [17:40] Right. [17:40] So that's one of the pain points of grid-based approaches, [17:45] is that the quality of the solution, [17:47] in general, depends on the resolution. [17:50] You have to be a little bit careful with the-- [17:53] I'll come back to this in a second-- [17:55] on how you choose your neighbors. [17:57] But in general, as the resolution [17:59] gets smaller and smaller, then the solution that you compute [18:03] is going to converge to the one that you would compute [18:06] in the continuous problem. [18:07] The question is that as the resolution gets smaller, [18:10] then the number of vertices gets larger, [18:13] which basically means you run more computation, which [18:15] basically means that this becomes more compute intensive. [18:20] So there is a trade-off. [18:21] Nobody tells you what the trade-off is there. [18:24] It really also depends on the obstacle space. [18:27] So indeed, the resolution is something [18:29] that is very difficult to tune. [18:31] So what people do is a little bit of a trial and error, [18:35] even in real time, and also try to do adaptive grids, whereby [18:41] you have a finer discretization in regions [18:44] of the environment when you think [18:46] it's more promising to do motion planning and coarser [18:49] discretization. [18:50] So there is a lot of work on dealing with that issue, [18:53] but in general, there is not an exact answer. [18:56] It is one of the difficulties and disadvantages [18:58] of grid-based approaches. [18:59] What I was saying is that you have [19:01] to be careful with the neighbors, [19:03] is that consider this example, whereby we are on a square, [19:12] discretized. [19:17] So you start here, and you want to get here. [19:20] So this is the goal. [19:21] This is the start. [19:23] And so we consider as points in the vertices these points here. [19:29] And we only connect them by looking at the four neighbors. [19:33] So essentially, we consider this connection, this connection, [19:35] and this connection. [19:37] We don't consider the diagonal connections. [19:41] So we want to get from here to here. [19:44] And we assume that each edge is of length 1. [19:50] So the optimal path, meaning that this path, [19:55] has length square root of 2. [19:58] But the problem is that any path that I [20:01] consider in this grid to go to the goal has length what? [20:09] What's that? [20:09] 2. [20:10] Because at least I have to go one up and then one right. [20:14] And this doesn't really depend on the resolution [20:17] if I make the resolution finer and finer, [20:29] so now this will be my connections. [20:32] I still have this issue that, at the very least, [20:34] I have to go one up and one right. [20:37] So basically, the length of the optimal solution [20:39] that I get by optimizing over this graph [20:43] is going to be equal to 2, no matter how [20:46] small the resolution is. [20:50] But you will say, OK. [20:51] So then the right way to do it is actually [20:53] to also include for the diagonal motions, which [20:57] is what people typically do. [20:59] The point of this example is to tell you [21:00] that some care has to be taken in order [21:04] to come up with the right neighborhood. [21:10] Having said that, then the resolution [21:11] is really a trade-off between the quality of the solution [21:14] and computation time. [21:15] What is the right solution is experience. [21:19] And there are some guidelines out there as well. [21:22] The key point is that the resolution problem [21:27] notwithstanding, we had translated the motion planning [21:29] problem into the problem of searching a path over a grid. [21:33] So something that is a prototypical problem [21:37] in the field of computer science. [21:40] So in other words, we have turned [21:41] the problem of motion planning into a graph search problem. [21:46] So then the next logical question [21:47] is, how do we best search a path over this graph? [21:53] So the class of algorithms that we're [21:55] going to consider in this class is a very broad class [21:57] of algorithms, referred to as label-correcting algorithms, [22:02] where the idea is to label each node [22:06] in the graph with a function q. [22:12] So this is the node q with a function C of q [22:15] that represents the cost-to-arrive [22:20] to this node from the origin, meaning from the start [22:24] point for your motion planning problem, [22:27] with respect to the best path that we have found so far. [22:32] And the idea is to iteratively improve all these levels [22:37] until we find an optimal path to the goal. [22:39] In particular, the way it works is relatively simple. [22:45] So this also referred to as a cost-to-arrive, as I said, [22:54] is the cost-to-arrive to node q with respect to the best path [23:02] that we've found so far, which is not necessarily [23:04] the optimal one. [23:05] This is something that we're going to iteratively improve [23:08] along the algorithm, but it's the one that the best one [23:11] that we have found so far. [23:12] If we haven't found any path to a given node, say, q prime, [23:17] then the algorithm is simply initialized [23:19] with Cq prime equal infinity. [23:22] And then progressively, this level goes down. [23:26] Of course, in practice, you cannot code infinity [23:28] in the computer. [23:29] It will be a very large number. [23:30] So, say, 1 trillion. [23:33] So the way these algorithms work is [23:35] that we maintain a list of other nodes or vertices-- [23:42] I'm going to use the words nodes or vertices equivalently-- [23:46] into a queue referred-- depends a bit [23:48] on the community as either frontier [23:51] queue, a live queue, priority queue. [23:53] These are all synonyms. [23:54] So this is a list of nodes that we are currently [24:01] considering as a milestones, meaning [24:04] as nodes that are included in the optimal path to the goal. [24:10] So currently, nodes that we are considering [24:13] for further exploration in order to see [24:14] whether they should belong or not [24:16] to the optimal path to the goal. [24:19] And the idea of label-correcting algorithms [24:21] then is as follows, is to iterate on the nodes. [24:25] And in particular, at each step we extract a node q or vertex q. [24:31] As I said, node and vertices are the same. [24:33] We extract the vertex from the frontier q, [24:38] and we look at all its neighbors. [24:54] So we extract node q for which we have a label Cq. [25:02] And we look at all its neighbors, [25:04] q prime, q prime 1, q prime 2, q prime 3, and so on. [25:12] These are nodes that can be reached from the origin [25:16] by going through q first, and then in one step, to q prime. [25:22] And then we ask the following question. [25:34] So now we focus on a specific neighbor. [25:43] So it will be q prime. [25:44] So is it possible to get to q prime through q [25:49] in a way that is cheaper than the current path that I [25:52] currently have found for q prime, meaning is C of q [25:57] plus the cost of going from q to q prime directly less [26:03] than C of q prime. [26:10] Again, what this means don't consider for now upper. [26:15] Consider just this. [26:17] So what this is saying is, can I get [26:20] to q prime by going through q and then directly [26:26] to q prime with the cost that is less than the cost of the path [26:30] that I have found in some previous iteration? [26:34] If yes, let's revise the path to q prime with this new path [26:40] that we have just found. [26:42] Does it make sense? [26:43] That's probably this one of the core routines [26:47] in a level-correcting algorithms. [26:50] Any questions about this? [26:53] Yeah. [26:54] So the notation Cq to prime is from q to q prime? [26:57] In one step. [26:59] Correct. [26:59] Yeah. [27:00] So in the case of the graph would be, [27:02] say, going diagonal one step. [27:07] Yeah. [27:09] A question about the grid formation. [27:11] So what's the role of the outer boundary of your robot [27:15] when you're forming this grid, assuming it's just a point? [27:18] Yeah. [27:19] No, very good question. [27:20] So typically, once we discretize the problem in this way, [27:26] we are essentially viewing the robot as a point. [27:30] But in general, the robot is not a point. [27:34] So for example, typical thing to do [27:36] is that if you have a robot with some weird shape, [27:40] say, with six wheels like a Mars rover, then what you do, [27:46] you build like a circle. [27:47] So I'm overapproximating an object. [27:51] And then you inflate all obstacles [27:54] by the radius of the circle. [27:56] And then you shrink the robot to a point. [27:58] Does it make sense? [28:00] This is basically what is done on the Mars rovers. [28:04] I can't tell you the radius. [28:05] For whatever reason, that is considered secret. [28:08] But you can assume what the radius would be. [28:13] Yeah. [28:13] What is your cost of arrival? [28:15] Is that half length? [28:17] Yeah. [28:17] Typically, it's path length. [28:21] So there are version of grid-based algorithms [28:23] that also account for motion constraints. [28:27] Here, will not account for any dynamics, and so on. [28:30] This is a purely geometric setting in which a typical class [28:33] is just a path length. [28:34] So then when you're doing this algorithm, [28:37] is the robot actually going down that path [28:40] and just going back and forth, or how is it? [28:43] Right. [28:44] How can it know ahead of time without traveling here? [28:48] So you have found a path like this one. [28:50] You've already found that? [28:52] Oh, yes. [28:53] So we're assuming that we have already [28:57] found another path to q prime. [29:01] Remember that we initialize all nodes with infinity. [29:04] It means that this is certainly the best [29:08] path that we've found so far. [29:09] Since there is no other paths, so we just consider this one. [29:12] Does it make sense? [29:13] So at the beginning, we initialize the cost-to-arrive [29:17] for all nodes as infinity. [29:18] Yeah. [29:19] So then this will pick a node and travel there? [29:22] No, we don't. [29:28] So this is an iterative algorithm. [29:30] So we start from the start, and we put the start node [29:35] into the open list. [29:37] So at the beginning, there is only one. [29:39] We pull it out, and we look at all its neighbors. [29:44] And then we say for all its neighbors, [29:46] is the current cost-to-arrive to this node less or larger [29:53] than the cost-to-arrive to denote by going to start [29:55] and then doing this one step? [29:57] The answer is yes because we are initializing [29:59] all the cost-to-arrive with infinity. [30:01] So we start building a tentative path, this path. [30:04] And then we put this node into the open list. [30:06] We do the same for this one and the same for this one. [30:09] So at the end of the first iteration, [30:11] we have these three nodes in the open list. [30:13] And then we start extracting from the open list, this one. [30:17] And then we look at its neighbors. [30:19] Look at it now. [30:20] I'm considering, for the second time, this node. [30:24] My last question is, how can you know the cost of arrival [30:27] to the neighbors without going there? [30:29] How is the robot knowing what the cost arrival is when [30:33] it's sitting at that moment? [30:35] So you start. [30:38] So this is the start position for your robot. [30:41] And this is the goal position for your robots. [30:44] And you have a map of the environment. [30:46] So you have a notion of-- [30:48] Oh, you have a map? [30:49] Oh, yeah. [30:49] Oh, it's a known environment. [30:51] Oh, yeah. [30:52] Oh, maybe this was not clear. [30:54] So thank you for the question. [30:55] Yeah, absolutely. [30:57] I thought it was an unknown environment. [30:59] No, no, no. [30:59] That's a very good point. [31:01] And thank you very much for that question. [31:03] So now I understand your question, [31:04] which is a very good one. [31:06] So here. [31:08] So this is your start position. [31:10] This is your goal position. [31:12] So from here, it seems that you don't have a visibility for what [31:18] you have here. [31:20] So how do you know if you can travel this corridor or not? [31:23] So in the standard motion planning formulation, [31:25] we're assuming that we have a static formulation. [31:28] So the map of the environment is known. [31:29] So the input to the algorithm is the full specification [31:32] of the problem. [31:34] So the full specification of obstacles and so on. [31:38] That's a standard setting. [31:39] Now, for example, in some applications, like warehousing, [31:46] robotics, and so on, is a very good one. [31:49] But say that you have a robot that [31:51] is exploring an environment. [31:53] Obviously, that robot. [31:54] Does not have information beyond its visibility [32:00] cone, which means that now you're [32:02] planning under occlusions. [32:04] That's the term, which is very much still [32:06] a state of the art topic. [32:08] So in that case, what you do is you [32:09] plan a trajectory within the known or observed space. [32:13] So this would be the environment with respect to which [32:17] you do motion planning. [32:18] And then you're careful in putting some intermediate goals [32:23] at the frontier of the environment. [32:24] The frontier of the environment is [32:26] what divides, what is seen from what is unknown. [32:29] And there are different ways of doing it. [32:31] And that gives rise to different algorithms. [32:33] This is a very relevant problem. [32:34] For example, when a self-driving car approaches an intersection, [32:38] part of the scene might be occluded. [32:45] So this is an intersection. [32:49] So this is your car. [32:50] Your car is not seeing what is here. [32:54] So that's what it means that we are planning under occlusion. [32:56] So this is beyond the scope of this class. [32:59] But this is, in many ways, still a physics problem [33:02] and a very wide problem. [33:05] Is everything else here? [33:08] All right, cool Yeah. [33:10] So please ask questions because sometimes I [33:13] take things for granted, having done [33:14] this stuff for several years. [33:16] We acknowledge that sometimes things are not so obvious. [33:19] All right, so we take a node. [33:23] We said that node q prime. [33:25] We may have a path that we have already computed to this node. [33:29] In this case, we refer to the cost of that path as q prime. [33:33] If we don't have that, q prime is set to infinity. [33:36] And then we ask the question, is the cost-to-arrive [33:39] to q prime through q lower than the cost [33:43] of the path we currently have? [33:44] If we don't have it, that will be infinity. [33:46] If yes, then we update the label of node, the q prime, [33:52] with the cost of the path that we've just found. [33:57] And then we put q prime into the open list. [34:01] Unless q prime is the goal node, in which case, [34:05] we found a path to the goal. [34:08] And we just iterate. [34:09] So we start with the start node. [34:12] We expand to its neighbors. [34:14] And then we keep, especially moving upwards. [34:17] So it's a sort of a wavefront. [34:21] So I think it's a very intuitive and reasonable algorithm. [34:24] Any other questions? [34:28] So why I refer to the level-correcting algorithms [34:32] as a class of algorithms instead of a single algorithm? [34:35] Because indeed, there are a number [34:37] of degrees of freedom in how you select a node that you extract [34:42] from the open list. [34:44] In general, you may have many. [34:46] Which one do you pick? [34:48] Randomly, with some more clever strategy and so on. [34:52] And how you strengthen some steps. [34:54] Like for example here, upper is an additional variable [35:00] that encodes the notion of the cost of the best path [35:04] that we've found so far to the goal. [35:07] Initially, upper is set to equal to infinity. [35:11] But then as we progress, we may find a path to the goal. [35:16] We don't know yet if that is the optimal path. [35:18] But say that we find a path to the goal. [35:21] So then the idea is that there is no reason [35:25] to further considering a node q prime if you already [35:28] know that the cost-to-arrive to q prime [35:31] is larger than the cost of a path to arrive to the goal [35:35] that we have already found simply because we are assuming [35:38] that the cost is non-negative. [35:39] So if the cost to arrive to q prime [35:41] is already larger than the cost of a path to the goal [35:45] that we already found, there is no hope [35:47] that an optimal path is going to go to that node q prime. [35:52] Make sense? [35:54] Any question, or you want a drawing? [35:59] Upper is basically an upper bound on the optimal cost, [36:03] initially set to infinity. [36:05] As soon as we find a path to the goal, [36:08] we set it to the cost of that path. [36:13] And then we discard the nodes whose cost to arrive [36:16] is greater than an upper bound on cost, [36:19] because there is no point in considering those paths, [36:21] because it is guaranteed that an optimal path is not [36:24] going to go through that point or node vertex. [36:29] All right. [36:31] So this is how it works. [36:35] This is the start position. [36:36] This is the goal. [36:37] The blue nodes are the ones that are currently [36:41] in the open list that are considered [36:42] for further evaluation. [36:44] And you see how this algorithm basically [36:46] expands outward, evaluating new neighbors at each stage. [36:54] The color basically denotes the cost. [36:58] Red means highest. [37:02] Green means lower, meaning that we are very close to the goal. [37:06] Does it require that you complete the entire path [37:09] before you start? [37:10] No. [37:11] Movement? [37:11] Or you can do this as you're moving through the world? [37:14] Oh, yeah. [37:16] You can do it. [37:17] But the danger is that you may not find [37:22] that the a priori optimal path. [37:24] But certainly. [37:26] And as I said, most likely, instead [37:29] of doing that, what you would do is [37:31] to have a finer discretization here and then a coarser [37:35] discretization here, and then move a little bit [37:38] and then recompute, with the idea that what really matters [37:41] is that you're very careful about what is around you. [37:44] So yes, there are a lot of variations. [37:45] But here in the philosophy of this class, [37:48] I want to give you the main concepts. [37:49] Yeah. [37:50] So when do you know when to stop and when [37:53] to stop picking out nodes? [37:55] So let me first summarize the algorithm, [38:02] and then I'll come back to that question. [38:04] So this is just a summary of what [38:06] we have just discussed so far, is mostly for self-consistency [38:09] of the slides. [38:10] The way the algorithm works is that we remove a node [38:13] from the frontier queue. [38:14] And for each child or neighbor-- child or neighbor are synonyms-- [38:18] we execute step two, which is whether we [38:21] see if we found a better path to q prime than what we had before. [38:28] At some point, the frontier is going [38:31] to become depleted of nodes, so it's empty. [38:36] At that point, we terminate the algorithm. [38:39] So then the natural question is once we the frontier q is empty [38:43] and we have terminated the algorithm, [38:47] are we guaranteed to have a path. [38:48] In particular, are we guaranteed to have [38:50] the optimal path with respect to the discretization? [38:55] And the answer is yes. [39:00] Actually, you can prove. [39:01] We're not going to do the proof that once-- [39:05] if a feasible path exists between the start and goal [39:08] nodes. [39:09] Here, the start node is referred to as qI, q init, q initial, [39:14] and q goal. [39:15] Then the algorithm terminates in finite time, [39:18] so it does not start oscillating or doing a weird stuff. [39:21] And the cost of the path that you find to the goal [39:24] is indeed the optimal cost to the goal. [39:27] That's a correctness that applies [39:29] to all variations of the label-correcting algorithm. [39:33] But there are several variations, [39:35] depending on how you pull nodes out of the queue. [39:38] You had a question? [39:39] [INAUDIBLE] seems pretty terrible began to you. [39:44] Well, no. [39:45] I mean, because some algorithms could be actually guaranteed [39:47] to converge only in infinite time, only asymptotically. [39:50] Yeah, we'll see that. [39:51] For example, sampling-based algorithms [39:53] are guaranteed to find a solution that is feasible [39:56] or that is optimal only when the number of steps [39:59] goes to infinity. [40:00] OK. [40:01] Now, actually, these algorithms are incredibly fast, super fast. [40:07] So running the algorithm per se is not what consumes time. [40:11] What consumes time is to build a discretization [40:13] and represent the obstacles. [40:15] But let me talk a little bit about the differences [40:18] about implementation of label-correcting algorithms. [40:23] As I said, there are many different ways [40:24] you can pull-- oh, by way, as already mentioned, but let [40:29] me mention it, to initialize the algorithm, [40:33] we set the labels of all nodes to infinity except for the start [40:38] vertex or origin vertex, whereby the label is set to 0, [40:41] and that vertex is placed into the open list or frontier list. [40:47] And the transition cost is something [40:49] you encode to the program. [40:52] Typically, in this setting is just a path length, [40:54] so it will be just Euclidean distance between the two points, [40:58] or in some cases, could be the Manhattan distance. [41:02] So basically, the path distance as you [41:03] go through the edges of the grid. [41:05] In that case, there's a large chunk of space that's open, [41:09] and it still works [INAUDIBLE] because we're [41:14] covered [INAUDIBLE]. [41:17] Yeah. [41:17] So if you have that knowledge, it [41:20] can be included into your discretization. [41:24] And that leads to multi-resolution discretization [41:30] schemes that I mentioned before, whereby you discretize more [41:33] finely in regions around the obstacles, [41:35] and you discretize less finally in open space. [41:38] Yeah, absolutely. [41:39] This is really the most basic version of the algorithm. [41:44] So different ways of implementing the algorithm. [41:47] How do we pull the nodes from the frontier list? [41:52] One way, obviously, is to do it randomly. [41:54] But there are better ways. [41:55] So one way is to implement the strategy of last [41:59] in, first out, which means that the last node that we put [42:04] in the frontier list is the first one that [42:07] is going to be retrieved from the frontier list. [42:10] This corresponds to a depth first search strategy, [42:14] whereby if we have a graph that looks like a tree, [42:18] we first go deep into a branch before moving [42:21] into the other branches. [42:26] This is opposite to what breadth-first search does. [42:30] The idea is to keep q as a list, whereby [42:33] we apply the strategy first in, first out, which means that, [42:40] basically, if we have a graph that looks like here as a tree, [42:46] we first explore all the vertices at one level [42:51] before going to the next level. [42:54] Because the first one that is going to be up expanded [43:00] is the first one that was put into the frontier list. [43:06] So these two strategies are really orthogonal to each other. [43:11] By the way, the breadth-first search [43:13] applied to label-correcting methods [43:14] is also referred to as the Bellman-Ford algorithm [43:16] for graph search. [43:21] The last one very popular is to pick the nodes in the frontier [43:26] queue that has the lowest cost-to-arrive, which arguably [43:30] is the most natural. [43:32] Since we're trying to find an optimal path, [43:35] it's natural to consider the candidate [43:40] as the most promising node along the optimal path, the one that [43:43] has lowest cost-to-arrive. [43:46] The downside of this approach, though, [43:48] is that performing this operation can be expensive. [43:53] So there are specialized data structures [43:56] that allow you to perform this operation fairly efficiently. [43:59] But in general, it takes a little bit longer. [44:03] So for problems where you have several nodes, [44:11] this operation could be time-consuming. [44:14] But the advantage is that, actually, the Dijkstra [44:17] perform a number of steps that is lower, in general, [44:20] than the other algorithms. [44:22] And one of the key properties of Dijkstra's that [44:24] is intuitive but not straightforward-- [44:28] you will need a proof that we're not going to do-- [44:30] is that under these exploration scheme, [44:33] each node enters the frontier queue only once. [44:36] In general, you might enter multiple times, [44:38] which are guaranteed by the theorem [44:40] that I stated before that these algorithms eventually converge. [44:44] But a node, in general, might enter the frontier queue [44:46] many times. [44:47] With the Dijkstra algorithm, it enters only once again. [44:53] This would require proof it's not entirely straightforward. [44:58] Any question about the different-- [45:02] for your P-set you're going to implement [45:05] a variant of the Dijkstra algorithm, [45:07] whether used in robotics? [45:10] When I think depth-first search, usually, it's [45:13] not finding optimal pathways, or unless you're exhaustively [45:17] searching the space [45:20] When depth-first is executed within [45:23] the label-correcting framework, it indeed [45:25] finds the optimal path. [45:27] But yes, you might eventually have to search the entire space, [45:31] absolutely. [45:32] Otherwise, yes, you wouldn't find the optimal path. [45:34] One advantage of the depth-first search [45:37] is that, at any point in time, you only [45:40] need to store in memory a part of the graph because you go deep [45:43] into a branch. [45:44] Well, with depth-first, you may need to store the entire graph [45:47] as you keep exploring. [45:53] In particular, this is the Djikstra algorithm, [45:55] how it looks like. [45:57] So you expand almost uniformly as a wavefront on the way [46:01] to the goal. [46:05] This is fine. [46:06] But for example, assume that you had action environment that [46:12] looked more like a circle. [46:14] The expansion will be isotropic, which [46:17] means that you would expand in this direction toward the goal [46:19] but also in the opposite direction. [46:21] The reason is that in the label-correcting algorithm, even [46:26] with the Djikstra strategy for node selection, [46:30] you only look at the cost-to-arrive, [46:33] but you don't have any biasing term [46:35] that tries to favor nodes that are closer to the goal. [46:40] So that's why you have a wavefront that [46:42] expands isotropically. [46:44] You're only capturing the notion of cost-to-arrive, [46:47] which does not capture any notion of where the goal is, [46:50] whereas to your start region. [46:52] So the point is that by running the Dijkstra, [46:59] also referred to as uninformed search [47:02] because you don't use the information of where the goal is [47:04] with respect to the start, you may be wasting [47:06] some computation because you are searching in a region [47:09] where probably you should not search, [47:13] which motivates the topic of informed search, [47:18] how to bias the search toward those nodes that [47:22] have more potential to lead you to the goal in a minimum cost. [47:30] So, as I said, by only account for the cost-to-arrive, [47:41] we said that the search is uninformed. [47:44] Now we're trying to include some information [47:46] about where the goal is with respect to the start vertex. [47:50] In order to inform the search, bias it more toward the goal [47:54] so that you don't waste effort in regions [47:56] where you know that, basically, an optimal path is not [48:00] going to go through. [48:04] One of the key ideas is to order the nodes not only by cost [48:09] to arrive-- that is what the Djikstra does-- [48:12] but by cost-to-arrive plus an approximation of the cost-to-go. [48:19] In particular, that approximation [48:21] has to be a lower bound for the cost [48:26] of the path between the node q and the goal, more specifically. [48:40] Let this be the goal. [48:43] This is your start node. [48:46] This is q. [48:48] You have found a path to q. [48:53] This is q. [48:56] In standard, the Djikstra nodes are ordered by Cq, [48:59] and you pick from the frontier queue, [49:01] the node that has lowest Cq. [49:04] The idea is to look at Cq plus an approximation [49:09] of the cost-to-go. [49:13] The cost-to-go is the cost of the path [49:17] from node q to the goal. [49:21] You don't know the cost exactly because you [49:23] haven't computed any paths yet. [49:26] But in many cases, you have an approximation [49:29] of that cost-to-go. [49:31] For example, a very simple approximation [49:33] is just a straight line that neglects all obstacles. [49:38] So basically, just Euclidean distance. [49:47] Euclidean distance. [49:54] So the idea then is to order the nodes by cost-to-arrive, [50:00] plus approximation of the cost-to-go. [50:03] In particular, for these procedure to be well posed, [50:07] the approximation of the cost-to-go, referred to as hq, [50:12] has to be a lower bound of the actual cost-to-go. [50:17] Otherwise, you might not include nodes [50:23] in your search that actually could [50:25] be part of the optimal path. [50:27] As I said, from all practical purposes, [50:30] very common choice is just the Euclidean length [50:33] of a straight line, neglecting all obstacles. [50:36] What this really matters is that in this approximation, [50:38] you have a lower bound. [50:39] So this is lower than the actual optimal path from node q [50:43] to the goal. [50:44] Second of all, it has to be something very quick to compute. [50:47] Usually, straight line works very well. [50:50] So then what you do is to order the nodes [50:52] by cost-to-arrival plus approximation of cost-to-go, so [50:55] the function hq. [50:56] And if you're doing the test with respect [51:00] to the upper bound to speed up the algorithm even further, [51:03] you strengthen the test by adding also the cost-to-go. [51:08] The idea, again, is that if you have a node q prime for which [51:14] the cost-to-arrive plus the underestimate [51:17] is larger than the best path that you have found [51:21] so far to the goal, then it's worthless to consider it [51:23] any longer. [51:26] Key point is that in this way, fewer nodes are [51:30] placed in the priority queue. [51:32] And in particular, your P-set. [51:34] So this strategy is referred to as the A-star algorithm, [51:37] one of the main algorithms for graph search. [51:40] And in your P-set, set you will implement it. [51:42] You will be implementing exactly A-star. [51:45] An A-star will be your reference planner [51:48] for the rest of the quarter. [51:52] So this is, again, the Djikstra. [51:54] As you see, uniform search expand as a wavefront. [51:58] This will be the implementation with A-star. [52:00] You see how now we are favoring those nodes that [52:07] are closer to the goal. [52:08] So it's much more direct way of searching. [52:12] The reason being that we include these approximate notion [52:15] of how far we are from the goal in order to bias our search. [52:19] So we don't waste effort around here or around here [52:22] because you already know that an optimal path wouldn't [52:24] go through there. [52:27] Yeah. [52:27] Will this also always find the most optimal path? [52:31] Yep. [52:31] By the way, there is no notion of most optimal path, [52:33] the optimal path. [52:35] Yeah. [52:37] All right, so to sum up, the advantage [52:41] of grid-based approaches is that they are simple to use [52:44] and, in general, very fast. [52:46] The disadvantage is that since you rely on a discretization, [52:49] as you pointed out, there is the issue of how [52:52] to select that discretization that oftentimes is not [52:56] very clear. [52:57] And moreover, since you are discretizing an environment, [53:00] the number of vertices scale exponentially [53:03] with the number of dimensions. [53:06] If you have a three dimension, then the number of vertices [53:10] will scale with 2 to the power of the dimensions. [53:13] So 3 is fine. [53:14] 4 is fine. [53:15] 5 becomes a problem. [53:18] Having said that, very useful approach. [53:20] So make sure that you familiarize yourself [53:23] with the A-star. [53:24] And the first problem in the P-set will help you to do so. [53:30] So this is the grid-based approach. [53:33] Another approach is the one where [53:35] we try to solve the problem directly [53:37] in the continuous space, by using data structures that [53:40] retain all the information from the continuous formulation [53:43] of the problem. [53:44] I'll be a little bit faster here because this class of algorithms [53:47] is useful conceptually, but it's much less useful [53:50] from a practical standpoint. [53:53] So, as we already said, the problem of motion planning [53:56] is to plan the motion for a robot from initial configuration [53:59] to a final configuration in a continuous space. [54:04] Let's look again at the setup that we looked before. [54:08] So for example, in this case, the problem [54:10] is to find a sequence of translations and rotations [54:13] that deliver us to the goal region. [54:16] This actually allow us to reinterpret the problem motion [54:26] planning in a more unified way of what we've done so far. [54:29] And this goes also back to a previous question [54:31] about how do we account for the dimension of the robot. [54:34] The problem motion planning is really [54:36] to find a sequence, a trajectory, basically, [54:38] for your configuration variables. [54:41] Once you've done that identification, [54:43] the problem motion planning reduces [54:45] to the problem of planning the motion of a point [54:47] in configuration space. [54:50] So in general, the motion planning problem [54:52] is defined with respect to the configuration space, [54:55] meaning the set of all the configurations [54:57] that the robot can assume. [54:59] To be specific, in the case of a two-dimensional robot, [55:03] like, for example, a triangle, its configuration [55:05] will be its Cartesian position, x, y, and its heading. [55:11] So the problem of motion planning [55:13] is to find a sequence for the x, y, and theta [55:17] so that the real robot do not collide with the obstacles [55:22] and reaches the goal. [55:26] So the configuration in this case is subset of R3. [55:30] In reality, there is a nuance due to the fact [55:32] that theta is an angle. [55:34] And theta plus 2 pi is the same. [55:37] So in reality, the correct way of representing this [55:39] mathematically is a subset of R2-- [55:42] this is the Cartesian space for the position-- [55:45] times the space for the rotation. [55:48] So basically, a circle, where you have to account for the fact [55:52] that theta plus 2 pi is equal to theta. [56:09] So this is 0, and this is 2 pi. [56:11] And you do not recognize that a 0 is equal to 2 pi. [56:15] So you may think that going in this direction is not possible. [56:20] So basically, swinging from 2 pi to 0 is not possible. [56:25] So let's assume that this is your origin, [56:28] and this is your goal. [56:30] So if you don't identify 2 pi with 0, [56:33] then the motion that you will plan [56:34] is something like this, which is very long. [56:36] If indeed you identify 2 pi with 0, [56:38] then the motion that you will plan [56:40] is this one that is much shorter. [56:45] But anyway, the point here is that, in general, the motion [56:49] planning problem in its full generality [56:52] can be considered as a problem of finding [56:54] a path in the configuration space, whereby the robot has [56:59] been shrunk to a point. [57:00] And accordingly, the obstacles have been enlarged in order [57:05] to account for the fact that the robot has [57:08] been shrunk to a point. [57:10] For example here, if we have a triangular object-- [57:14] yeah. [57:14] So we have triangular obstacle and a circular robot. [57:18] In the configuration space, so in the space of x, y positions, [57:23] the obstacle will be represented as this inflated set, [57:28] with the idea that if the x, y position-- [57:31] so the center of your robot is here-- [57:33] then the robot will collide with obstacle. [57:36] If it is outside, then the robot does not [57:38] collide with the obstacle. [57:40] So this goes back to the discussion [57:42] we had before about the need to inflate the obstacles [57:46] and basically represent or more technically [57:48] to represent the obstacles in the configuration space [57:51] in order to do motion planning, whereby we interpret [57:54] the robot as a point, in particular, its configuration. [58:00] It's a new set of point but something [58:01] important to keep in mind. [58:03] If you're dealing with non-circular robots how to-- [58:07] efficiently, possible ways to account for the robot's geometry [58:10] when inflating. [58:11] And the answer is no. [58:13] That's why motion planning is a hard problem. [58:16] Very good point. [58:17] In general, how do we represent the obstacles [58:21] that are in the workspace? [58:22] So in the three dimensions into the configuration space, [58:26] which might be much higher dimensional. [58:28] For example, if you have a robot with a manipulator, [58:31] the configuration space account for the joint angles [58:33] of your arm plus the Cartesian position [58:36] of the base of your manipulator plus the heading. [58:39] So the space could be like a 10 dimensional. [58:42] So now you have an obstacle in the workspace. [58:45] So in the real world, you have to inflate it [58:48] in the configuration space that has [58:51] a different number of dimensions, in particular, much [58:54] larger. [58:55] How do you do that? [58:56] Extremely complicated. [58:57] That's exactly the reason why these combinatorial algorithms [59:00] do not work in practice because they require [59:02] an explicit representation of the obstacles [59:05] in the configuration space that oftentimes in-- [59:08] I'll say, almost always is not available-- [59:10] which motivates the sampling-based algorithms [59:12] that we're going to discuss on Thursday. [59:15] But yes, that's the entire point of these slides, [59:17] to show that conceptually, if you can represent obstacles [59:21] from the real world into the configuration space where [59:24] the motion planning problem really lives, [59:26] then you can apply the algorithm that I'm [59:28] going to mention in a second, and everything is easy. [59:31] But in reality, this translation is very difficult. [59:33] That's what motivates sampling-based algorithms [59:35] that we're going to discuss on Thursday. [59:39] So at this point clear? [59:41] Is it a bit subtle? [59:42] I would say that a good fraction of roboticists [59:45] missed this point. [59:46] I routinely review papers where this point is missed, [59:49] and the claims that people make are completely nonsensical. [59:54] So, as I said, conceptually, the problem again for the 10th time [59:58] is to shrink the robot to a point, its configuration, [60:03] which might be a high-dimensional object-- [60:05] the heading, the position, possibly the joints [60:08] for manipulator on top of your robot, and so on. [60:11] But accordingly, you have to represent the obstacles [60:16] from the real world, referred to as workspace, [60:20] into the configuration space. [60:23] This mapping, in general, is complicated. [60:26] In this case, whereby the dimensional configuration space [60:29] and the dimension the workspace are the same [60:31] is very easy to understand but, in general, [60:34] is very difficult to represent explicitly. [60:38] What is your robot model [INAUDIBLE] can change shapes [60:43] or potentially you can shrink the obstacles differently based [60:47] on how you're-- the [INAUDIBLE] of the robot. [60:51] It can change shape in the sense that it's not a rigid body. [60:53] It's a soft robot. [60:54] No, like with the arms extended this way, [60:58] then it's going to have a larger radius that kind of contracts [61:03] back to the body. [61:03] And you said it's smaller. [61:06] So how do you account for that when you [61:08] are inflating the obstacles? [61:10] Do you contract-- [61:11] So in general, representing explicitly obstacles [61:16] from the workspace in configuration space of robot [61:19] manipulator with multiple joints is extremely difficult. [61:23] That's why this is basically never done explicitly like here, [61:27] but it's done implicitly through the notion [61:29] of sampling-based algorithms that we're [61:31] going to see next time. [61:32] But again, very good point. [61:33] That's why these algorithms, even though we're [61:35] going to spend 15 minutes on them [61:38] more for self-consistency of this class, [61:40] are not really that useful in practice [61:42] because of, for example, this problem. [61:46] Having said that, in the context of a planning configuration [61:52] space, then the problem formally becomes the problem of finding [61:56] a continuous path for your configuration vector [62:02] that maps the interval 0, 1 into a trajectory that does not [62:08] intersect with any obstacle, so belongs [62:10] to the free portion of the configuration space, [62:13] with the property at 0, we are at the initial node. [62:16] And at the end, we are at the goal node. [62:18] The free portion of the configuration space [62:20] is the set of configuration such that when [62:24] the robot in the workspace is at that configuration, [62:28] it does not intersect with an obstacle. [62:33] Does that make sense? [62:36] So that's really the link between configuration space [62:38] and workspace. [62:40] The collision checking is done, obviously, in a workspace. [62:43] And this is the formula that allows [62:45] you to translate conceptually-- [62:52] obstacles in the workspace into constraints in the configuration [62:55] space. [62:56] Unfortunately, there is no general algorithm [62:58] that allows you to do that explicitly and efficiently. [63:01] Does that mean that the free space [63:02] is different for different orientations of the robot [63:05] or configurations of the robot? [63:07] So the free space is the set of all configurations such [63:11] that the robot does not intersect [63:13] an obstacle in the workspace. [63:15] So the configuration space is a space. [63:17] So it's a set of an uncountable number of configurations. [63:21] So having said that, the idea of a combinatorial planning is [63:26] to still compute a roadmap-- so basically, [63:30] a graph similar to grid-based search-- [63:33] but in a way that we are not losing [63:35] any information with respect to the underlying continuous [63:39] problem. [63:41] This means that this roadmap needs [63:43] to have two key properties. [63:45] Accessibility, which means that for every configuration qI, so [63:53] initial configuration and the final configuration q goal, [63:56] there is a way to connect this configuration to at least a node [64:01] in the roadmap. [64:02] So you don't end up in situations whereby, say-- [64:23] So this is an obstacle. [64:25] This is your qI, and this is your roadmap. [64:30] So basically, this is the graph that you [64:32] use to represent your continuous space. [64:35] So this is a case whereby you would like to solve the motion [64:38] planning problem from here, but you [64:40] don't have an easy way to connect the key qI [64:43] to any node in the roadmap. [64:45] So it will be a bad roadmap. [64:48] And connectivity, if there exists a path from a qI to qG, [64:52] there exists a path on the roadmap from S1 to S2. [64:56] This basically means that you're not losing any information [64:59] by having discrete representation [65:01] of your underlying continuous space. [65:05] Because given qI and qG, I can always connect those [65:10] to at least a node in the roadmap, [65:12] and it is always a path in the roadmap that [65:14] brings me to the node S2 that I can connect to qG. [65:21] Does it make sense? [65:24] So the question, of course, is how do you compute the roadmap [65:27] with these two properties? [65:29] If you have these two properties, [65:31] then you're guaranteed the roadmap [65:33] that provides a discrete representation [65:35] of the continuous motion planning problem [65:37] without losing any of the original connectivity [65:39] information needed to solve it. [65:41] The question now, of course, is how do you [65:43] compute such a roadmap? [65:47] So in the case of 2D-- and again, [65:49] the reason why I use the 2D case is, [65:51] a, because it's easy to visualize, two, [65:53] because this class of algorithm work well for these simple [65:57] problems. [65:58] They don't really work well for more complicated problems. [66:00] A simple way to compute such a roadmap [66:03] is to use the notion of self decomposition, whereby [66:07] the environment is decomposed into cells that [66:10] are easy to compute, and then connectivity within each cell [66:17] is straightforward. [66:18] For example, you consider cells that [66:19] are convex, which means that between any two nodes, [66:22] you have an interpolating line that [66:24] belongs to that set as well. [66:26] And finally, the identity between the cells [66:30] is also straightforward. [66:32] So a simple way to compute a cell decomposition [66:36] is by computing a trapezoidal cell decomposition. [66:39] Trapezoidal cell decomposition works as follows. [66:42] You start from each vertex of the forbidden space [66:48] of the obstacle space, like for example, here or here. [66:52] And you start shooting rays downward and upward [66:56] until you intersect the boundary of the feasible space. [67:01] Every time you have an intersection, you build a cell. [67:07] And then you place a representative point, [67:11] for example, at the center at the centroid of that cell. [67:14] In this way, you're guaranteed that each cell is indeed convex. [67:19] So you have the first accessibility property. [67:22] And also, due to the identity with which [67:24] you compute these trapezoidal cell decomposition, [67:27] you also have the connectivity property. [67:30] So then the way that motion planning would work [67:32] is that if you're given a qI here, [67:39] you connect it to the centroid of the cell where qI lies. [67:44] Then you do planning by using, for example, A-star [67:47] on the roadmap. [67:48] And then you connect the qG to the nodes [67:52] that is representative in the same cell. [67:57] This is a purely geometric formulation [67:59] of motion planning problem. [68:03] Yeah. [68:06] The vertices are not necessarily all sharp points, [68:08] but on space, there's some curves. [68:11] So you can't necessarily draw a straight-line trapezoid? [68:15] Yeah. [68:15] So these algorithms are simple to explain and to visualize, [68:21] to implement are really pain because of-- [68:26] basically, doing these geometric constructions computationally [68:29] is not easy. [68:30] There is a tool that I'm going to mention in a second. [68:34] But in general, coding this stuff is not easy. [68:37] And then if you start having curves, [68:39] then you have to do some polygonal approximation in order [68:42] to run this scheme. [68:44] So these are algorithms that are easy to explain [68:47] and easy to understand, but they're difficult to implement [68:50] and not very practical. [68:54] I'm talking about these algorithms more [68:55] for your knowledge. [68:58] But what you really need to learn from this class [69:00] is the A-star search algorithm from this lecture, [69:02] the A-star search algorithm. [69:04] There are other ways to build the roadmaps, like for example, [69:08] visibility graph that allow you to reason about optimality, [69:13] basically beyond the scope of this class. [69:18] As we already mentioned, a key difficulty of these algorithms [69:21] is that we are assuming that we can represent [69:23] in the configuration space the set of configurations [69:26] that are obstacle-free. [69:28] In general, it is very hard to do. [69:31] Going back to your question, there are actually [69:36] computational procedures to do it, [69:39] but they have a terrible scaling like double exponential [69:43] and these kind of things. [69:44] So conceptually, it's possible. [69:47] Practically, it's not possible. [69:50] And by stretching my drawing skills, [69:52] I want to give you yet another explanation about why [69:57] it's so hard to go from the representation of the obstacles [70:06] in the workspace into a representation of the obstacles [70:08] in the configuration space. [70:09] Let's go back to the previous example. [70:11] So moving in triangle from initial position [70:13] to the goal position. [70:14] The configuration space is three-dimensional. [70:16] You have translations, x, y, and the theta, the heading. [70:22] So for example, we can take any point in on the triangle [70:26] to be our representative point. [70:28] For example, this vertex. [70:31] So the first thing to do is, as we said before, [70:34] to enlarge the obstacles by using [70:37] the geometry of the robots. [70:39] So if your upper-left vertex is here, then-- [70:43] so here, I'm only considering translation. [70:45] Sorry. [70:45] In this problem, the configuration [70:47] is two-dimensional. [70:49] So if this vertex is here, the two robot will be in collision [70:53] or, otherwise, it is not. [70:55] So it's like this robot along the shape of the obstacle. [70:58] And this will be the enlarged version [71:00] of the obstacle in the configuration space [71:03] that will lead to a collision for the robot in the workspace. [71:07] Same for the other obstacle. [71:09] Then I have to trim the environment. [71:12] And having done that, then in the configuration space, which [71:16] is just xy for the triangular robot, [71:18] we are not allowing for now rotation. [71:20] The problem is just to find a path for a point [71:23] by using, for example, other star [71:25] or the trapezoidal decomposition and so on. [71:28] But let's now put the heading as an additional degree of freedom. [71:31] So now your robot can also rotate. [71:33] So now the workspace is two-dimensional, [71:36] but the configuration space is a three-dimensional. [71:39] You have x, y, and theta, which means [71:41] that you have to do that sliding procedure [71:43] to increase the obstacles for each fixed value of theta, which [71:48] will give rise to this very weird shape in three dimensions. [71:52] So those two seemingly benign obstacles in 2D becomes these [71:57] two horrible things in the configuration space. [72:01] And this is just to go from a two-dimensional workspace [72:04] into a three-dimensional configuration space. [72:06] Think about six-dimensional configuration spaces [72:10] with dynamics. [72:11] No way you can do it, which motivates the algorithm that I'm [72:15] going to discuss on Thursday, which [72:17] is the class of sampling-based algorithms. [72:20] Before leaving, though, so there was a question [72:22] about if you really want to apply combinatorial algorithms, [72:25] how you do it? [72:27] So dealing with the geometric construction [72:30] is actually not easy. [72:32] Part of my PhD was in doing so, and it was really [72:35] a pain in the neck. [72:36] Fortunately, there is a library called CGAL, the Computational [72:40] Geometry Algorithms Library that I extensively [72:43] leveraged for my PhD, that allows [72:46] you to do a number of primitive geometric computation, [72:50] like computing Voronoi diagrams and a lot [72:53] of things, trapezoidal decomposition, and so on, [72:56] in a very simple way. [72:58] So if you want to use combinatorial algorithms [73:01] and in particular geometric computation, [73:04] I strongly encourage you to look at the CGAL. [73:08] This link, you should click on it. [73:11] You can find an interesting visualization of the computation [73:14] space for a polygonal robot. [73:15] This gives you yet more insight on the challenge [73:18] of motion planning, which is again [73:19] this translation from the workspace to the configuration [73:22] space. [73:23] And finally, some additional readings if you are interested. [73:26] So to sum up, for combinatorial planning algorithms, [73:29] the advantage is that these approaches are complete. [73:32] If there is a feasible path, they will find it. [73:35] And they also have theoretical guarantees on the running time. [73:38] Unfortunately, due to this complication [73:41] of translating obstacles from the workspace [73:45] to the configuration space in an explicit manner [73:48] as required by combinatorial algorithms, [73:50] running these algorithms on problems with more than three [73:55] or four degrees of freedom is not really practical. [73:59] And also, they're difficult to implement. [74:03] I gave you a reference for a library that [74:05] would ease the implementation, but they're still [74:07] very tricky to implement. [74:08] The algorithms that I'm going to discuss on Thursday, [74:12] actually, are much simpler to implement, a scale much better [74:15] with respect to the number of dimensions. [74:18] And in particular, we look at those algorithms on Thursday. [74:21] So to sum up this lecture, I talked [74:24] about grid-based approaches to motion planning. [74:26] This is really what you have to remember from this lecture, [74:29] is a very important tool in the field of robotics. [74:32] And you're going to use, in particular, A-star [74:34] for the rest of the quarter. [74:36] As a bonus material, I talked a little bit [74:39] about combinatorial planning with the idea [74:42] that, conceptually, this is a very interesting approach [74:45] to motion planning. [74:46] Practically, not that useful. [74:48] For your midterm, I will ask you conceptual questions [74:51] about grid-based approaches, like, [74:53] for example, what's the difference between Dijkstra [74:56] and the breadth-first search, and questions [74:58] about sampling-based algorithms that I will explain on Thursday. [75:02] I won't ask questions about combinatorial planning, [75:05] since, as I said, it was more like a bonus introduction [75:08] to this material, but I really didn't go into the details. [75:13] So I'll see you on Thursday.