Stanford AA274A Principles of Robotic Autonomy | Autumn 2019 | Motion Planning Sampling-Based

斯坦福 AA274A 机器人自主性原理 | 2019秋 | 基于采样的运动规划 Marco Pavone 教授介绍了基于采样的运动规划，将其作为高维构型空间中替代精确组合方法的一种隐式且计算可行的方案。本讲座探讨了基础算法——将概率路线图（PRM）等多查询方法与快速探索随机树（RRT）等单查询方法进行对比，同时介绍了 Voronoi 偏差、目标偏置和惰性碰撞检测等核心概念。Pavone 分析了 PRM* 与 RRT* 的渐近最优性保证，解释了快速行进树（FMT*）以及动态约束下的运动动力学规划，最后论证了确定性低散度采样为何能够优于随机序列并实现安全性认证。 基于采样的运动规划器通过黑盒碰撞检测器隐式探测构型空间，规避了高维空间中组合方法难以处理的解析几何计算。 (6:03) 针对渐近最优性的理论修正表明，由于 RRT 随时间进行增量式建树，RRT* 的连接半径缩放指数需要为 1/(d+1)，而非经典 PRM* 的 1/d。 (48:20) 快速行进树（FMT*）算法通过惰性评估对每个采样样本仅执行一次碰撞检测，在保持 RRT 级速度的同时达到了 PRM 级的解质量。 (50:24) 确定性低散度采样在各项基准测试中均优于随机序列，实现了 O(n^(-1/d)) 的最优散度缩放，并支持对任务关键型自主系统进行严格的安全认证。 (72:48)

In this lecture, Professor Marco Pavone argues that the breakthrough advantage of sampling-based motion planning stems not from randomness, but from implicitly probing configuration space through workspace collision queries. He outlines the evolution of geometric, kinodynamic, and deterministic planning methods designed to balance computational cost against path optimality. Implicit Probing Bypasses High-Dimensional Geometric Complexity (0:06) Motion planning requires finding collision-free trajectories from a start pose to a goal region. While the mission is executed in physical workspace geometry, path generation takes place in configuration space (C-space), where the robot is treated as a single point and obstacles are mathematically expanded into C-obstacles. Introducing rotational degrees of freedom or higher state dimensions rapidly renders the explicit construction of C-obstacle boundaries analytically intractable. Combinatorial methods like exact cell decomposition demand these explicit boundaries, whereas sampling-based planners circumvent the issue entirely by adopting an implicit representation. Trade-offs Between PRM, RRT, and Hybrid Architectures (13:28) Sampling-based methods divide primarily into multi-query roadmaps and single-query search trees. The Probabilistic Roadmap (PRM) builds an unguided graph over collision-free space to answer multiple queries across arbitrary endpoints, but evaluating all-to-all edges causes collision checking to explode computationally. Lazy PRM mitigates this by postponing edge collision checks until candidate shortest paths are searched using A*. Conversely, Rapidly-exploring Random Trees (RRT) incrementally grow a single-query tree rooted at the initial state. RRT's rapid exploration is driven by the Voronoi bias—where outer frontier vertices with larger Voronoi cells are preferentially expanded—yet it produces jagged, highly suboptimal trajectories. While standard PRM and RRT provide probabilistic completeness as sample count approaches infinity, standard RRT routinely gets trapped in suboptimal topological classes. Asymptotically optimal variants like PRM* and RRT* guarantee convergence to optimal path costs as samples scale, though RRT* relies on local edge rewiring. To capture the path quality of PRM alongside the single-collision-check speed of RRT, Pavone's group developed the Fast Marching Tree algorithm (FMT*). By employing dynamic programming and a single lazy collision check per sample, FMT* avoids testing alternate parents upon collision, with analysis showing that lazy connection errors become vanishingly rare as sample density increases. Extending Sampling to Differential Constraints via Reachable Sets (57:43) When planning for nonholonomic or high-speed systems like automobiles and aircraft, geometric straight-line segments violate system dynamics. Kinodynamic planning addresses this through either forward simulation or trajectory optimization. In forward simulation, random control inputs and time durations are numerically integrated from the nearest tree node, which Pavone characterizes as a highly inefficient baseline that ignores domain knowledge. Alternatively, planners solve two-point boundary value problems using trajectory optimization to optimally steer between states before validating clearance. Dynamic differential constraints break the isotropic nature of Euclidean neighborhoods. Nonholonomic platforms require complex maneuvers to displace laterally compared to forward driving, distorting the reachable space. Consequently, geometric Euclidean balls of radius r must be replaced by cost-bounded reachable sets bounded by elapsed time or control effort. Obstacle evaluations remain decoupled from trajectory generation, allowing planners to compute reachability under unconstrained dynamics and evaluate obstacle collision purely a posteriori. Dispersion Bounds Enable Superior Deterministic Planners (71:33) Challenging the assumption that randomness is required for sampling-based planning, Pavone presents evidence that replacing uniform random sequences with deterministic low-dispersion sequences yields substantial advantages. In safety-critical aviation and automotive deployments, purely probabilistic guarantees hinder regulatory certification, whereas deterministic sequences allow explicit finite-sample convergence bounds. Deterministic sampling also unlocks offline caching of roadmaps, precomputed reachable sets, and simplified nearest-neighbor searches. Mathematically, the coverage of a configuration space is governed by L2 dispersion—the radius of the largest empty ball unoccupied by samples. While optimal random PRM* connection radii require scaling with logarithmic factors, deterministic low-dispersion sequences achieve optimal asymptotic dispersion scaling at O(n^(-1/d)). As a result, deterministic PRM converges to the optimal trajectory with a smaller connection radius, fewer edge evaluations, and demonstrably faster empirical performance across 2D to 8D robotic manipulator and aircraft benchmarks. Sampling-based planning remains computationally bounded by the curse of dimensionality—breaking down past 12 to 13 unconstrained dimensions—and by the CPU costs of collision checkers and nearest-neighbor lookups. While deterministic frameworks resolve certification hurdles and eliminate runtime variance, the lecture concludes by transitioning to perception modules to address real-world sensor uncertainties and scene estimation.

[9:27] Marco Pavone: say 50 lines of code [13:28] Marco Pavone: a number of roboticists out there actually confuse [16:08] Marco Pavone: coding a PRM algorithm is really a matter of 40, 50 lines of code. Super easy. [24:06] Marco Pavone: absolutely [34:21] Marco Pavone: Practically, if you don't implement goal biasing, this algorithm really doesn't work. If you implement goal biasing, this algorithm works pretty well. [35:53] Marco Pavone: knowing that there are another 2,199 variations on the theme [50:24] Marco Pavone: a posteriori fix of what is bad in RRT [75:00] Marco Pavone: randomization magically was solving a problem, which is obviously not the case

[0:06] All right. [0:06] So today, we're going to continue our discussion [0:09] about motion planning by discussing [0:14] another class of algorithms referred [0:17] to as a sampling-based algorithms, which [0:19] have become quite popular in the field of robotics and robot [0:23] autonomy. [0:25] In particular, as you know, the problem of motion planning [0:27] entails finding a sequence of actions [0:30] that drive a robot from an initial position [0:35] to a goal region, while avoiding obstacles, [0:39] respecting the dynamics of the system, [0:41] and possibly optimizing a cost function. [0:43] Last time, I had an image of a motion planning [0:46] problem, an image of a discretized world. [0:49] Because we focus more on the grid based approaches. [0:52] Here, I have an image of motion planning, [0:54] where we have a continuous path, because the idea is that today [0:57] we're going to look at algorithms that reason [1:01] about the continuous formulation of motion planning, where you're [1:05] reasoning about the continuous path. [1:08] In particular, the goal today is to learn [1:10] about the class of algorithms referred [1:11] to as a sampling-based motion planning algorithms. [1:14] A good reading for the lecture today [1:18] is chapter 5 in the book of Steve LaValle called [1:23] the Planning Algorithms. [1:26] So let's start from where we ended last time. [1:31] A critical point that I already made a few times, [1:34] but I want to make one more time, [1:36] is that while the motion planning problem is defined [1:40] in the workspace, you want to find a trajectory that does not [1:43] collide with obstacles. [1:45] The procedure, the process of building a trajectory [1:48] really happens in another space, the configuration space. [1:53] The space of configurations that a robot can have, [1:58] like in the case of a unicycle, where [2:01] you have as a configuration the x, y position of the unicycle [2:05] and the heading of the unicycle, the configuration space [2:08] is three-dimensional. [2:10] Now, the difficulty is that translating constraints [2:14] that are defined in the workspace in the real world [2:16] into constraints in the configuration space, [2:19] is in general, quite difficult. In particular, [2:23] to motivate-- actually, to better explain this difficulty, [2:26] this is the same sequence of figures that I had last time. [2:29] I'm going to show how the difficulty in making [2:33] this translation possible-- [2:35] increases as the number of dimensions of the configuration [2:38] space increases. [2:39] Let's consider, first, the case of a triangular robot that [2:42] wants to maneuver from initial configuration [2:45] to a final configuration. [2:46] Here, we're allowing the robot to only move [2:49] in the vertical direction, and in the horizontal direction, [2:52] meaning that the configuration space is two-dimensional, [2:55] and the workspace is two-dimensional as well. [2:58] So the way that motion planning works [3:00] is that we shrink the robot to a point, its configuration, [3:04] and accordingly, we enlarge the obstacles. [3:07] In this case, it's fairly simple. [3:08] So what we would do would be to slice the shape of the robot [3:12] around the obstacles. [3:15] And then the representation of the obstacle [3:17] in the configuration space of the robot [3:20] would be this gray area, plus the purple area, [3:24] with the idea that if the configuration of the robot. [3:27] In this case, taken to be the x, y position [3:30] of this corner of the robot is not in neither the purple area [3:36] nor in the gray area, then the robot [3:38] in the workspace, what we really care about [3:40] is not going to be in collision with the obstacle. [3:43] Does it make sense? [3:50] All right. [3:51] So we do the same for the other obstacle. [3:54] We do the same with respect to the boundaries [3:58] of the environment. [3:58] So we have to crop the boundaries to make sure [4:00] that our obstacle doesn't get outside [4:03] of the boundaries of the environment. [4:05] And now the problem has been translated [4:06] into the problem of finding a path for a point which [4:11] represents the configuration of the robot, which, as I said, [4:15] would represent the x, y-coordinates [4:20] of this vertex of the triangular robot. [4:23] Then we just find the path, knowing [4:25] that if the x, y position of this vertex [4:31] is collision-free in the configuration space, [4:33] then the robot is going to be obstacle-free in the real space [4:36] in the workspace. [4:37] So these are all good and fine. [4:40] But now, let's assume that we start [4:42] considering a more complicated model for the triangular [4:46] robot, whereby apart from just translating, [4:49] the robot can also rotate. [4:51] So now the configuration is both x and y [4:53] and theta, the heading angle. [4:56] In this case, the workspace is still two-dimensional, [4:59] but now the configuration space is three-dimensional, x, y, [5:02] and theta, which means that we would need to carry out the same [5:07] sliding procedure for each fixed value of theta, [5:11] and incrementally build a 3D representation of the obstacles [5:18] in the configuration space. [5:19] So even though the obstacles in the workspace [5:21] are two-dimensional, now in the configuration space, [5:24] they would look something like this, fairly complicated [5:27] three dimensional objects. [5:28] And you can imagine how this procedure, as the configuration [5:32] vector becomes larger and larger, [5:34] or if you start accounting for dynamics [5:38] becomes extremely, extremely complicated. [5:42] So the key point here is that what the motion planning [5:45] problem is described in the real world, also referred [5:48] to as the workspace, it really lives [5:50] in another space, the configuration space, whereby [5:53] the robot has been shrunk to a point, [5:55] and the obstacles have been enlarged accordingly [6:00] and represented in the configuration space. [6:03] So having said that, there are two main approaches [6:05] to continuous motion planning, continuous in the sense [6:08] that we reason about the continuous trajectory. [6:11] Combinatorial planning, which is what [6:12] we saw towards the end of the lecture [6:16] on Tuesday, whereby we build the data structures like, [6:21] for example, cell decompositions that discretely [6:25] approximate the continuous formulation [6:27] of the problem without neglecting any information that [6:31] is necessary for the problem. [6:34] This is, in principle, the ideal approach. [6:39] The disadvantage of this approach [6:40] is that in order to work, it requires [6:42] the explicit representation of the obstacles [6:45] in the configuration space, which, as I said, [6:48] is extremely challenging. [6:51] To build a cell decomposition, you [6:53] need to have an explicit representation [6:54] of the obstacles in the configuration space, which [6:57] is very challenging. [6:58] This motivated the development of a second class of algorithms, [7:03] sampling-based algorithms, which only implicitly represent [7:07] the obstacles in the configuration space. [7:10] And this is what makes them really work for complex motion [7:14] planning problem. [7:15] I will discuss in a second what implicit representation [7:19] of an obstacle in the configuration space means. [7:27] The idea is as follows. [7:29] Translating the obstacles from the workspace [7:33] to the configuration space is hard, [7:36] but what is easy is to say if a robot is a given [7:40] configuration, say x, y, theta, would the robot [7:44] be in collision with an obstacle in the workspace, yes or no? [7:49] Such yes or no type of question can be easily answered [7:52] by using a collision checker. [7:55] So the idea of a sampling-based algorithms [7:57] is to avoid the explicit representation of obstacles [8:01] in a configuration space, and instead probe, the configuration [8:05] space with a set of samples that are drawn randomly. [8:10] For each sample, which is a specific instantiation [8:14] of the configuration vector, we call a collision checker, [8:18] asking the question if the robot were [8:20] to be at this configuration, would the robot [8:23] be in a collision, yes or no? [8:26] If the sample is collision-free, then we retain it. [8:30] If it actually will lead to a collision, then we discard it. [8:34] And then by using these samples, we [8:36] build a roadmap or some variation of a roadmap [8:39] that we can leverage to compute a path. [8:42] I will give you explicit examples of algorithms [8:46] instantiating this idea in a second, [8:48] but overall, this is a key idea behind [8:52] the sampling-based algorithms. [8:54] As I said, to capture the structure [8:56] of the computational space by random sampling, [8:59] then use a black box component, a collision [9:05] checker to check if every sample configuration will [9:09] lead to a collision, yes or no. [9:11] And then use such a set of samples [9:14] in order to build a roadmap. [9:17] A good reference for this is a paper 2011 [9:20] from Steve LaValle, the same author [9:22] of the book I mentioned before. [9:27] Now, before we go into specific instantiation of this idea [9:32] by discussing some popular algorithms implementing [9:36] this idea, let me say that the main advantages of this class [9:39] of algorithms, which has made them quite [9:42] popular in robotics domain, is that, as you will see, [9:45] these algorithms are conceptually simple. [9:48] Remember, when we were talking about combinatorial algorithms, [9:52] we were talking about doing a cell decompositions that [9:54] require complicated geometric structures that require quite [9:59] a bit of coding, and so on. [10:01] This algorithm are super simple to implement, [10:04] say 50 lines of code. [10:05] That's it. [10:06] Very difficult to make mistakes, which is a big plus. [10:11] Second of all, they are quite flexible. [10:14] Like for example, with combinatorial algorithms, [10:17] the cell decomposition approach is [10:21] really dependent on how you represent your obstacles. [10:24] Like in a polygonal way. [10:26] It really depends on the specific of your robot, [10:30] whether it has dynamics as not dynamics. [10:32] These algorithms, as we will see, [10:34] can be applied very broadly. [10:37] And in particular, they can be applied [10:39] beyond the geometric case. [10:40] They can be applied to the case, where [10:42] we account for dynamics in a motion planning procedure, [10:45] can be extended to the case where we also account [10:47] for uncertainty in environment, even though we won't see it [10:50] in this class, and so on. [10:53] There are disadvantages, as for everything in engineering. [10:56] First of all, as you will see, a key question [10:59] is how many samples do we need in order to accurately cover [11:04] the configuration space. [11:05] In general, there is no rigorous answer [11:09] to this question, which means that you still [11:11] need some experience in order to select [11:16] a good enough number of samples that you want to use. [11:20] Second of all, if there is a path, [11:22] these algorithms will most likely find it. [11:26] And actually, we're going to have more precise statements [11:29] about this. [11:30] But if a path does not exist, in general, [11:33] these algorithms won't tell you that it does not exist. [11:36] The algorithm will continue running [11:38] and then you continue waiting, but it will never [11:41] terminate saying sorry, no path exists, exist, [11:43] which is annoying. [11:45] From an algorithm, you would like [11:46] to know that if a solution exists, it finds it, [11:49] and if it does not exist, it tells you [11:51] that it does not exist. [11:52] This algorithm, unfortunately, do not tell you [11:54] that a solution does not exist. [11:56] But in reality, this is a mild disadvantage because [12:00] in practice, if an algorithm runs for a long time, [12:04] you know that the most likely, solution does not exist. [12:06] And that's what you basically claim. [12:09] So today, we're going to discuss three main topics [12:13] in the general umbrella of sampling-based motion planning [12:16] algorithms. [12:17] First, I'm going to discuss a representative algorithms that [12:21] are sampling-based, and address the problem [12:24] of geometric planning. [12:25] Meaning, we try to find a path that avoids obstacles, [12:28] but we do not account for dynamics in the motion planning [12:32] procedure. [12:34] Then we consider the case where we actually [12:36] account for dynamics in the motion planning procedure. [12:39] In this case, motion planning is referred [12:41] to as a kinodynamic, planning to make it explicit that we're [12:45] counting for dynamics. [12:46] And finally, I will discuss a fairly natural question, [12:51] which is does the probing of the configuration [12:55] space through the sampling procedure needs to be random. [12:59] As I said, since the first description of these algorithms [13:06] used randomized sampling, but a natural question [13:09] is, does the probing sampling sequence needs to be random, [13:15] or can be done actually deterministically? [13:19] Let's start with the geometric case. [13:22] Any questions so far? [13:28] So the key concept is a bit subtle. [13:30] And I will say that a number of robots out there [13:33] actually confuse this concept a little bit. [13:36] It's the main difference between combinatorial [13:38] and sampling-based, is that the combinatorial algorithms [13:40] explicitly represent obstacles in the configuration space. [13:44] Sampling-based rely on implicit representation of obstacles [13:48] in the configuration space. [13:50] Through this notion of sampling. [13:52] Whereby you just throw some possible instantiation [13:58] of the configurations. [13:59] And for each one, you call a collision checker [14:02] to see if those samples are in collision or not. [14:06] By doing so, you only implicitly represent the obstacles, [14:10] because the obstacles are only represented [14:13] by the samples that have been declared, respectively, [14:17] collision-free or not. [14:22] So there are two main approaches in the field [14:26] of sampling based planning. [14:28] The first approach is referred to as a multi query planning. [14:32] Multi query means that this class of algorithms [14:36] give you a data structure that can be used to solve. [14:42] Any motion planning instance that you want, [14:45] any means, for any pair of origin and destination, [14:49] you can use this data structure to find the path. [14:52] Single query instead of planners, [14:54] that allow you to solve only one instance of the motion planning [14:57] problem, basically for one specific origin and goal region [15:02] pair. [15:03] Depending on your application, one [15:05] might be preferable to the other one. [15:07] So the first algorithm that was proposed in the multi query [15:12] setting was the PRM algorithm, Probabilistic Roadmap, [15:16] which was devised about 20 years ago by Lydia Kavraki, [15:20] and co-authors. [15:21] And it's seen a lot of development [15:24] in the subsequent 20 years. [15:27] The other approach that is single [15:29] query is dominated by a class of algorithm referred [15:32] to as RRTs, Rapidly-exploring Random Trees, which was proposed [15:37] a few years later with respect to PRM, [15:39] was proposed in 2001 by LaValle. [15:42] And here, there is a typo. [15:44] It should be Kuffner. [15:45] LaValle is the same author of the book [15:48] that I mentioned before. [15:50] And these algorithms as well, has seen a lot of development [15:53] in the past 15 years or so. [15:56] So what we're going to do now is to discuss, in turns, PRM [16:00] and RRT, discuss the relative advantages and disadvantages, [16:04] and then discuss also some alternatives. [16:08] So let's consider PRM first. [16:10] The idea of PRM is fairly simple. [16:14] So let's say that this is the configuration [16:18] space of our robot. [16:21] White means that this region is collision-free. [16:25] So if the configuration of the robot is this point, [16:29] remember that the configuration represents [16:30] the value for a configuration of the robot, [16:38] in the case of a unicycle, specific values for x, y, [16:41] and theta. [16:44] So white means free-space. [16:47] Gray means a set of configurations [16:50] that will lead to a collision for the robot in the workspace. [16:58] In sampling-based, we assume that we do not [17:02] have an explicit characterization [17:04] of the free region of the configuration space. [17:08] I'm showing it here because of course that notion exists, [17:12] but the algorithm does not have access to it. [17:15] So what the algorithm does is to throw [17:17] a bunch of possible configurations, each one, [17:22] again, corresponds to a given pose [17:23] of the robot in the workspace. [17:26] For each one, we call a collision checker. [17:29] So we say, if the robot were to have this pose, [17:33] would the robot be in a collision, yes or no? [17:36] In this case, the answer is yes. [17:38] Here, it will be in collision. [17:40] No, here you would not be in a collision. [17:43] So you discard all these samples that will lead to a collision, [17:48] and you keep those one that would ensure that the robot is [17:52] collision-free. [17:54] And that's our implicit representation [17:56] of the free configuration space. [17:59] The robot does not have access to these boundaries, [18:02] but has an implicit characterization [18:04] of what is free through the samples that have [18:06] been declared collision-free. [18:11] Then what we do is for each sample, [18:15] we try to connect that sample to neighbors. [18:20] Basically, we try to answer the question, [18:22] can I go from this sample to this sample in a way [18:25] that it's a collision-free. [18:27] So in the case where we're solving a geometric problem, [18:30] the best way to connect to points [18:31] is through a straight line. [18:34] So for each point, we look at a neighbor-- for each sample, [18:37] we look at a neighborhood around that sample, [18:40] and we try to connect that sample to all its neighbors. [18:46] In particular, we look for each sample x, at all samples x [18:52] prime, that are within a ball of radius r. [18:57] So you might say, why don't you try [18:59] to connect each sample to every other sample? [19:03] In principle, that will give you a very dense graph [19:06] that you could use to plan high quality paths. [19:10] So that seems reasonable. [19:12] So why, in practice, people do not consider as radius in order [19:16] to establish connections to neighbors, that full diameter [19:21] of the environment? [19:24] Try to be incomputable. [19:25] Yeah. [19:26] Because that would require you to evaluate [19:30] a lot of connections. [19:31] For each one of these connections, [19:33] you will need to call a collision [19:34] checker to make sure that this connection is collision-free. [19:38] Remember that the robot does not have [19:39] an explicit characterization of the obstacles [19:41] in the configuration space. [19:43] So in order to check whether these edge is collision-free, [19:46] what, in practice, you had to do is [19:48] to discretize this edge in multiple configurations. [19:55] For each one, call a collision checker. [19:58] So you can see that the more connections you're trying [20:02] to establish, the more computation that you have to do, [20:06] up to a point where this cannot be done in real time anymore, [20:09] which means that the algorithm is not useful anymore, [20:12] in general. [20:14] So you try to limit the size of these radius in such a way [20:19] that you still connect to a representative number [20:23] of neighbors, but you don't do too much computation. [20:26] We'll discuss later on how to choose that radius in a way that [20:31] represents a good compromise between solution quality [20:34] and computation. [20:37] So you establish these connections. [20:39] And once you've done that, what you [20:41] have is a road map in your free configuration space. [20:46] Then for any motion planning instance, which [20:51] means for each pair of a goal, and starting configuration [20:57] and final configuration, what you do [20:59] is to attach your initial configuration to the closest [21:03] sample in the roadmap. [21:05] Your goal configuration to the closest sample in the roadmap, [21:11] like this. [21:11] And then you plan a path by using [21:14] one of the path-finding algorithms [21:16] that we discussed last time, like Dijkstra or A star, [21:20] and so on. [21:22] And that's it. [21:23] As you can see, this is a very simple algorithm. [21:27] So once you have a collision checking module, which [21:31] oftentimes is relatively easy. [21:33] The collision checking module only [21:34] has to answer the question if the robot is at this pose, [21:38] is the robot going to be in collision? [21:39] So it amounts to just evaluating the intersection, in general, [21:43] between polygons. [21:44] It's still computationally intense, [21:47] but it's not the end of the world. [21:48] So once you have that in place, once you [21:51] have collision checking in place, [21:53] then coding a PRM algorithm is really a matter of 40, [21:58] 50 lines of code. [21:59] Super easy. [22:01] Yeah. [22:02] What are the only [INAUDIBLE] What if it leads [22:05] you to go backwards first? [22:07] Or what if the graph is disconnected and connected [22:10] to one side to the other side? [22:12] Sure, that can happen. [22:13] The only problem is that you don't have any-- [22:18] so sometimes, you have additional information that [22:21] can guide you in that choice. [22:23] But usually, in absence of other information, [22:26] this is the best you can do. [22:30] When you pick your initial and final configuration [22:33] and you go to the closest node, basically you [22:36] need to check that straight line. [22:37] Oh, yes. [22:38] Absolutely. [22:39] OK. [22:39] So be very-- well, the case that this is actually there [22:41] is an obstacle here. [22:43] Well, so there is an obstacle in the workspace that [22:46] translates into the configuration space [22:48] will lead to a gray area here, which [22:50] would invalidate this edge. [22:52] Which means that now you have to consider the second best. [22:55] Yeah. [22:57] Would it be more efficient to do more random samplings [23:01] closer to the robot, and then less further away [23:04] from the robot? [23:05] Yeah. [23:05] So there are a number of parameters [23:07] that we haven't discussed about this algorithm, [23:09] in particular, how we select the radius, how densely we [23:15] sample something that would remedy to the issue [23:18] that you mentioned, and with which distribution. [23:21] So here, I just show a uniform distribution. [23:26] But in reality, as you said, much better way to sample. [23:31] And that's what people actually do in practice [23:34] is to do a non-uniform sampling informed by some priors. [23:39] At the very least, you try to sample [23:41] more closely toward robot, toward your goal region. [23:44] Sometimes you have some understanding [23:47] about what are the best pathways in the configuration space, [23:52] and so on. [23:53] A current trend, and also something [23:54] that we work quite a bit in the lab, [23:56] is to use a data-driven method. [23:58] So basically, machine learning methods [24:00] to learn good sampling distributions. [24:02] So basically, you solve 1 million [24:05] mission-planning problems. [24:06] And then you have a neural net that learns given as an input [24:10] the representation of the obstacles in the workspace. [24:13] What is a good sampling distribution [24:16] in the configuration space? [24:17] So that's an excellent question. [24:19] And just to make sure I understand. [24:21] So we still presume that we know where all the obstacles are [24:23] in the workspace. [24:24] Yes. [24:24] Yeah. [24:25] That's an assumption of-- [24:26] That's the input to the algorithm. [24:28] What we do not assume we know is the representation of them [24:32] in the configuration space. [24:34] Then the other version is what if the obstacle [24:36] are time varying. [24:38] In the workspace, what if we have uncertainty [24:42] about what the obstacles are in the Workspace, And so on. [24:46] And there are answers to all those questions, [24:47] but those are beyond the scope of this class. [24:51] Would it ever be worthwhile to somewhere down during the path [24:56] to recalculate and resample? [24:59] Yes, absolutely. [25:00] So the way these algorithms are run, [25:02] and that's why they need to run fast, [25:04] is that you compute this path, you [25:07] execute the first part of it, and then [25:10] you recompute the path in a receding horizon matter, [25:15] or in a repeated optimization method. [25:18] Yeah. [25:19] So typically, I mean, it really depends [25:21] on the tempo of your dynamics. [25:24] But usually, as a rule of thumb, you [25:26] want these algorithms to run between 200 milliseconds [25:29] and a couple of seconds in order to re-optimize [25:33] quickly enough to account for variations in your environment. [25:39] I guess, the collision checker actually work, [25:41] because we still need a full representation of our-- [25:44] not a full representation, but a pretty big representation [25:47] [INAUDIBLE] workspace like you showed in the diagram before. [25:50] Because we would still need to find the intersection between-- [25:53] But in the workspace, not in the configuration space. [25:56] In the workspace, yeah. [25:57] Yes. [25:57] Well, the problem becomes a purely geometric problem. [26:02] For example, sometimes there are some libraries to do that, [26:05] or you can also use bullet and so on. [26:07] But the idea is that it's a purely geometric problem [26:09] in the workspace. [26:10] You don't have to do any complicated transformation. [26:13] What you have is that each configuration corresponds [26:15] to a pose of the robot x, y position, position [26:18] the joint angles if you have manipulator, and so on. [26:21] So now you have a certain configuration of the robot [26:24] in the workspace/ you have an obstacle. [26:26] And then you call bullet or something else to say [26:29] are these two geometric entities intersecting or not? [26:32] How you do that is not entirely straightforward. [26:35] But by now, there are libraries that do that fairly, reliably. [26:39] Usually, as a roboticist, you do not implement collision checker. [26:43] There are libraries out there that do a very good job. [26:45] What you do is to implement these algorithms. [26:49] Out of curiosity, those algorithms convert the robot [26:52] into the workspace, and then check in configuration collision [26:56] space, where they translate the obstacles [26:58] into the configuration. [26:59] No, no, no. [27:00] No. [27:00] But that's the whole point, that you [27:01] don't want to do this translation because it's [27:03] intractable. [27:04] So it's easier to translate the robot into the workspace. [27:07] It's not that you translate because every point indeed [27:09] corresponds to a pose in the configuration space. [27:13] So then you just-- [27:16] well, yes. [27:17] So if you will translate that pose [27:19] into a set of points in the workspace [27:21] that are taken up by the robot. [27:23] Yeah, that's how they work. [27:24] So the collision checking is done in the workspace, [27:26] not in the configuration space. [27:30] Cool. [27:30] I like these questions because it really [27:32] means that you understood what this business of configuration [27:39] space and workspace. [27:40] And also, that I'm a good teacher [27:42] that I was able to communicate that to you. [27:46] It's in the configuration space. [27:47] Yes, absolutely. [27:49] That question actually makes me more worried [27:51] that I'm a good teacher, because yes, absolutely. [27:54] The point of this is that this is a configuration space. [27:57] This is the free area in the configuration space, [28:01] that we only implicitly represent [28:03] through this sampling scheme. [28:06] So as you said, with this method, [28:11] this method runs infinitely, if we can't find the path to-- [28:16] because say that we don't find a solution, that you don't know [28:22] whether you don't find a solution because you don't have [28:25] a problem scheme that is fine enough, or because actually, [28:29] there is no feasible solution. [28:31] So what you do is you redraw an additional n samples, making [28:38] the discretization finer. [28:40] If you don't find a solution, you keep going. [28:42] But you never know whether if you don't find [28:44] it is because you don't have enough samples [28:46] or because there is no solution. [28:49] But if we are unable to find the path to the final destination, [28:54] it means we don't have solution. [28:57] No, it means we don't have a solution. [28:59] Up to this discretization because think about this case. [29:06] So I'm just representing that roadmap [29:09] around the starting point. [29:14] So you have a point here. [29:15] Well, more or less. [29:17] So this is your start goal. [29:20] So let's assume that now, we have a situation. [29:23] So that will be the start goal, the pink one over there. [29:26] And this will be the five blue neighbors, [29:29] most immediate neighbors. [29:30] So let's assume that we have a planning problem that [29:32] looks like this one. [29:37] So this will be obstacles in the configuration space. [29:41] So we try to establish this connection, doesn't work. [29:43] We try to establish this connection, doesn't work. [29:45] This does not work. [29:45] This does not work. [29:46] This does not work. [29:48] The algorithm will say I can't find a path. [29:51] But if you sample more densely, then I [29:58] can find a path to get out. [30:02] So by the way, these situations are [30:03] referred to as bug traps, the motion planning literature, [30:07] notoriously difficult to deal with. [30:10] They happen to, I mean, they happen, sometimes, [30:13] with planning for robotic manipulators, [30:16] where you have to maneuver in very tight spaces, [30:18] for example to place a cup in a refrigerator, and so on. [30:23] All right. [30:24] So that's PRM in a nutshell. [30:27] So easy algorithm to implement. [30:31] Very general. [30:32] Tends to work pretty well. [30:34] Reliable, what do you use. [30:36] It's a multi query algorithm because if now you give me [30:40] a different initial and goal position, [30:43] I would just connect them to the roadmap, [30:45] and I will solve the problem. [30:46] So it's not really specific to the initial goal position. [30:52] The other class of-- [30:54] well, the other algorithm that is [30:56] very popular under the umbrella of sampling-based algorithm [30:59] is the so-called RRT algorithms, which [31:02] is a single query algorithm that works [31:05] by growing a tree or path rooted at the starting configuration. [31:12] In particular, the way it works is as follows. [31:15] Let's assume that we have already [31:21] grown a portfolios of paths. [31:25] It's really treem which is rooted at initial configuration. [31:28] The way it works is that we build this tree, iteratively, [31:33] by sampling one configuration at a time. [31:36] So for example, here, we sample a new configuration, that one. [31:41] We collision check whether that configuration is collision-free [31:46] or not. [31:47] In this case, it's collision-free, so we keep it. [31:50] Then we find the nearest point in the tree [31:55] to that configuration. [31:58] Then we try to extend the tree either all the way [32:02] to the newly sampled configuration, [32:05] or a fraction of the way. [32:08] Usually, you don't want to extend it all the way, [32:10] because otherwise, it will lead to trees with very long branches [32:15] that might lead to paths that are highly non-optimal. [32:18] So usually, you just extend by, say, 10% of the distance or so. [32:24] Then you collision check this edge. [32:27] In this case, this edge is collision free. [32:29] And then you keep it. [32:30] And then you have added a new tiny branch to your tree. [32:36] And then you keep going. [32:37] So initially you start just from the root of the tree, which [32:42] is the initial configuration. [32:44] And then you, incrementally, proceed with this process. [32:49] You sample new configuration. [32:52] Here, in this case, this configuration [32:55] will be in collision. [32:59] But you still try to extend to it, at least in a way [33:03] that your edge is collision-free. [33:06] And then you keep this edge and so on. [33:09] You keep doing that, and eventually, you [33:12] will get to the goal. [33:16] Does it make sense? [33:17] So this is an incremental algorithm, [33:20] whereas the PRM algorithm, is a batch algorithm. [33:25] The PRM, you throw n samples, and then you build a roadmap. [33:29] Here, you throw one sample at a time, [33:32] and you incrementally build a tree. [33:35] A single query, because we have built the tree [33:38] at the initial configuration. [33:41] So if now you give me another initial configuration, [33:44] is unclear how I will reuse this data structure to plan a path. [33:49] There are ways actually to make RRT also multi query, [33:53] but in general, it's a single query algorithm. [33:56] So this is the procedure in more mathematical terms. [33:59] We sample a new configuration, which we call x rand. [34:02] Rand stands for randomize. [34:04] We find the nearest vertex in the tree to x rand. [34:07] We extend the tree from the nearest neighbor [34:11] to x rand in such a way that the extended edge is collision-free. [34:16] And then we update the tree with a new branch. [34:19] And then we repeat. [34:21] In general, if you do this process, [34:24] it will be very unlikely. [34:27] And actually, it will have a probability 0 that at some point [34:31] you sample the goal tree, and you get the goal tree. [34:34] So RRT is usually implemented with the so-called goal-biasing [34:38] strategy, whereby every few times, [34:41] you sample as a configuration, the goal configuration. [34:47] So basically, in that way, you try [34:49] to see if you can make a direct connection to the goal. [34:52] So say every 20 samples, you sample non-randomly, [34:58] the goal configuration. [35:00] And then you try to extend the tree toward that goal, [35:03] is referred to goal biasing. [35:05] Because doing so, you're biasing the growth [35:07] of the tree toward the goal. [35:09] Practically, if you don't implement goal biasing, [35:12] this algorithm really doesn't work. [35:13] If you implement goal biasing, this algorithm [35:16] works pretty well. [35:18] [INAUDIBLE] the goal you're close enough. [35:20] Like, if you're really far away from the goal, [35:22] would you actually want to check? [35:23] Then that edge would be really long. [35:25] Yeah. [35:25] So there are about 3,000 papers that are offspring [35:31] of the original paper that propose RRT with all sorts [35:35] of variations on the theme. [35:38] How much do we extend, when do we do it, [35:41] how do we select this fraction? [35:44] Zillions of it. [35:45] Because it's a very-- again, This [35:46] is an algorithm that you can implement [35:48] in very few lines of code. [35:49] Very general, works very well in practice. [35:51] Yes, absolutely. [35:53] You could do that as you could do many other things. [35:55] We're going to talk about one particular variation, [35:58] which is what makes this algorithm guaranteed to find [36:02] an optimal solution as the number of samples goes [36:04] to infinity, knowing that there are another 2,199 variations [36:09] on the theme. [36:10] But optimized, of course, is one of the most important [36:13] variations. [36:14] Any questions about this? [36:17] Could you go back a slide? [36:21] [INAUDIBLE] last algorithm, which is why do you [36:26] try to fully connect the graph to all of its neighbors [36:28] beforehand. [36:30] You mean for PRM? [36:31] Yeah. [36:34] Why wouldn't you just try to use some kind of heuristic [36:38] to go towards the end position, and then [36:40] only check if the points are connected, [36:42] if you might use them? [36:43] There's work done in the bottom right of that graph, which [36:46] is never used. [36:47] And so the question is why would you [INAUDIBLE]. [36:50] So one of the very first variations of PRM [36:53] is to build the-- [36:57] so basically, sample, then this-- so the variation [37:03] that I'm referring to is referred to as a lazy PRM. [37:06] So roughly speaking, the way that lazy theorem works [37:10] is that once you have a sample, you just [37:12] build the graph by using connection to your neighbors, [37:17] but without collision checking those connections. [37:21] Then you run A star, and you find the path, [37:25] assuming that there are no collisions. [37:27] And then you collision check the edges along the path. [37:31] If you have not found any edge in collision, [37:34] that's the optimal path. [37:35] If you do, you actually discard that edge and you rerun A star. [37:42] It's called lazy because in that case, [37:43] you evaluate collision checking only when it is really needed [37:47] to validate the terminal path. [37:49] In that way, you, as you said, avoid these wasted effort here. [37:55] This effort would not be wasted if actually you [37:58] plan to use the algorithm in a true multi-query setting, [38:02] meaning that you want to use it for all possible source and goal [38:06] destinations. [38:07] But if you do not want to do it, then lazy PRM is the way to go. [38:11] Indeed, lazy PRM is one of the most popular implementation [38:15] of PRM. [38:17] So that's the main [INAUDIBLE] difference between these two [38:20] algorithms is whether you want to use it [38:21] as a multi-query or a single query. [38:23] Yes, absolutely. [38:24] Got you. [38:26] So this will be more computationally intensive. [38:28] Yes. [38:29] PRM is more computationally intensive by far. [38:32] And you can see why that is the case. [38:34] Because in general, roughly speaking, [38:37] and we'll come back to this point, here, for each sample, [38:41] we have to collision check, at least [38:43] in the basic implementation. [38:44] For each sample, we have to collision [38:46] check each edge to a neighbor. [38:51] In RRT, we collision check only one edge per sample. [39:00] So it's much less computationally intensive. [39:04] Just to be sure, are we taking kind of like an implicit breadth [39:06] first approach, where we add a new edge to every node [39:11] and go back to the beginning? [39:13] No, we don't go back to the beginning. [39:15] Not the beginning. [39:16] But if we have a set of nodes, of set of n nodes, [39:19] are we like just adding one branch to each n node. [39:21] No. [39:22] In RRT, we sample nodes incrementally. [39:27] So we don't have-- in the standard path-finding setup, [39:31] you're given n nodes and you try to find the best [39:34] way to explore all of them. [39:35] Here, we actually explore them. [39:38] So we build the graph incrementally. [39:41] Does that make sense? [39:42] So basically, we sample node here. [39:44] We extend. [39:45] We sample another node here. [39:46] We extend. [39:47] Actually, this question brings to an interpretation of RRT [39:52] that is useful to understand what [39:56] works better in practice, which is the notion of a Voronoi bias. [40:01] The reason why RRT works well, and it's [40:04] called rapidly-exploring random tree, [40:07] is that consider the vertices of the tree, [40:11] and consider the Voronoi diagram with respect [40:15] to the vertices of the tree. [40:17] A Voronoi diagram is basically a diagram [40:19] whereby you have partitioned environment [40:21] into cells with the property that within each cell, [40:27] each point in that cell is closer [40:30] to the representative node of that cell [40:32] than to any other node. [40:34] So for example here, this point is closest to this point [40:37] than to this point. [40:40] The boundary is at the set of points [40:43] that is equidistant from these two nodes. [40:47] There were only bias tells the following, [40:50] that at the frontier of the tree, [40:54] the Voronoi cells tend to be larger, [40:58] Which means that the likelihood that you sample in this region [41:02] is larger. [41:04] The likelihood of sampling is proportional to the area [41:08] of the corresponding cell, which means that basically there [41:12] is a strong bias in the sampling procedure towards sampling [41:16] in unexplored regions. [41:19] So this is basically, going back to your question, [41:22] is related in a way to greedy search on an implicit graph. [41:26] Implicit because we still have to sample the graph. [41:30] So it's just one interpretation of why RRT is called [41:32] rapidly-exploring random trees. [41:40] Yeah. [41:41] In practice for high dimensions, it's pretty intractable [41:45] to compute the Voronoi diagram. [41:47] No, you don't compute the Voronoi diagram for this. [41:50] The Voronoi diagram is only a conceptual structure [41:54] that we use in order to understand [41:56] why RRT work in practice. [41:57] In the field of motion planning, usually, low dimension [42:01] is rule of thumb. [42:01] But low dimension means a two to four. [42:05] Reasonable dimensions means a 4 to, maybe, 12. [42:09] Beyond 12 is beyond what even these algorithms can do. [42:14] And the reason is quite simple. [42:16] So assume that. [42:19] You have a two-dimensional problem, [42:21] and the problem is non-trivial in any other dimensions, which [42:26] means that at the very least, you [42:28] may want to consider a sample in this quadrant, [42:31] a sample in this quadrant, a sample in this quadrant, [42:33] a sample in this quadrant. [42:34] So I would claim that at least you [42:36] need four samples to have a remotely decent coverage [42:42] of the configuration space. [42:44] Now, let's assume that we go to three dimensions. [42:47] So in order to put a sample in each-- [42:53] how do you call the quadrants in two-dimensions? [42:57] Well, but that thing. [42:58] So how many samples do you need? [43:01] [INAUDIBLE] [43:02] So 2 to the 3. [43:03] And then you go to four dimensions. [43:05] How do you call quadrants in four dimensions? [43:08] I don't know. [43:08] Anyway, 16. [43:10] So basically, just roughly speaking, [43:13] you need a number of samples that grows exponentially [43:16] with the dimensions. [43:17] So beyond 12, 13 does not work. [43:19] So what you do in those cases is to either decompose [43:22] the problem into subspaces or in many cases, [43:28] the problem is not interesting in some dimensions, [43:33] meaning that you don't have collisions in other dimensions, [43:36] so you just collapse some of the dimensions. [43:39] But this is beyond the scope of this class. [43:41] For the system we deal with, these algorithms are fine. [43:45] In particular, in this class, we deal mostly [43:49] between three and eight dimensions, so we're good. [43:55] So in practice, when you're finding the nearest vertex, [43:58] do you use some fast running algorithm? [44:00] Yes. [44:00] We're going to go back to what are the bottlenecks [44:02] of these algorithms. [44:03] Nearest neighbor search is certainly [44:05] one of the bottlenecks. [44:06] Yeah. [44:07] And keep in mind that there are about, say, ICRA, [44:11] which is one of the most important robotics conferences [44:15] that has about, say, 1,500 papers per year, [44:18] there are about 200 papers on motion planning. [44:20] So this is still a very active field, particularly, [44:23] for example, the question of scalability is still quite open. [44:27] All right. [44:28] So what about theoretical guarantees. [44:30] Unfortunately, in general, there are no theoretical guarantees [44:34] in finite time. [44:35] The only theoretical guarantees are with respect [44:37] to the asymptotic behavior of these algorithms. [44:39] Asymptotic means that we say something in the case [44:44] where the number of samples goes to infinity. [44:48] And in particular, the very first guarantees [44:50] that were derived are that PRM with a constant radius [44:57] is guaranteed to find a feasible solution if one [44:59] exists as the number of samples goes to infinity. [45:03] So the probability that PRM does not [45:04] find a solution if one exists, that goes to 0 as n [45:09] goes to infinity. [45:12] A similar theoretical guarantee exists for RRT. [45:17] As the number of samples goes to infinity, [45:20] then RRT is guaranteed to find a feasible trajectory, which [45:26] is OK. [45:27] I mean, it's not the best guarantee you can have. [45:29] It's a probabilistic one. [45:30] It's asymptotic, but it's better than nothing. [45:34] In practice, as I said, they work pretty well. [45:37] Now, the guarantees that I just mentioned [45:40] are with respect to feasibility. [45:42] What about quality of the path? [45:46] It turns out that RRT, in terms of quality, [45:50] does not do a good job. [45:52] In particular, oftentimes, as you will see in the P sets, [45:56] RRT produces trajectories that have a lot of zigzag, which [46:01] means that the cost is large. [46:04] Actually, you can prove mathematically [46:06] that with non-negligible probability, in some instances, [46:10] RRT provides a path whose cost is a factor away [46:15] from the optimal cost. [46:17] So, for example, in this case, you [46:19] can prove that it may happen that RRT [46:22] with a non-zero probability will select [46:24] as a path is very long one, as opposed to this blue one. [46:28] The exact reason why that happens, [46:30] we're not going to discuss it, but there [46:32] are a couple of references out there [46:34] that you can look at if you're interested. [46:37] This has prompted in the past 10 years [46:40] or so, quite a bit of research in trying [46:42] to understand what additional conditions should [46:49] be done to the algorithms in order to ensure quality [46:55] of the solution. [46:56] Meaning, having a property that says that as a number of samples [46:59] goes to infinity, then we recover not just a feasible [47:02] solution, but actually, an optimal solution. [47:05] So it turns out that if you choose, for PRM, the radius, [47:11] in a way with a scaling of log n over n to the power 1 over d, [47:16] then PRM is guaranteed to find with probability [47:19] one, an optimal solution if one exists. [47:23] So log n-- so n here is the number of samples, [47:26] d is the number of dimensions. [47:32] As n goes to infinity, log n over n [47:35] goes to 0, which makes sense because basically [47:39] as n goes to infinity. [47:41] So we have an increasing more samples. [47:43] We want to make the radius with respect [47:45] to which we find neighbors smaller and smaller in order [47:48] to contain the number of connections [47:53] that we want to make, that are computationally expensive. [47:57] On the other hand, a ball has a radius of-- [48:04] in the dimension of n over d, which [48:07] means that with this scaling, we are guaranteed that for all n, [48:12] we have roughly log n neighbors. [48:15] That's more or less the idea. [48:17] So this is a good trade off between computation [48:20] and retaining enough connectivity of your roadmap. [48:23] And in particular, it turns out that with this scaling, [48:25] or with a larger scaling, we are guaranteed [48:27] to find an optimal solution. [48:31] And this has been a very influential paper. [48:33] So it turns out that was a logical gap [48:36] in the proof of this result. So this result as stated [48:39] is not quite correct. [48:41] So together with [INAUDIBLE] Solovey, [48:45] who is a postdoc in my lab and a couple of students, [48:47] I recently published a paper where we collect the proof, [48:50] but that required also changing a tiny bit, the results. [48:53] In particular, the scaling should be [INAUDIBLE] 1 [48:55] over d plus 1. [48:57] The plus 1, if you care, is due to the fact [48:59] that in RRT, you sample the nodes incrementally. [49:04] So you also have to account for time as an additional state [49:08] variable. [49:08] So that's why you had plus 1. [49:10] But anyway, it's just a fine detail. [49:12] Similar optimality extension exists for RRT. [49:17] In order to make RRT an optimal sampling-based planner, [49:22] optimal means that as the number of samples goes to infinity, [49:24] you recover an optimal solution. [49:26] You have to do a couple of variations to the RRT algorithm. [49:30] Basically, you have to rewire some edges in such a way [49:34] that you straighten up your RRT. [49:37] We're not going to cover the details, [49:39] but here, pictorially, I show you the typical output of an RRT [49:43] algorithm. [49:44] You see that there is a lot of zigzag. [49:46] And this is zag doesn't really go away as the number of points [49:49] goes to infinity. [49:50] With RRT star, we straighten up the path [49:53] by doing some local rewiring, that I'm not explaining here. [49:56] And so we have a much straighter path, and hence, optimal path. [50:02] So the key point here is that the optimal balance [50:06] of PRM and RRT exist. [50:09] And they require minor variation to the PRM and RRT algorithm. [50:13] So that's good news. [50:15] And there are fairly rigorous mathematical statements [50:19] about optimality in terms of asymptotic convergence. [50:24] So observation, PRM, in general, for given n samples, [50:29] is the algorithm that gives you the highest quality [50:33] solution because it carefully connects the configuration [50:36] space. [50:36] RRT is a much greedier algorithm, [50:39] going back to that question, which means that it's faster. [50:42] Per sample, you only do one collision check. [50:45] PRM, in general, does log n collision checks. [50:48] Because as we said before, the number of neighbors per sample [50:51] is about log n. [50:52] And then you collision check connections to every neighbor. [50:57] So much faster. [50:59] But in general, the quality of the path [51:01] is poor, basically trap itself in initial paths that it finds, [51:05] and then it's unable to get out of those [INAUDIBLE] paths. [51:09] You can offset this behavior, partially, with RRT star, [51:14] but basically RRT star represents a posteriori fix [51:17] of what is bad in RRT. [51:20] Which prompted my research group to design another algorithm [51:23] referred to as FMT star. [51:26] The details are in the paper here. [51:29] Lucas Janson was a former PhD student in my group. [51:32] He's now at Harvard. [51:33] Anyway, so the idea of FMT is to get the best of both worlds, [51:40] from PRM and RRT. [51:41] Basically, it tend to have an algorithm that [51:43] gives you the solution quality of PRM, [51:45] but with the same computational speed of RRT. [51:48] And the way it works is quite similar to I think, [51:51] what you are saying. [51:53] So the way it works is that assume that we [51:56] have built a tree of a path. [51:59] And u1 and u2 are two vertices. [52:07] So this is three. [52:07] This is one branch. [52:08] This is another branch. u1 is terminal node [52:11] for this branch, u2, terminal node for this branch. [52:14] So the idea of FMT is that let's sample a new node, v. [52:18] And let's try to attach v to the branch such [52:25] that the cost of arrival is minimized, [52:28] neglecting a possible collisions of the last step. [52:32] In particular, we connect v to the node u, [52:35] such that the cost to arrive to u [52:38] plus the cost of going from u1 to v, [52:42] or the cost to go from u2 to v is minimized. [52:47] In doing this, we are not-- we are lazily neglecting [52:53] a collision avoidance. [52:55] We just greedily select the node that [52:59] minimizes the cost to arrive to v, for example through u2. [53:03] And then of course, we have to call the collision checker [53:06] in order to validate whether this edge is in collision [53:09] or not. [53:10] In this case, this edge will be collision. [53:13] So you might say, well, then you should [53:15] consider connecting v to u1. [53:18] The algorithm actually says, OK, I will just keep v, altogether, [53:22] which means that for each sample, [53:24] you only carry out one collision check. [53:26] So the key thing that requires a little bit of math to prove [53:30] is that since we are lazily collision-checking edges, [53:37] and sometimes we are suboptimally [53:39] connecting due to this laziness, we sometimes do errors [53:43] in building this roadmap. [53:45] But as the number of points goes to infinity, [53:47] such error becomes vanishingly rare, [53:50] to the point that actually we cover the same graph of PRM, [53:55] but by only doing one collision check per sample. [53:58] So basically, we have, in a way, the computational complexity [54:01] of RRT, one collision check per sample [54:03] with the solution quality of PRM. [54:08] This is the high level idea. [54:10] There are a few additional details, [54:11] but this is basically the bottom line. [54:13] And if you will, it's related to the comment [54:16] that I think you made or some other somebody else made. [54:19] So once you connect to your roadmap, [54:23] why you're not more careful, and just instead of just [54:28] greedily connect, you also account for the notion of cost [54:31] to arrive, which is very much in style [54:34] of path-finding algorithms. [54:43] Cool. [54:43] All right. [54:44] So summary about sampling-based algorithms. [54:48] So the idea of sampling-based algorithms is to represent [54:51] obstacles in the workspace-- [54:53] implicitly, in the workspace. [54:55] And then to transform the problem of computing [54:59] a path of a trajectory into a bunch of subproblems [55:04] that are much easier to solve. [55:07] And in particular, the subproblems [55:08] are the problems whereby we try to connect to samples [55:12] that are close to each other. [55:14] The two main bottlenecks of sampling-based algorithms [55:18] are one, the collision checker. [55:21] We already discussed it. [55:22] There are libraries out there that [55:24] do collision checking for you. [55:26] As a roboticist, typically, you don't [55:27] have to develop a new collision checker, [55:30] but you have to be aware that collision checking, in general, [55:34] is a time-consuming procedure. [55:37] The second bottleneck is presented by nearest neighbor [55:40] search. [55:40] As you were pointing out, we need to search for neighbors. [55:45] And even if you use a dedicated data structures [55:48] that can be computationally intensive. [55:51] But even in that case, there are good solutions that work well, [55:55] both theoretically and practically. [55:58] Yeah. [55:59] So in your diagrams, I didn't see any path collisions [56:02] between two feasible paths. [56:05] Are you also checking those collisions too? [56:08] Path collision between two feasible paths? [56:10] Yeah. [56:11] So first of all, there is no such a notion [56:12] because these are paths for individual robots. [56:15] So the individual robot would take either one or the other. [56:18] So it would never crash with itself. [56:20] But of course, there are generalization [56:25] of these algorithms to the setting whereby [56:27] you have multiple robots, in which case [56:30] you have to account for collisions [56:33] between the trajectories of the different robots. [56:36] Dealing with multiple robots is complicated because quickly, [56:41] the configuration space scales up, because now you're [56:44] adding the configuration of the robot number one [56:46] and configuration robot number two. [56:48] But there are techniques out there. [56:50] In particular, the book from Steve LaValle [56:52] covers some of these techniques. [56:53] But again, those are a bit beyond the scope of this class. [56:57] [INAUDIBLE] almost uniform across the configuration space. [57:01] No. [57:02] As I was mentioning before, practically, it's [57:04] done in non-uniform fashion. [57:06] Non-uniform could be inspired by other-- [57:09] driven by a priori information about the problem. [57:12] Say, you want to sample more closely to your robot, [57:16] or maybe you have some vague idea [57:17] about where the obstacle could be in configuration space. [57:20] But as I said, the other trend right [57:21] now is to use data-driven methods [57:24] to learn non-uniform sampling distribution. [57:27] Yeah. [57:28] Very good point. [57:29] Yes, typically it's done in a non-uniform way. [57:32] But again, these are all variants [57:34] that unfortunately we're not going to cover in this class, [57:36] but I'm going to go-- but I'm giving you [57:38] all the references where these variants are discussed [57:40] at length. [57:43] Second step kinodynamic. [57:45] So you already know what kinodynamic planning really [57:48] entails. [57:49] Now we want to find a trajectory that not only avoids obstacle [57:52] and possibly optimizes the cost function, [57:54] but also respects the dynamic constraints of your system, [57:59] which basically means that at each configuration, [58:03] your set of available motions, as represented by the velocity [58:08] vector, is a constraint. [58:12] Like for example, with a car, you cannot move laterally. [58:15] You can only go forward or turn. [58:19] So the question is, how do we account for dynamic constraints [58:25] into planning by using sampling-based algorithms? [58:28] Conceptually, the good news is that this is relatively simple. [58:32] The implementation is much more complicated, [58:34] but conceptually it's very simple. [58:35] And there are two main ways of handling dynamics [58:40] within motion planning with sampling-based algorithms. [58:42] The first approach is referred to as a forward [58:45] propagation-based approach. [58:48] The idea is to only rely on the forward propagation [58:52] of the dynamics. [58:54] Specifically, let's consider, again, RRT. [58:58] The way that RRT can be implemented [59:01] to account for dynamic constraints [59:03] is relatively simple. [59:05] So what you do is to sample a new state, randomly, [59:11] as you do with RRT. [59:18] So this is your tree. [59:22] I have bended paths because now I'm [59:25] saying that we're doing dynamic planning. [59:27] So the way we go from one sample to the next one [59:31] is not necessarily a straight line. [59:32] It might be curved line because we [59:35] have a kinodynamic constraints. [59:37] So we sample a new point, which we call x rand. [59:43] So that's the line here. [59:45] We find the closest sample in the tree to x rand, [59:53] say, this one here. [59:56] And at this point, I select, randomly, [60:00] the amount of time through which I want to forward propagate [60:05] the motion of the vehicle, and the control that I'm [60:09] going to use in order to forward propagate [60:12] the motion of the vehicle. [60:14] So basically, what I do is to solve the differential equation [60:17] x dot equals f of x near. [60:22] Sorry. [60:23] Whatever I say? [60:25] x dot equals f of x and u. [60:31] Let's call this sample x near. [60:35] With the initial condition x0 equal x near, [60:40] with the control input u, which has been randomly selected. [60:57] So I call this input u bar. [60:59] And then I integrate this differential equation forward [61:02] for t time units. [61:04] So I say that this will lead to a path that looks like this. [61:09] And then I stop. [61:10] And this is my new sample. [61:14] Yeah. [61:15] So just simulate the dynamics forward. [61:17] But you have certain constraints on the sampling of u [61:19] [INAUDIBLE]. [61:20] It wouldn't go backwards. [61:21] No. [61:22] So you randomly sample u in such a way [61:24] that it belongs to capital set U, [61:27] which is the set of available controls at that state. [61:31] Yeah. [61:35] So from intuition standpoint, that's [61:37] probably the easiest thing that you can do. [61:38] You just forward propagate your dynamics [61:40] in order to expand your tree by using [61:42] a randomly selected controls. [61:44] And randomly selecting horizon to forward [61:47] expands your dynamics. [61:49] Why would you randomly select control? [61:52] It might not bring you in the right direction to the point [61:56] you're-- [61:57] That's right. [61:58] But the idea is that if you do this with 1 trillion nodes, [62:03] then chances are, that sooner or later, [62:05] you'll find something that brings you. [62:07] But it's completely uninformed. [62:08] And you are currently right. [62:10] So this is the simplest version of a sampling-based algorithm [62:14] for kinodynamic problems. [62:16] But as you said, it's extremely wasteful [62:18] because it does not capture any domain knowledge [62:22] about the problem. [62:25] So why would people consider this algorithm? [62:28] Is that in some cases, the structure of the dynamics, [62:31] or the structure of the problem is so complicated [62:34] that that's the best you can do. [62:35] So this is really your baseline. [62:37] If the problem looks desperately complicated, [62:40] at least you have this algorithm that you can basically always [62:43] implement. [62:44] But in some cases, in many cases, [62:47] actually, this goes back to your point, [62:51] you can exploit domain knowledge. [62:53] And in particular, what I refer to here [62:55] as domain knowledge is the following. [63:01] Let's assume that now, again, I sample a new node, x rand. [63:09] But we spent two weeks on a trajectory optimization [63:12] that basically, is the problem of finding a trajectory that [63:17] connects two points. [63:19] So now, instead of just randomly selecting an input, [63:23] you solve a trajectory optimization problem [63:25] that optimally connects these two samples. [63:28] This provides a much more direct way to get to your goal [63:31] and in much less wasteful way of assembling controls. [63:36] What is the downside? [63:37] That now, instead of forward-propagating [63:40] the dynamics, here, you're solving [63:43] a trajectory optimization problem. [63:51] Which is more computationally intense. [63:53] And that's where actually we bring together [63:55] the field of motion planning and trajectory optimization. [63:57] Kinodynamic motion planning, if you [63:59] want to account for domain knowledge, [64:01] amounts to breaking the problem into small trajectory [64:05] optimization problems, solve for those, and then [64:08] a posteriori collision check for this path. [64:12] Does it make sense? [64:14] So in some cases, these trajectory optimization [64:16] can be done analytically or by solving [64:19] simple systems of linear equations, [64:21] like by using differential flatness. [64:23] In some other cases, you need to use a full blown optimal control [64:27] methods, such as indirect methods or direct methods, [64:31] as taught to the graduate students [64:33] last week, up to a point where it becomes so complicated, [64:36] then you go back to start with the forward expansion. [64:40] Is it harder to check [INAUDIBLE] with curve line [64:44] generated by the motion planning as opposed to a-- [64:46] No, it's the same. [64:47] Because the way you do is that you discretize the edge. [64:51] Each point corresponds to a configuration. [64:53] So you can call the very-- [64:54] [INAUDIBLE] linear structure in the previous [INAUDIBLE]. [64:58] No. [64:58] Yeah. [64:58] For collision checking, it's just a black box that tells you, [65:01] given this pose, a minor collision. [65:02] That's exactly the same question that you [65:04] answer, either in this setting or in the other setting. [65:09] Yeah. [65:12] In the trajectory optimization situation, [65:15] what if you randomly sample the point that was not available [65:20] in your space-- [65:21] It is not reachable. [65:22] Yeah. [65:22] It is not reachable because of the dynamic. [65:24] Yeah. [65:24] No. [65:25] Great question. [65:25] So one of the nuances of kinodynamic planning [65:28] is that not only you have to reason [65:31] about collision avoidance, but also about non-reachability [65:36] of some of the configurations. [65:39] In particular, and this is related to this point here, [65:44] the two differences between running say, PRM or FMT [65:48] star, with and without dynamic constraints, [65:51] is that if you want to account for dynamic constraints, [65:54] the connection between two points is not a straight line [65:58] was actually solution to trajectory optimization problem, [66:00] number one. [66:01] Number two, what you want to find the neighbors, [66:06] you're no longer just finding points within a given radius. [66:11] That is really meaningless for kinodynamic systems. [66:13] You have to somehow account for the dynamics of your system, [66:17] in particular through the notion of reachable sets. [66:20] What is the reachable set? [66:21] Let's assume that this is the pose of your robot, [66:24] like a unicycle. [66:28] The reachable set is the set of points that [66:32] can be reached within time t. [66:36] So why has this weird form? [66:38] Is that for a car, reaching a point in the forward direction [66:44] is easier than reaching it in the lateral direction. [66:46] Because to reach a point, the lateral direction, [66:48] you have to go forward, say, and then backward. [66:53] Which means that the amount of travel [66:54] that you can do laterally, within a given time horizon t, [66:58] is less than the amount of travel [67:00] that you can do frontally within a given time t. [67:04] So in this class of algorithms, the way [67:07] that you use nearest neighbors is by replacing bolts [67:10] of spheres with reachable sets. [67:13] And this would account for the fact that whether, for example, [67:15] within a time. [67:16] So for example, let's assume that I'm here, [67:21] and I consider this reachable set. [67:26] The point being here, it means that it is beyond what [67:29] can be reached within time t. [67:31] So it would not be considered for inclusion. [67:35] So usually, instead of considering balls of radius r, [67:39] you consider reachable sets for a given time horizon t, [67:42] or in general, for a given class C, [67:44] where the cost could be a combination of time and control [67:46] effort. [67:47] Does it make sense? [67:49] The geometric case, if you think about it, [67:52] is just a special case. [67:53] For the geometric case, what is the reachable set? [68:00] It's just a ball. [68:01] Because for a point with no constraints in the dynamics, [68:04] every direction looks equally feasible. [68:07] So space is isotropic, and the reachable set is a ball. [68:12] You can view kinodynamic planning as the setting [68:14] whereby you have to squish these balls in order to align them [68:17] with the dynamics. [68:18] So two main differences with respect to geometric planning. [68:23] You have to connect points by using reachable sets. [68:28] There might have sometimes [INAUDIBLE] shapes. [68:30] And two, when you connect to samples, [68:34] you cannot just use a straight line, but actually, [68:36] you have to solve a trajectory optimization problem. [68:39] If you can do that quickly, for example, [68:41] by exploiting offline computation, [68:44] then this can run very fast. [68:46] For example, for a double integrator model, [68:50] which is simple, but arguably quite representative [68:52] for a number of systems, because you account for momentum, [68:55] the dynamics, you can solve like a 3D planning problems in about [69:00] 50 milliseconds. [69:03] How do you generate the regional set or that shape. [69:07] Yes. [69:08] You take my class [INAUDIBLE] in the spring. [69:11] So there are well-known methods in optimal control [69:18] to compute those reachable sets. [69:21] So the reachable set, in some cases, [69:22] can be computed analytically. [69:25] In many cases, it can be computed numerically. [69:28] But unfortunately, it is beyond the scope of this class. [69:34] For this class, if we ask you to consider kinodynamic planning, [69:37] we will give you that information [69:39] about the reachable set. [69:40] But knowing that for most representative systems, [69:42] that is known. [69:44] So practically, you can do it. [69:45] So for PRM, when we had just a circle. [69:48] Because it's geometric. [69:49] PRM and kinodynamic, then you will have this-- [69:52] Right. [69:52] But in that case, we were picking a radius. [69:54] And that was a parameter we could change. [69:56] So in this case for the reachable set, [69:58] which would be changing time. [69:59] No. [69:59] Yeah. [70:00] So time, or in general, we talked about the cost [70:04] bounded reachable sets. [70:05] So the set of the configuration that you can reach within a cost [70:10] c upper bar, where c could be time. [70:13] In this case, we talk about the t limited reachable sets. [70:16] Or it could be a combination of time and control effort. [70:21] So just for clarification, [INAUDIBLE] of all [70:24] configurations-- [70:24] That can be reached within a given cost. [70:28] Subject to the differential-- [70:29] Subject to differential constraints. [70:31] And here is again is-- [70:32] let's assume that the cost is really time. [70:35] And you have a car. [70:37] And the car has the heading in this direction. [70:41] So you can go farther in the frontal direction. [70:47] And you can travel a smaller distance [70:48] in the lateral direction. [70:49] Because you need to concatenate motion primitives. [70:52] You can still do it because you can go forward [70:54] and backward, and so on. [70:56] But it takes more time. [70:58] That's why in terms of limited reachable set, the space [71:01] you can travel leads to the squishing reachable set. [71:04] Make sense? [71:07] So it's very natural, I think. [71:09] So reachable set can be a static thing, [71:11] or we want to change it based off [71:13] of the geometry of the space [INAUDIBLE] [71:15] if there are any obstacles, or stuff like that? [71:17] Yeah. [71:18] So the idea of planning is try to separate [71:21] in the couple trajectory optimization [71:23] from obstacle avoidance. [71:25] So the reachable sets, and the trajectory optimization problem [71:28] are solved without accounting for obstacles. [71:32] And then you account for obstacles [71:33] a posteriori through collision checking. [71:35] That's really the hallmark of sampling-based planning, [71:37] to try to decouple the two things in such a way [71:40] that you gain computational tractability. [71:45] All right. [71:46] Very last topic. [71:47] And here, I'll be quite quick. [71:49] Question, do probabilistic planners, [71:52] like PRM that stands for probabilistic roadmap, [71:55] or RRT, that stands for rapidly-exploring random tree, [71:59] really need to be random, meaning relying [72:02] on a randomized sequence? [72:04] Answer to this question is important. [72:06] Because if you're able to actually deal [72:08] with deterministic sequences, you [72:10] may have a number of advantages. [72:12] It would ease certification process. [72:13] So say that you want to apply this algorithm to a flying [72:16] self-driving car, which is safety critical. [72:20] Having a probabilistic guarantee is not [72:22] the best you can hope for. [72:25] It would allow you to reuse offline computation. [72:27] So instead of sampling new roadmaps every time [72:31] you want to solve a motion planning problem, [72:33] you would reuse the same roadmap, which [72:36] means that you can restore that roadmap, pre-compute the, say, [72:41] two-point vulnerable problem, reachable sets, and so on. [72:44] It would also simplify some operations [72:46] like nearest neighbor searches. [72:48] It turns out that this question is related [72:50] to the notion of l2 dispersion. [72:55] The l2 dispersion of a set of points [72:58] is loosely defined as the radius of the largest ball [73:04] that you can fit in that set of points [73:06] without touching none of the points. [73:10] So we say that we have large dispersion if we can fit a ball. [73:14] This does not intersect any of the points [73:17] with the larger radius. [73:20] So given n points, the stupid way of assembling [73:23] would be to put all the n points here, [73:26] which will lead to a very large dispersion sequence, [73:29] because then, the ball that does not intersect any of the point [73:32] will have, say, this radius here. [73:35] This will be a decent sequence. [73:38] In particular, formally, the dispersion for a set of points [73:43] is defined as the supremum over all points [73:47] in the space of the minimum distance between that point [73:51] and one of the sample. [73:52] Which basically means fitting the largest ball [73:56] that doesn't hit any points. [74:00] So you can think that for planning, you [74:04] want to have a sampling, a set of samples that [74:13] have low dispersion, because that [74:15] ensures that you have good coverage on the environment. [74:17] So the natural question is, are sequences, [74:24] with the minimum possible dispersion, known? [74:28] The answer is that, in general, that is not known. [74:31] But what is known is that there are [74:33] deterministic sequences whose dispersion [74:36] is asymptotically optimal. [74:37] So as n goes to infinity, at least the scaling [74:40] is asymptotically optimal scaling. [74:42] And in particular, the scaling goes with n [74:44] to the power of minus 1 over d. [74:47] n is the number of samples. [74:48] It makes sense that the optimal dispersion [74:51] goes to 0 as the number of samples increases. [74:55] But it's not clear how optimal it should go. [74:57] That's the optimal scaling. [75:00] Now it turns out that if you go back to PRM. [75:04] In PRM, as I said, there are two, basically, [75:07] degrees of freedom. [75:09] How big is the ball that you use to connect points? [75:15] And then how do you place the samples [75:19] in the configuration space? [75:21] This is again the animation I'll help PRM works. [75:24] So it turns out that if you choose, [75:27] as a sampling distribution, and the distribution of sample [75:31] that has a low dispersion, meaning [75:34] that the scaling is the asymptotically optimal scaling, [75:37] and those sequences are known. [75:40] And if the radius of the ball decreases no faster than n [75:45] to the power of 1 over d, then PRM [75:49] is guaranteed to converge to the optimal path, deterministically, [75:52] which basically means that contrary to what people believe [75:56] for 25 years, you don't really need random sampling. [75:59] A deterministic sequence that has [76:03] low dispersion and those sequences [76:04] are known achieve asymptotic optimality. [76:07] So that it was a result that they [76:09] published with one of my students [76:11] about three or four years ago. [76:13] And so it's surprising that still, people [76:15] believe that the randomization is really [76:17] the key to the success of RRT and PRM, [76:20] where that is absolutely not true. [76:22] That's not really the reason why they're successful. [76:24] They're successful because they cleverly [76:27] exploit the notion of implicit representation of obstacles [76:30] in the configuration space. [76:32] For some reason, people believe that randomization magically [76:35] was solving a problem, which is obviously not the case. [76:38] Here, it's just a bunch of problems, [76:40] where I compare the performance of PRM [76:44] on deterministic sequences. [76:46] And PRM will run on randomized sequences [76:50] and deterministic sequences. [76:52] Basically, less than 100% means deterministic sequence [76:55] is perform better. [76:57] Higher than 100% means that randomized sequences [77:00] perform better. [77:01] You can see that across the board for planning problems [77:04] in different dimensions from two to eight, [77:07] with kinodynamic chains for manipulators, aircraft, [77:11] and all sorts of systems. [77:13] Deterministic sampling not only has a better [77:15] theoretical properties, but actually performs better [77:18] in practice. [77:19] On top of it, actually turns out that we can also [77:21] provide a convergence rates guarantees, [77:24] meaning how many samples you need in order [77:26] to get to a solution within a certain optimality gap. [77:30] Something that was not known until a few years ago. [77:34] So the main takeaways is that asymptotic optimality can indeed [77:38] be achieved with deterministic sequences, [77:41] and indeed with a smaller connection radius, which [77:43] means less computation. [77:45] And we can also provide convergence rates. [77:48] And experimentally, deterministic sequences [77:51] appear to provide superior performance. [77:53] So this is something that we published quite recently. [77:55] And it's quite surprising that for so many years [77:58] people believed that, as I said, randomization [78:00] was the key to the success of these algorithms, whereby [78:03] running the same algorithms on carefully [78:06] constructed deterministic sequences, [78:08] in particular, deterministic sequences with low dispersion, [78:10] gives you a better performance, that that is actually [78:13] deterministic. [78:14] That opens the door to apply these algorithms to safety [78:17] critical systems, for example. [78:20] All right. [78:21] So the time is up. [78:22] Next time-- well, you had a midterm, sorry. [78:26] But the next, next time, which is Tuesday, [78:29] we start the second module of this class, which will deal [78:32] with the robotic perception. [78:35] In particular, we, first, give an overview [78:38] of the sensors that are available for robots. [78:41] And then we'll focus on cameras, how to model them, [78:45] how to process the data that is collected by camera, how [78:47] to extract features, how to extract semantic meaning, [78:50] and so on and so forth. [78:52] All right. [78:52] So I'm going to have office hours from now till 1:00. [78:56] And otherwise, I'll see you on Tuesday. [78:59] And good luck with the midterm.