WEBVTT 00:00:00.000 --> 00:00:01.580 align:middle line:90% 00:00:01.580 --> 00:00:03.920 align:middle line:84% The following content is provided under a Creative 00:00:03.920 --> 00:00:05.340 align:middle line:90% Commons License. 00:00:05.340 --> 00:00:07.550 align:middle line:84% Your support will help MIT OpenCourseWare 00:00:07.550 --> 00:00:11.640 align:middle line:84% continue to offer high quality educational resources for free. 00:00:11.640 --> 00:00:14.180 align:middle line:84% To make a donation or to view additional materials 00:00:14.180 --> 00:00:18.110 align:middle line:84% from hundreds of MIT courses, visit MIT OpenCourseWare 00:00:18.110 --> 00:00:19.080 align:middle line:90% at ocw.mit.edu. 00:00:19.080 --> 00:00:22.414 align:middle line:90% 00:00:22.414 --> 00:00:23.580 align:middle line:90% JOE LEAVITT: Good afternoon. 00:00:23.580 --> 00:00:24.770 align:middle line:90% My name is Joe Leavitt. 00:00:24.770 --> 00:00:27.390 align:middle line:84% My group members are Ben Ayton, Jess Noss, Erlend Harbitz, 00:00:27.390 --> 00:00:29.870 align:middle line:84% Jake Barnwell, and Sam Pinto, and we're 00:00:29.870 --> 00:00:32.290 align:middle line:84% going to introduce you to the topic incremental path 00:00:32.290 --> 00:00:34.850 align:middle line:90% planning. 00:00:34.850 --> 00:00:37.160 align:middle line:84% Listed on screen are just some of the references 00:00:37.160 --> 00:00:39.620 align:middle line:84% we used in putting together this lecture. 00:00:39.620 --> 00:00:42.332 align:middle line:84% The top three are the main references we used 00:00:42.332 --> 00:00:46.320 align:middle line:84% in developing our material on the D* Lite algorithm, 00:00:46.320 --> 00:00:48.980 align:middle line:84% but also take note of the bottom reference, 00:00:48.980 --> 00:00:52.530 align:middle line:84% which is a paper done within Professor William's group here 00:00:52.530 --> 00:00:53.180 align:middle line:90% at MIT. 00:00:53.180 --> 00:00:56.180 align:middle line:84% It talks about all-pairs, shortest paths 00:00:56.180 --> 00:00:57.797 align:middle line:90% incremental search method. 00:00:57.797 --> 00:00:59.880 align:middle line:84% So if you're interested in more incremental search 00:00:59.880 --> 00:01:01.740 align:middle line:84% methods than those we're going to talk about today, 00:01:01.740 --> 00:01:02.614 align:middle line:90% please get into this. 00:01:02.614 --> 00:01:05.540 align:middle line:84% Especially if you're going to go ahead and do our P set, 00:01:05.540 --> 00:01:10.570 align:middle line:84% these references will be very helpful to you. 00:01:10.570 --> 00:01:13.719 align:middle line:84% So today, we're going to first Monday problem, 00:01:13.719 --> 00:01:16.260 align:middle line:84% then we're going to introduce the idea of incremental search. 00:01:16.260 --> 00:01:18.480 align:middle line:84% Then we're going to go into detail on the D* Lite 00:01:18.480 --> 00:01:21.285 align:middle line:84% algorithm, which is type of incremental path planning, 00:01:21.285 --> 00:01:23.790 align:middle line:84% an algorithm used for incremental path planning, 00:01:23.790 --> 00:01:25.540 align:middle line:84% run through an example of D* Lite, 00:01:25.540 --> 00:01:28.600 align:middle line:84% talk about when it is good to use incremental path planning 00:01:28.600 --> 00:01:32.152 align:middle line:84% when other path planners might be better, 00:01:32.152 --> 00:01:34.110 align:middle line:84% and then I'll go into some algorithm extensions 00:01:34.110 --> 00:01:36.600 align:middle line:84% and related topics, and some applications in mobile 00:01:36.600 --> 00:01:38.250 align:middle line:90% robotics. 00:01:38.250 --> 00:01:40.480 align:middle line:84% So in order to motivate the problem, 00:01:40.480 --> 00:01:43.900 align:middle line:84% we're going to use a robot living in a grid world. 00:01:43.900 --> 00:01:47.640 align:middle line:84% It's at a start location trying to get to a goal location. 00:01:47.640 --> 00:01:50.520 align:middle line:84% It can move up, down, left, or right, or diagonally. 00:01:50.520 --> 00:01:54.130 align:middle line:84% And it can only see the eight cells around it, 00:01:54.130 --> 00:01:56.010 align:middle line:84% as it's moving through this grid world. 00:01:56.010 --> 00:01:59.100 align:middle line:84% And we're going to give some pretty fine map of the world. 00:01:59.100 --> 00:02:02.270 align:middle line:84% This is a map that has the world before it starts moving. 00:02:02.270 --> 00:02:05.520 align:middle line:84% And it's possibly moving through some of these, 00:02:05.520 --> 00:02:08.650 align:middle line:84% new obstacles could appear, or obstacles could go away. 00:02:08.650 --> 00:02:11.614 align:middle line:84% And the idea is that it's going to make observations 00:02:11.614 --> 00:02:13.655 align:middle line:84% as it's moving to this world and update its plane 00:02:13.655 --> 00:02:17.400 align:middle line:84% to get to the goal based on what it sees as it's moving along. 00:02:17.400 --> 00:02:20.750 align:middle line:90% 00:02:20.750 --> 00:02:23.822 align:middle line:90% So I must correct myself. 00:02:23.822 --> 00:02:25.530 align:middle line:84% The map that you're seeing now is the map 00:02:25.530 --> 00:02:26.904 align:middle line:84% that the robot has ahead of time. 00:02:26.904 --> 00:02:29.180 align:middle line:84% The previous map is what the environment actually 00:02:29.180 --> 00:02:30.510 align:middle line:90% looks like. 00:02:30.510 --> 00:02:32.420 align:middle line:84% So as it starts moving, it's going 00:02:32.420 --> 00:02:34.750 align:middle line:84% to make a plan to get from the start to the goal, 00:02:34.750 --> 00:02:36.470 align:middle line:84% and that's what we want it to do. 00:02:36.470 --> 00:02:38.262 align:middle line:84% And it's going to move to the next position 00:02:38.262 --> 00:02:40.470 align:middle line:84% and then realize that there's an obstacle in the path 00:02:40.470 --> 00:02:42.110 align:middle line:84% that it previously planned, and now it 00:02:42.110 --> 00:02:47.250 align:middle line:84% must replan in order to find a valid path to the goal. 00:02:47.250 --> 00:02:49.360 align:middle line:84% And so now, it replans, and continues 00:02:49.360 --> 00:02:52.130 align:middle line:84% to move on the new plan, and now it finds another obstacle 00:02:52.130 --> 00:02:54.810 align:middle line:84% that it has to replan around, and it does the same thing. 00:02:54.810 --> 00:02:56.990 align:middle line:84% And then it continues to walk through 00:02:56.990 --> 00:02:59.660 align:middle line:84% until it gets to a point where we have 00:02:59.660 --> 00:03:01.140 align:middle line:90% a change in the environment. 00:03:01.140 --> 00:03:03.740 align:middle line:84% The change is represented by these two cells 00:03:03.740 --> 00:03:06.200 align:middle line:84% going black here, and now its original plane 00:03:06.200 --> 00:03:07.420 align:middle line:90% is no longer valid again. 00:03:07.420 --> 00:03:09.003 align:middle line:84% And what we want it to do is to replan 00:03:09.003 --> 00:03:13.580 align:middle line:84% to find a new valid optimal path to the goal. 00:03:13.580 --> 00:03:17.190 align:middle line:84% This is just showing here what happened to our environment. 00:03:17.190 --> 00:03:21.740 align:middle line:84% And then the robot replans and continues 00:03:21.740 --> 00:03:23.477 align:middle line:84% to march towards the goal So this 00:03:23.477 --> 00:03:25.310 align:middle line:84% is the behavior we want from a mobile robot, 00:03:25.310 --> 00:03:27.518 align:middle line:84% whether it's a two-dimensional path-planning problem, 00:03:27.518 --> 00:03:32.160 align:middle line:84% or we're talking about multi-dimensional robot arms, 00:03:32.160 --> 00:03:35.510 align:middle line:84% planning, working with humans on a manufacturing 00:03:35.510 --> 00:03:39.124 align:middle line:84% environment, or an activity-planning process. 00:03:39.124 --> 00:03:40.540 align:middle line:84% The idea is that we want the robot 00:03:40.540 --> 00:03:42.890 align:middle line:84% to be able to account for new obstacles or changes 00:03:42.890 --> 00:03:45.280 align:middle line:84% in its representation of the environment 00:03:45.280 --> 00:03:50.150 align:middle line:84% and continue to plan on the fly quickly and optimally. 00:03:50.150 --> 00:03:53.510 align:middle line:84% So there's methods that accomplish this. 00:03:53.510 --> 00:03:56.330 align:middle line:84% One method used to in path planning 00:03:56.330 --> 00:04:00.620 align:middle line:84% is rapidly exploring random trees, or RRTs. 00:04:00.620 --> 00:04:03.930 align:middle line:84% RRTs are basically going to plan, replan, 00:04:03.930 --> 00:04:06.680 align:middle line:84% on every time step based on changes in the environment. 00:04:06.680 --> 00:04:09.420 align:middle line:84% And one problem in my experience here, 00:04:09.420 --> 00:04:13.850 align:middle line:84% though, is that RRTs are suboptimal, even though fast. 00:04:13.850 --> 00:04:22.198 align:middle line:90% 00:04:22.198 --> 00:04:24.830 align:middle line:84% See, here you see the robot has come up with a plan. 00:04:24.830 --> 00:04:27.080 align:middle line:84% It's seeing new obstacles, the table in front of it. 00:04:27.080 --> 00:04:27.705 align:middle line:90% The coffee cup. 00:04:27.705 --> 00:04:30.771 align:middle line:84% And its planning for those things, so the plan it came up 00:04:30.771 --> 00:04:33.541 align:middle line:90% with is clearly not optimal. 00:04:33.541 --> 00:04:38.730 align:middle line:90% 00:04:38.730 --> 00:04:41.400 align:middle line:84% So we can perform some additional computation. 00:04:41.400 --> 00:04:45.360 align:middle line:84% We can use a method that builds upon the RRT, RRT*, 00:04:45.360 --> 00:04:47.920 align:middle line:84% that it finds an optimal path, but that requires a pretty 00:04:47.920 --> 00:04:50.360 align:middle line:84% significant amount of offline computation time, 00:04:50.360 --> 00:04:53.190 align:middle line:84% and then you can accomplish something that looks a little 00:04:53.190 --> 00:04:54.900 align:middle line:90% more normal. 00:04:54.900 --> 00:04:58.154 align:middle line:84% Again, we have to allow the robot 00:04:58.154 --> 00:05:00.549 align:middle line:84% to perform a lot of the computation off-line. 00:05:00.549 --> 00:05:04.390 align:middle line:90% 00:05:04.390 --> 00:05:08.820 align:middle line:84% So how do we accomplish what we want to? 00:05:08.820 --> 00:05:10.990 align:middle line:90% We want to compute quickly. 00:05:10.990 --> 00:05:12.700 align:middle line:84% We want to compute optimally, and then 00:05:12.700 --> 00:05:14.560 align:middle line:84% have the robot continue to move as it's 00:05:14.560 --> 00:05:17.890 align:middle line:84% received new information coming to the environment. 00:05:17.890 --> 00:05:19.660 align:middle line:84% Some problems we're facing, there's 00:05:19.660 --> 00:05:21.266 align:middle line:84% changing environmental conditions. 00:05:21.266 --> 00:05:22.390 align:middle line:90% There's sensor limitations. 00:05:22.390 --> 00:05:23.765 align:middle line:84% You might not be able to see everything 00:05:23.765 --> 00:05:25.570 align:middle line:84% in the environment around you, and then we 00:05:25.570 --> 00:05:27.005 align:middle line:90% have limited computation time. 00:05:27.005 --> 00:05:29.860 align:middle line:90% 00:05:29.860 --> 00:05:32.090 align:middle line:84% And the solution that we're going to explore here 00:05:32.090 --> 00:05:35.510 align:middle line:84% is to reuse that from the previous search, 00:05:35.510 --> 00:05:37.580 align:middle line:84% in order to speed up the search process, 00:05:37.580 --> 00:05:40.516 align:middle line:90% and continue to plan optimally. 00:05:40.516 --> 00:05:42.710 align:middle line:84% And the method we're going to use to accomplish this 00:05:42.710 --> 00:05:44.487 align:middle line:90% is incremental search. 00:05:44.487 --> 00:05:46.070 align:middle line:84% So now, we're going to talk about what 00:05:46.070 --> 00:05:49.170 align:middle line:90% incremental search means. 00:05:49.170 --> 00:05:52.370 align:middle line:84% First to show you kind of how powerful incremental search can 00:05:52.370 --> 00:05:55.290 align:middle line:84% be, this is one example of an incremental search method, 00:05:55.290 --> 00:05:57.030 align:middle line:84% incremental all-pairs shortest paths, 00:05:57.030 --> 00:06:01.190 align:middle line:84% which allows you to online compute paths between any start 00:06:01.190 --> 00:06:03.042 align:middle line:90% and goal location. 00:06:03.042 --> 00:06:05.000 align:middle line:84% The idea is that you have a robot that's trying 00:06:05.000 --> 00:06:06.590 align:middle line:90% to move around one obstacle. 00:06:06.590 --> 00:06:09.380 align:middle line:84% That's what you see in the upper sequence of events there. 00:06:09.380 --> 00:06:11.240 align:middle line:84% And another obstacle is added, and now it 00:06:11.240 --> 00:06:13.120 align:middle line:84% has to plan around this new obstacle. 00:06:13.120 --> 00:06:16.220 align:middle line:84% In using this algorithm, you have a significant increase 00:06:16.220 --> 00:06:18.950 align:middle line:84% in performance and computation time 00:06:18.950 --> 00:06:22.550 align:middle line:84% using the all-pairs shortest paths versus an RRT. 00:06:22.550 --> 00:06:24.380 align:middle line:84% RRT can act as another version of an RRT, 00:06:24.380 --> 00:06:26.880 align:middle line:84% where you're planning basically from both the goal and start 00:06:26.880 --> 00:06:30.080 align:middle line:90% and connecting your paths. 00:06:30.080 --> 00:06:33.230 align:middle line:84% It's on the order of what you get with RRT, 00:06:33.230 --> 00:06:37.890 align:middle line:84% but this RRT connect algorithm doesn't give you 00:06:37.890 --> 00:06:39.740 align:middle line:84% an optimal result. And then it's even 00:06:39.740 --> 00:06:41.987 align:middle line:84% fast than another algorithm probablistic road 00:06:41.987 --> 00:06:44.320 align:middle line:84% mapping, where you're randomly sampling the environment, 00:06:44.320 --> 00:06:48.380 align:middle line:84% and connecting points to produce a path. 00:06:48.380 --> 00:06:50.540 align:middle line:84% So the idea here is that we can really 00:06:50.540 --> 00:06:53.810 align:middle line:84% leverage this idea of reusing results from previous searches 00:06:53.810 --> 00:06:57.005 align:middle line:84% to accomplish fast replanning online. 00:06:57.005 --> 00:06:59.956 align:middle line:90% 00:06:59.956 --> 00:07:01.580 align:middle line:84% So the whole idea of incremental search 00:07:01.580 --> 00:07:05.290 align:middle line:84% is that we're going to form a normal graph search, 00:07:05.290 --> 00:07:08.150 align:middle line:90% just like anybody's used to. 00:07:08.150 --> 00:07:11.620 align:middle line:84% And then we're going to repeat as the robot moves 00:07:11.620 --> 00:07:14.610 align:middle line:84% through the environment, or you're executing your plan, 00:07:14.610 --> 00:07:16.830 align:middle line:84% you're going to execute the next point in the plan, 00:07:16.830 --> 00:07:19.540 align:middle line:90% or the next action in the plan. 00:07:19.540 --> 00:07:23.130 align:middle line:84% We're going to receive changes from the environment, 00:07:23.130 --> 00:07:27.470 align:middle line:84% either through our sensors or information from the outside. 00:07:27.470 --> 00:07:29.510 align:middle line:84% And then we're going to use that information 00:07:29.510 --> 00:07:32.320 align:middle line:84% to update our previous search results instead of completely 00:07:32.320 --> 00:07:36.190 align:middle line:84% replanning, and then continue to form that loop, 00:07:36.190 --> 00:07:38.950 align:middle line:90% until you reach your goal. 00:07:38.950 --> 00:07:41.650 align:middle line:84% So before we get into too many of the details of how we're 00:07:41.650 --> 00:07:43.130 align:middle line:84% going to accomplish this, first, we 00:07:43.130 --> 00:07:44.963 align:middle line:84% want to review the graph search problem. 00:07:44.963 --> 00:07:49.300 align:middle line:84% So in graph search, you have a graph consisting 00:07:49.300 --> 00:07:51.030 align:middle line:90% of vertices or nodes. 00:07:51.030 --> 00:07:53.780 align:middle line:84% We're going to use nodes throughout this lecture 00:07:53.780 --> 00:07:57.670 align:middle line:84% to talk about the vertices or nodes in the graph that 00:07:57.670 --> 00:07:59.530 align:middle line:90% are connected by edges. 00:07:59.530 --> 00:08:01.510 align:middle line:84% And then, there's an edge weighting function 00:08:01.510 --> 00:08:04.800 align:middle line:84% that describes the cost going from one node to another. 00:08:04.800 --> 00:08:07.930 align:middle line:84% And then, a heuristic function is an estimate 00:08:07.930 --> 00:08:11.576 align:middle line:84% of the cost to get from a current node to the goal node. 00:08:11.576 --> 00:08:13.450 align:middle line:84% And then we a start vertex and a gold vertex. 00:08:13.450 --> 00:08:18.108 align:middle line:90% 00:08:18.108 --> 00:08:19.570 align:middle line:84% In the graph search problem, we're 00:08:19.570 --> 00:08:23.310 align:middle line:84% also going to have this idea of a g value, or a cost so far. 00:08:23.310 --> 00:08:26.500 align:middle line:84% We're going to use term g value, which is essentially 00:08:26.500 --> 00:08:29.070 align:middle line:84% the cost that it's taken to get to some predecessor node, 00:08:29.070 --> 00:08:32.200 align:middle line:84% plus the cost of the node that you're actually expanding. 00:08:32.200 --> 00:08:37.059 align:middle line:84% So you would have this W start to S1 cost 00:08:37.059 --> 00:08:38.549 align:middle line:90% to get from start to S1. 00:08:38.549 --> 00:08:40.570 align:middle line:84% And then the g value for S goal would 00:08:40.570 --> 00:08:46.300 align:middle line:84% be the weight from S1 of the goal plus the S1's g value. 00:08:46.300 --> 00:08:46.940 align:middle line:90% Yes, go ahead. 00:08:46.940 --> 00:08:48.025 align:middle line:90% AUDIENCE: What is W? 00:08:48.025 --> 00:08:50.140 align:middle line:84% JOE LEAVITT: W is the weighting function 00:08:50.140 --> 00:08:51.640 align:middle line:84% that I talked about previously here. 00:08:51.640 --> 00:08:54.280 align:middle line:84% So the weighting function describes the cost 00:08:54.280 --> 00:09:00.510 align:middle line:84% to get from one node to another along any edge in a graph. 00:09:00.510 --> 00:09:02.260 align:middle line:84% And then we'd use those waiting functions 00:09:02.260 --> 00:09:05.410 align:middle line:84% to develop the idea of the G value. 00:09:05.410 --> 00:09:10.840 align:middle line:90% So W is just the edge cost. 00:09:10.840 --> 00:09:15.740 align:middle line:84% And then our heuristic value, like, 00:09:15.740 --> 00:09:17.990 align:middle line:84% we use our heuristic function to establish a heuristic 00:09:17.990 --> 00:09:20.120 align:middle line:84% value from one goal to another, and we're going 00:09:20.120 --> 00:09:22.540 align:middle line:90% to call that our h value, or h. 00:09:22.540 --> 00:09:25.241 align:middle line:84% And then the total estimated cost to get from a node 00:09:25.241 --> 00:09:27.240 align:middle line:84% that we're currently expanding in a graph search 00:09:27.240 --> 00:09:34.556 align:middle line:84% is f, which is your cost so far, plus your estimated cost to go. 00:09:34.556 --> 00:09:36.430 align:middle line:84% I think I might have messed that up in there. 00:09:36.430 --> 00:09:38.894 align:middle line:84% Call g value associated cost so far, not cost to go, 00:09:38.894 --> 00:09:40.560 align:middle line:84% and then h is an estimate of cost to go. 00:09:40.560 --> 00:09:44.730 align:middle line:90% 00:09:44.730 --> 00:09:48.087 align:middle line:84% So in order to reason about a real-world environment, 00:09:48.087 --> 00:09:49.670 align:middle line:84% we first have to take that environment 00:09:49.670 --> 00:09:51.961 align:middle line:84% and convert it to a graph, so we can form graph search, 00:09:51.961 --> 00:09:54.330 align:middle line:84% and then do everything we're talking about. 00:09:54.330 --> 00:09:55.650 align:middle line:84% Some of the examples, we're going to talk about, 00:09:55.650 --> 00:09:57.650 align:middle line:84% and some of the ones we've already talked about, 00:09:57.650 --> 00:09:59.510 align:middle line:84% we're using this notion of a grid world. 00:09:59.510 --> 00:10:02.440 align:middle line:84% You can have a four connected graph 00:10:02.440 --> 00:10:05.670 align:middle line:84% or a grid, where a robot can move up, down, left, or right, 00:10:05.670 --> 00:10:08.540 align:middle line:84% basically in x or y directions, or an eight connected graph, 00:10:08.540 --> 00:10:09.920 align:middle line:84% where you can also move along the diagonals. 00:10:09.920 --> 00:10:11.370 align:middle line:84% And then had here, we just showed 00:10:11.370 --> 00:10:13.860 align:middle line:84% the graphs associated with those, with the nodes 00:10:13.860 --> 00:10:16.120 align:middle line:90% and edges. 00:10:16.120 --> 00:10:19.510 align:middle line:84% And then you would have an associated admissible heuristic 00:10:19.510 --> 00:10:23.650 align:middle line:84% with those if you're going to form a heuristic search. 00:10:23.650 --> 00:10:26.450 align:middle line:84% So just know up front that any time we're 00:10:26.450 --> 00:10:28.239 align:middle line:84% moving from a real world environment, 00:10:28.239 --> 00:10:30.530 align:middle line:84% that we're trying to reason about these search methods, 00:10:30.530 --> 00:10:34.690 align:middle line:84% we have to develop an associated graph. 00:10:34.690 --> 00:10:37.297 align:middle line:84% And one more thing that is important as far 00:10:37.297 --> 00:10:38.880 align:middle line:84% as understanding incremental searches, 00:10:38.880 --> 00:10:42.360 align:middle line:84% is the notion or idea of relaxation. 00:10:42.360 --> 00:10:45.320 align:middle line:84% We're going use Dijkstra's algorithm. 00:10:45.320 --> 00:10:47.445 align:middle line:84% Everybody here familiar with Dijkstra's algorithm, 00:10:47.445 --> 00:10:49.550 align:middle line:90% I'm assuming, in the past. 00:10:49.550 --> 00:10:51.176 align:middle line:84% We're going to use Dijkstra's algorithm 00:10:51.176 --> 00:10:53.008 align:middle line:84% to kind of illustrate the idea of relaxation 00:10:53.008 --> 00:10:55.380 align:middle line:84% and remind everybody of what that means, because it's 00:10:55.380 --> 00:10:57.190 align:middle line:84% an important concept when we're talking 00:10:57.190 --> 00:10:58.972 align:middle line:84% about incremental search algorithms. 00:10:58.972 --> 00:11:01.130 align:middle line:84% So essentially, in Dijkstra's algorithm, 00:11:01.130 --> 00:11:03.980 align:middle line:84% it's a single source, shortest path algorithm, a best 00:11:03.980 --> 00:11:04.800 align:middle line:90% first search. 00:11:04.800 --> 00:11:07.050 align:middle line:84% So we're going to make eight expanding nodes according 00:11:07.050 --> 00:11:10.830 align:middle line:84% to their best g value, or cost so far. 00:11:10.830 --> 00:11:13.170 align:middle line:84% And we have our start node initialized 00:11:13.170 --> 00:11:17.670 align:middle line:84% to zero cost so far, because at your start, 00:11:17.670 --> 00:11:18.727 align:middle line:90% haven't accrued any cost. 00:11:18.727 --> 00:11:20.185 align:middle line:84% And then, every other node is going 00:11:20.185 --> 00:11:21.731 align:middle line:90% to be initialized to infinity. 00:11:21.731 --> 00:11:23.730 align:middle line:84% And the idea is we're going to find the shortest 00:11:23.730 --> 00:11:25.190 align:middle line:84% distance to all of the other nodes 00:11:25.190 --> 00:11:30.810 align:middle line:84% by relaxing the cost to those nodes from infinity 00:11:30.810 --> 00:11:35.610 align:middle line:84% down to their actual shortest or lowest cost value. 00:11:35.610 --> 00:11:38.770 align:middle line:84% So we start by expanding the start node, 00:11:38.770 --> 00:11:41.560 align:middle line:84% and then you find the cost so far 00:11:41.560 --> 00:11:44.640 align:middle line:84% to each of the children nodes of the start node. 00:11:44.640 --> 00:11:48.260 align:middle line:84% So you have an edge way to one, and so the node 00:11:48.260 --> 00:11:50.170 align:middle line:90% on the bottom as a value of 1. 00:11:50.170 --> 00:11:53.580 align:middle line:84% Similarly for the middle node at 5, and the top node for 3. 00:11:53.580 --> 00:11:55.289 align:middle line:84% Now, the next node we're going to expand, 00:11:55.289 --> 00:11:56.871 align:middle line:84% can anybody tell me what the next node 00:11:56.871 --> 00:11:59.370 align:middle line:84% we're going to expand based on performing best first search 00:11:59.370 --> 00:12:02.970 align:middle line:90% here using cost so far? 00:12:02.970 --> 00:12:04.410 align:middle line:90% AUDIENCE: The 1? 00:12:04.410 --> 00:12:05.232 align:middle line:90% 1? 00:12:05.232 --> 00:12:05.940 align:middle line:90% JOE LEAVITT: Yes. 00:12:05.940 --> 00:12:08.820 align:middle line:84% So the node labeled 1 here, that has a g value of 1 00:12:08.820 --> 00:12:12.090 align:middle line:84% is the lowest out of 1, 3, and 5, and infinity. 00:12:12.090 --> 00:12:15.110 align:middle line:84% So we're going to go ahead and expand 1 next. 00:12:15.110 --> 00:12:20.370 align:middle line:84% And now, we find that using this value of 1 plus 3 00:12:20.370 --> 00:12:22.830 align:middle line:84% equals 4, which is less than what we previously 00:12:22.830 --> 00:12:23.646 align:middle line:90% had here as 4. 00:12:23.646 --> 00:12:26.020 align:middle line:84% So now the center node has been relaxed from a value of 5 00:12:26.020 --> 00:12:27.060 align:middle line:90% down to 4. 00:12:27.060 --> 00:12:32.480 align:middle line:84% So now the shortest path we found to the center node is 4. 00:12:32.480 --> 00:12:35.220 align:middle line:84% And we're going to continue to perform this 00:12:35.220 --> 00:12:40.200 align:middle line:84% by going through each node in order of the lowest cost. 00:12:40.200 --> 00:12:42.570 align:middle line:84% And similarly, you saw how the top node 00:12:42.570 --> 00:12:46.400 align:middle line:84% was relaxed to 6 by going through 1, 3, and 2, where 00:12:46.400 --> 00:12:48.730 align:middle line:84% as previously, it had a lowest cost of 7 going 00:12:48.730 --> 00:12:50.920 align:middle line:90% through 3 and 4. 00:12:50.920 --> 00:12:52.790 align:middle line:84% And if we format through the whole graph 00:12:52.790 --> 00:12:54.790 align:middle line:84% until all nodes have been unexpanded, 00:12:54.790 --> 00:12:56.780 align:middle line:84% then we've relaxed every node in the graph 00:12:56.780 --> 00:12:59.460 align:middle line:90% to its shortest path value. 00:12:59.460 --> 00:13:02.600 align:middle line:84% And then, we can just do a greedy search 00:13:02.600 --> 00:13:06.800 align:middle line:84% from a chosen goal node, back through the graph 00:13:06.800 --> 00:13:07.810 align:middle line:90% to find shortest path. 00:13:07.810 --> 00:13:10.680 align:middle line:90% 00:13:10.680 --> 00:13:13.770 align:middle line:84% AUDIENCE: Is the term relaxation standard? 00:13:13.770 --> 00:13:17.880 align:middle line:84% Because the fact that you're bringing numbers from infinity 00:13:17.880 --> 00:13:20.300 align:middle line:84% double to small value sounds more 00:13:20.300 --> 00:13:22.040 align:middle line:84% like tightening as opposed to relaxation. 00:13:22.040 --> 00:13:24.090 align:middle line:84% So I was just wondering if people use it 00:13:24.090 --> 00:13:25.830 align:middle line:90% as like, a standard-- 00:13:25.830 --> 00:13:28.440 align:middle line:84% JOE LEAVITT: I'm not sure if it is in all areas, 00:13:28.440 --> 00:13:33.230 align:middle line:84% but I have seen it in multiple places. 00:13:33.230 --> 00:13:35.080 align:middle line:84% When you're talking about graph search, 00:13:35.080 --> 00:13:38.940 align:middle line:84% specifically with respect to Dijkstra's algorithm, 00:13:38.940 --> 00:13:41.670 align:middle line:84% that is where the term relaxation is used. 00:13:41.670 --> 00:13:43.690 align:middle line:84% And we're going to use that as we go throughout 00:13:43.690 --> 00:13:45.660 align:middle line:84% and talk about incremental search. 00:13:45.660 --> 00:13:48.282 align:middle line:84% We're kind of repairing the results because of new edge 00:13:48.282 --> 00:13:49.490 align:middle line:90% weights and things like that. 00:13:49.490 --> 00:13:51.031 align:middle line:84% We're going to figure out which nodes 00:13:51.031 --> 00:13:53.243 align:middle line:84% we have to relax back down to their shortest value 00:13:53.243 --> 00:13:56.571 align:middle line:84% to be able to find the new shortest path. 00:13:56.571 --> 00:13:58.320 align:middle line:84% So that's the term we're going to use here 00:13:58.320 --> 00:14:00.590 align:middle line:90% if it's not used universally. 00:14:00.590 --> 00:14:02.730 align:middle line:90% I can't say for certain. 00:14:02.730 --> 00:14:07.920 align:middle line:84% But we will use relaxation to talk about reducing-- 00:14:07.920 --> 00:14:10.860 align:middle line:90% it's kind of related to-- 00:14:10.860 --> 00:14:13.830 align:middle line:84% think about like, a tension spring. 00:14:13.830 --> 00:14:18.340 align:middle line:84% As you reduce the constraints on it, 00:14:18.340 --> 00:14:21.122 align:middle line:84% it'll slowly relax to its completely relaxed position. 00:14:21.122 --> 00:14:22.330 align:middle line:90% That's kind of the idea here. 00:14:22.330 --> 00:14:25.704 align:middle line:90% 00:14:25.704 --> 00:14:29.510 align:middle line:84% So now that we have talked about the ideas of relaxation 00:14:29.510 --> 00:14:33.860 align:middle line:84% and graph search, how do we reuse the results 00:14:33.860 --> 00:14:36.110 align:middle line:84% from a previous search in a new search field 00:14:36.110 --> 00:14:40.850 align:middle line:84% to employ an incremental search method 00:14:40.850 --> 00:14:43.430 align:middle line:84% where you have done a search, you found the shortest path, 00:14:43.430 --> 00:14:45.760 align:middle line:84% and now, you had some changes to your graph, 00:14:45.760 --> 00:14:50.110 align:middle line:84% and you want to use your previous results to come up 00:14:50.110 --> 00:14:51.470 align:middle line:90% with your new shortest path. 00:14:51.470 --> 00:14:53.060 align:middle line:84% So the way we're going to do that, 00:14:53.060 --> 00:14:54.851 align:middle line:84% is we're going to store our optimal results 00:14:54.851 --> 00:14:58.219 align:middle line:90% from a previous search. 00:14:58.219 --> 00:14:59.760 align:middle line:84% This can be done in a number of ways. 00:14:59.760 --> 00:15:02.460 align:middle line:84% It's going to depend on the algorithm. 00:15:02.460 --> 00:15:06.020 align:middle line:84% But you can soar like, a list of shortest paths 00:15:06.020 --> 00:15:09.210 align:middle line:84% as an incremental all-pairs shortest paths problem. 00:15:09.210 --> 00:15:12.620 align:middle line:84% The one we're going to talk most about is storing g values, 00:15:12.620 --> 00:15:16.090 align:middle line:84% and that's what's done in a D* Lite algorithm, 00:15:16.090 --> 00:15:18.690 align:middle line:84% where you're finding the shortest distance from any 00:15:18.690 --> 00:15:22.815 align:middle line:84% given node that you've expanded so far in your search 00:15:22.815 --> 00:15:25.160 align:middle line:90% to the goal node. 00:15:25.160 --> 00:15:27.560 align:middle line:84% And then, you'll use that data to find out, find 00:15:27.560 --> 00:15:28.710 align:middle line:90% the shortest path. 00:15:28.710 --> 00:15:31.520 align:middle line:84% And then when you go to perform your next iteration, 00:15:31.520 --> 00:15:34.280 align:middle line:84% you'll look for inconsistencies in that graph. 00:15:34.280 --> 00:15:37.030 align:middle line:84% And then in order to find a new shortest path, 00:15:37.030 --> 00:15:40.540 align:middle line:84% we'll leverage that previous data and the inconsistencies 00:15:40.540 --> 00:15:45.464 align:middle line:84% for relaxations to come up with a new optimal solution. 00:15:45.464 --> 00:15:46.880 align:middle line:84% And that's all we're showing here. 00:15:46.880 --> 00:15:48.629 align:middle line:84% On the right side of these two graphics is 00:15:48.629 --> 00:15:50.290 align:middle line:90% you have the initial search. 00:15:50.290 --> 00:15:53.000 align:middle line:84% You have an obstacle that is now in a place that 00:15:53.000 --> 00:15:54.965 align:middle line:84% was wasn't there previously, which 00:15:54.965 --> 00:15:58.100 align:middle line:84% causes you to update edge weights in the graph associated 00:15:58.100 --> 00:15:59.470 align:middle line:90% with this grid world. 00:15:59.470 --> 00:16:01.580 align:middle line:84% And then using these values which 00:16:01.580 --> 00:16:05.690 align:middle line:84% represent the g values in the graph search, 00:16:05.690 --> 00:16:08.420 align:middle line:84% you only update the ones you need 00:16:08.420 --> 00:16:11.870 align:middle line:84% to update to find a new optimal path. 00:16:11.870 --> 00:16:16.404 align:middle line:84% And so completely reperforming our search. 00:16:16.404 --> 00:16:17.820 align:middle line:84% So what incremental search methods 00:16:17.820 --> 00:16:22.850 align:middle line:84% exist, now that we've kind of talked about what they are 00:16:22.850 --> 00:16:25.350 align:middle line:90% and what we can do with them? 00:16:25.350 --> 00:16:27.890 align:middle line:84% So the whole idea of this slide here 00:16:27.890 --> 00:16:30.670 align:middle line:84% is to show you that incremental search can 00:16:30.670 --> 00:16:32.090 align:middle line:90% be used across many domains. 00:16:32.090 --> 00:16:34.810 align:middle line:84% It's not just specific to path planning. 00:16:34.810 --> 00:16:37.520 align:middle line:84% It's used in temporal planning to determine 00:16:37.520 --> 00:16:38.840 align:middle line:90% temporal consistency. 00:16:38.840 --> 00:16:41.100 align:middle line:84% It can be used propositional satisfiability. 00:16:41.100 --> 00:16:44.130 align:middle line:90% 00:16:44.130 --> 00:16:48.870 align:middle line:84% Instead of every time you're updating another algorithm, 00:16:48.870 --> 00:16:50.960 align:middle line:84% you can use your previous results. 00:16:50.960 --> 00:16:54.860 align:middle line:84% And the one we're going to focus on today is D* Lite, 00:16:54.860 --> 00:16:57.300 align:middle line:84% which is specific to mobile and robots. 00:16:57.300 --> 00:16:59.850 align:middle line:84% But the ideas and concepts we're going to talk about there 00:16:59.850 --> 00:17:01.760 align:middle line:84% are applicable elsewhere, and you 00:17:01.760 --> 00:17:05.660 align:middle line:84% can use to help you learn about and expand your knowledge 00:17:05.660 --> 00:17:09.175 align:middle line:90% in other incremental search. 00:17:09.175 --> 00:17:11.609 align:middle line:84% So the D* incremental path planning approach. 00:17:11.609 --> 00:17:13.319 align:middle line:84% The whole idea behind it is that we're 00:17:13.319 --> 00:17:16.349 align:middle line:84% going to take the idea of incremental search 00:17:16.349 --> 00:17:18.180 align:middle line:84% that we just talked it about, and we're 00:17:18.180 --> 00:17:20.880 align:middle line:84% going to combine it with heuristic search, which 00:17:20.880 --> 00:17:22.599 align:middle line:90% is an optimal solution. 00:17:22.599 --> 00:17:26.314 align:middle line:84% And the idea is that two things are 00:17:26.314 --> 00:17:28.560 align:middle line:84% orthogonal so we can combine them, 00:17:28.560 --> 00:17:30.690 align:middle line:84% and then we can form efficient incremental path 00:17:30.690 --> 00:17:33.030 align:middle line:84% planning, getting both an optimal result, 00:17:33.030 --> 00:17:35.452 align:middle line:84% and quickly getting a new path when 00:17:35.452 --> 00:17:37.410 align:middle line:84% things change in the environment without having 00:17:37.410 --> 00:17:38.650 align:middle line:90% to completely replan. 00:17:38.650 --> 00:17:42.330 align:middle line:84% And so what that looks like, when 00:17:42.330 --> 00:17:45.410 align:middle line:84% we have a grid world, which is shown in four instances here up 00:17:45.410 --> 00:17:48.690 align:middle line:84% on the board, and we're trying to plan from a start node 00:17:48.690 --> 00:17:51.510 align:middle line:84% to a goal node, and showing each of these sections. 00:17:51.510 --> 00:17:55.522 align:middle line:84% And in the upper left or on the left axis, we have-- 00:17:55.522 --> 00:17:58.959 align:middle line:84% whether we're performing a complete search 00:17:58.959 --> 00:18:00.000 align:middle line:90% or an incremental search. 00:18:00.000 --> 00:18:02.280 align:middle line:84% And on the horizontal axis, whether it's 00:18:02.280 --> 00:18:04.320 align:middle line:84% an uninformed search or a heuristic search. 00:18:04.320 --> 00:18:07.680 align:middle line:84% So the upper left is a complete search, 00:18:07.680 --> 00:18:10.136 align:middle line:84% just using best-first search and no heuristics. 00:18:10.136 --> 00:18:12.510 align:middle line:84% And what it's showing is all the nodes that are expanded, 00:18:12.510 --> 00:18:16.046 align:middle line:84% and trying to find a path from the start to the goal. 00:18:16.046 --> 00:18:19.035 align:middle line:84% In the upper right is a heuristic search, A*. 00:18:19.035 --> 00:18:21.330 align:middle line:84% Employing the heuristic, we're able to collapse 00:18:21.330 --> 00:18:24.200 align:middle line:84% the number of nodes expanded by quite a bit. 00:18:24.200 --> 00:18:28.620 align:middle line:84% In the bottom left is an incremental search method 00:18:28.620 --> 00:18:32.380 align:middle line:84% that's using uninformed search or best-first 00:18:32.380 --> 00:18:35.070 align:middle line:84% search as an underlying search algorithm, 00:18:35.070 --> 00:18:38.650 align:middle line:84% so it's going to expand the same nodes as the best-first search. 00:18:38.650 --> 00:18:42.330 align:middle line:84% And then in the bottom right, you see an incremental 00:18:42.330 --> 00:18:46.780 align:middle line:84% heuristic search, the lifelong planning A* algorithm, 00:18:46.780 --> 00:18:49.447 align:middle line:84% which is kind of the baseline for the D* Lite we're going 00:18:49.447 --> 00:18:53.050 align:middle line:84% to talk about, that the initial search will expand the same 00:18:53.050 --> 00:18:55.510 align:middle line:84% nodes as the heuristic search, since it's using A* as its 00:18:55.510 --> 00:18:58.210 align:middle line:90% baseline. 00:18:58.210 --> 00:19:00.990 align:middle line:84% So now let's say the robot moves. 00:19:00.990 --> 00:19:02.490 align:middle line:84% We get an update to the environment. 00:19:02.490 --> 00:19:05.730 align:middle line:84% There's a bunch of changed edges. 00:19:05.730 --> 00:19:08.280 align:middle line:84% And now best-first search, as you can see, 00:19:08.280 --> 00:19:11.760 align:middle line:84% is going to have to expand pretty much the same number, 00:19:11.760 --> 00:19:14.820 align:middle line:84% if not more nodes, then our previous search. 00:19:14.820 --> 00:19:17.850 align:middle line:84% And A* also is going to expand about the same number of nodes 00:19:17.850 --> 00:19:18.960 align:middle line:90% as it did previously. 00:19:18.960 --> 00:19:22.920 align:middle line:84% The incremental search method using best-first search, 00:19:22.920 --> 00:19:25.290 align:middle line:84% by using previous results, reduces significantly 00:19:25.290 --> 00:19:27.420 align:middle line:84% the number of nodes that needs to be expanded. 00:19:27.420 --> 00:19:30.195 align:middle line:84% And then, lifelong planning A*, the incremental search plus 00:19:30.195 --> 00:19:35.400 align:middle line:84% the heuristic search, reduces that number by even more. 00:19:35.400 --> 00:19:37.776 align:middle line:84% Even when there's quite a few changes in the environment, 00:19:37.776 --> 00:19:39.316 align:middle line:84% you have very few nodes that you need 00:19:39.316 --> 00:19:41.010 align:middle line:84% to expand on the next search in order 00:19:41.010 --> 00:19:45.384 align:middle line:90% to find a new optimal path. 00:19:45.384 --> 00:19:49.872 align:middle line:84% With that, I'll turn it over to Ben to talk about the D* Lite 00:19:49.872 --> 00:19:52.824 align:middle line:90% algorithm. 00:19:52.824 --> 00:19:53.790 align:middle line:90% BEN AYTON: Thanks. 00:19:53.790 --> 00:19:56.800 align:middle line:84% So I'll be walking you through the D* Light algorithm today. 00:19:56.800 --> 00:19:59.315 align:middle line:84% Now as we're emphasizing, the D* Lite is not the only 00:19:59.315 --> 00:20:02.190 align:middle line:84% incremental algorithm, as we mentioned previously. 00:20:02.190 --> 00:20:05.042 align:middle line:84% But it is a widely used incremental algorithm, 00:20:05.042 --> 00:20:09.180 align:middle line:84% and you'll see it's quite efficient in what it does. 00:20:09.180 --> 00:20:12.440 align:middle line:84% D* Lite is an algorithm that searches from a single start 00:20:12.440 --> 00:20:14.480 align:middle line:90% node to a single goal node. 00:20:14.480 --> 00:20:17.950 align:middle line:84% But as we see changes occur in the path along the way, 00:20:17.950 --> 00:20:20.020 align:middle line:90% we'll adjust that. 00:20:20.020 --> 00:20:24.370 align:middle line:84% So it's nice to think about D* Lite from two perspectives. 00:20:24.370 --> 00:20:26.860 align:middle line:84% One is to think about it as a modification of the principle 00:20:26.860 --> 00:20:28.170 align:middle line:90% space. 00:20:28.170 --> 00:20:30.910 align:middle line:84% Now, you'll see that D* Lite in its first iteration or its 00:20:30.910 --> 00:20:33.750 align:middle line:84% first run through before any modifications behaves very 00:20:33.750 --> 00:20:36.737 align:middle line:84% similar to A*, and that's where it's nice to keep that main 00:20:36.737 --> 00:20:37.510 align:middle line:90% model in mind. 00:20:37.510 --> 00:20:41.040 align:middle line:84% And you'll see that in an example further along. 00:20:41.040 --> 00:20:44.730 align:middle line:84% The second perspective to look at A* is from the perspective 00:20:44.730 --> 00:20:47.730 align:middle line:84% of Dijkstra's algorithm, from which we see many 00:20:47.730 --> 00:20:51.690 align:middle line:84% of the principles of relaxation that we covered beforehand. 00:20:51.690 --> 00:20:54.600 align:middle line:84% From this perspective, we see that when we do reparations 00:20:54.600 --> 00:20:58.710 align:middle line:84% to the graph or changes to the graph, our methods of repairs 00:20:58.710 --> 00:21:02.000 align:middle line:84% the graph is essentially to relax down our changed edge 00:21:02.000 --> 00:21:04.080 align:middle line:84% weights until they reach their truth. 00:21:04.080 --> 00:21:06.510 align:middle line:84% Now, where we differ from Dijkstra's algorithm with 00:21:06.510 --> 00:21:11.010 align:middle line:84% something like D* Lite is selecting only the nodes that 00:21:11.010 --> 00:21:12.570 align:middle line:90% we want to hit efficiently. 00:21:12.570 --> 00:21:17.160 align:middle line:84% So whereas Dijkstra's algorithm tried to fit the entire graph, 00:21:17.160 --> 00:21:20.250 align:middle line:84% we only want to travel from a single start node 00:21:20.250 --> 00:21:21.750 align:middle line:90% to a single goal node. 00:21:21.750 --> 00:21:24.860 align:middle line:84% And so D* Lite behaves like Dijkstra's, but a guided 00:21:24.860 --> 00:21:26.970 align:middle line:90% Dijkstra's. 00:21:26.970 --> 00:21:30.400 align:middle line:84% So I'd like to just remind you of several concepts from A* 00:21:30.400 --> 00:21:32.950 align:middle line:84% first, because it's easier to introduce them in that context. 00:21:32.950 --> 00:21:35.070 align:middle line:84% And then we'll modify them a little bit. 00:21:35.070 --> 00:21:38.340 align:middle line:84% So Joe mentioned before that for A*, 00:21:38.340 --> 00:21:41.370 align:middle line:84% we're searching from a start node to a goal node. 00:21:41.370 --> 00:21:43.380 align:middle line:84% We're sorting the nodes in our queue 00:21:43.380 --> 00:21:47.130 align:middle line:84% by total cost, which is the sum of what we're calling g, which 00:21:47.130 --> 00:21:50.495 align:middle line:84% are costs to get to a single node, 00:21:50.495 --> 00:21:53.640 align:middle line:84% and our heuristic, which is the cost to get from a node, 00:21:53.640 --> 00:21:55.546 align:middle line:90% s, to the goal node, your s. 00:21:55.546 --> 00:21:58.740 align:middle line:90% 00:21:58.740 --> 00:22:01.320 align:middle line:84% Now, what we don't usually consider with A*, 00:22:01.320 --> 00:22:05.580 align:middle line:84% but is very important for D*, is how to distinguish between ties 00:22:05.580 --> 00:22:08.880 align:middle line:84% when two different nodes on the search cube have the same total 00:22:08.880 --> 00:22:09.792 align:middle line:90% cost. 00:22:09.792 --> 00:22:11.250 align:middle line:84% So here's what we're doing is we're 00:22:11.250 --> 00:22:14.770 align:middle line:84% introducing a couplet, which consists of two values. 00:22:14.770 --> 00:22:17.940 align:middle line:84% The first, which I'm calling f1 here for any given state, 00:22:17.940 --> 00:22:20.370 align:middle line:84% is the same as what we're usually used to. 00:22:20.370 --> 00:22:22.500 align:middle line:84% It's the linear combination of your cost 00:22:22.500 --> 00:22:25.000 align:middle line:84% to reach the goal node and heuristic function. 00:22:25.000 --> 00:22:28.770 align:middle line:84% But we also introduced a second value 00:22:28.770 --> 00:22:31.140 align:middle line:84% for this couplet that's used in the case for only those 00:22:31.140 --> 00:22:32.250 align:middle line:90% to draw. 00:22:32.250 --> 00:22:35.290 align:middle line:84% And that is simply the cost to go, that g value. 00:22:35.290 --> 00:22:38.155 align:middle line:84% So the order of expansion of nodes from the q 00:22:38.155 --> 00:22:41.880 align:middle line:84% will order them in terms of f1 first, 00:22:41.880 --> 00:22:44.050 align:middle line:84% and if two nodes at the front of the cube 00:22:44.050 --> 00:22:46.120 align:middle line:84% tie in terms of their f1 value, we 00:22:46.120 --> 00:22:49.110 align:middle line:84% select the node with the lowest g value. 00:22:49.110 --> 00:22:52.200 align:middle line:84% So looking at our graph down here, 00:22:52.200 --> 00:22:56.250 align:middle line:84% we're looking at our states s1 here and s2 here. 00:22:56.250 --> 00:23:01.350 align:middle line:84% Can anyone tell me the total cost for our node s1 00:23:01.350 --> 00:23:03.990 align:middle line:84% which, remember, is a couplet of two different values? 00:23:03.990 --> 00:23:11.230 align:middle line:90% 00:23:11.230 --> 00:23:13.220 align:middle line:90% 5, 3. 00:23:13.220 --> 00:23:16.790 align:middle line:84% So five, yes, is our cost to reach 00:23:16.790 --> 00:23:21.170 align:middle line:84% this node one, plus the heuristic, which is 2 here. 00:23:21.170 --> 00:23:24.804 align:middle line:84% And also, 3 is our second value, because it's only our g value 00:23:24.804 --> 00:23:25.640 align:middle line:90% there. 00:23:25.640 --> 00:23:28.470 align:middle line:84% And so what would the value be by the same logic for s2 00:23:28.470 --> 00:23:30.960 align:middle line:90% down here? 00:23:30.960 --> 00:23:31.994 align:middle line:90% AUDIENCE: 5, 2. 00:23:31.994 --> 00:23:32.660 align:middle line:90% BEN AYTON: 5, 2. 00:23:32.660 --> 00:23:35.050 align:middle line:90% That's exactly right. 00:23:35.050 --> 00:23:37.540 align:middle line:84% And so, bearing in mind what I told you before, 00:23:37.540 --> 00:23:40.852 align:middle line:84% where should we take off the q first? 00:23:40.852 --> 00:23:42.230 align:middle line:90% AUDIENCE: S2? 00:23:42.230 --> 00:23:45.260 align:middle line:84% BEN AYTON: That's exactly right, because the first value, f1, 00:23:45.260 --> 00:23:47.060 align:middle line:84% for these two nodes is exactly the same. 00:23:47.060 --> 00:23:52.210 align:middle line:84% But the second value is lower is f(s)2. 00:23:52.210 --> 00:23:55.230 align:middle line:84% I'd also like to introduce the concept of successors 00:23:55.230 --> 00:23:58.390 align:middle line:84% and predecessors of any given node. 00:23:58.390 --> 00:24:01.071 align:middle line:84% In general, a graph consists of directed edges. 00:24:01.071 --> 00:24:03.070 align:middle line:84% And we could have a graph with undirected edges, 00:24:03.070 --> 00:24:05.890 align:middle line:84% but we can think of as just any edge being 00:24:05.890 --> 00:24:07.960 align:middle line:90% directed in both direction. 00:24:07.960 --> 00:24:11.080 align:middle line:84% So here, we define the concept of a successor of a node, which 00:24:11.080 --> 00:24:15.810 align:middle line:84% is every node can be reached from a given node, along edges 00:24:15.810 --> 00:24:17.430 align:middle line:90% that can be found. 00:24:17.430 --> 00:24:19.200 align:middle line:84% So the successors of this given red node 00:24:19.200 --> 00:24:22.770 align:middle line:90% are these two blue nodes here. 00:24:22.770 --> 00:24:25.600 align:middle line:84% We also introduced the concept of predecessors, 00:24:25.600 --> 00:24:28.990 align:middle line:84% which is every node from which that node can be reached, 00:24:28.990 --> 00:24:30.820 align:middle line:84% meaning nodes from which we can follow 00:24:30.820 --> 00:24:33.170 align:middle line:90% our directed edge to our node. 00:24:33.170 --> 00:24:35.690 align:middle line:84% So for our red node, the predecessors 00:24:35.690 --> 00:24:37.260 align:middle line:90% are these two nodes. 00:24:37.260 --> 00:24:38.920 align:middle line:84% What I'd like to emphasize here is 00:24:38.920 --> 00:24:41.500 align:middle line:84% comparing the two, that this node here 00:24:41.500 --> 00:24:45.120 align:middle line:84% was both the processor and the successor of our red node. 00:24:45.120 --> 00:24:46.395 align:middle line:90% And that's fine. 00:24:46.395 --> 00:24:49.460 align:middle line:84% That occurs when we have undirected edges or edges 00:24:49.460 --> 00:24:53.030 align:middle line:90% that direct in both directions. 00:24:53.030 --> 00:24:56.660 align:middle line:84% So think about D* Lite in the sense that is a repeated 00:24:56.660 --> 00:24:59.370 align:middle line:84% best-first search, which is where the A* principles come 00:24:59.370 --> 00:25:03.970 align:middle line:84% in through a graph with changing edge weights as that graph is 00:25:03.970 --> 00:25:07.280 align:middle line:84% traversed, meaning that as we travel from our start node 00:25:07.280 --> 00:25:11.780 align:middle line:84% to our goal node along a path that we are planning, 00:25:11.780 --> 00:25:14.056 align:middle line:84% we can see that edge weights are changing. 00:25:14.056 --> 00:25:15.430 align:middle line:84% And in this example, I'm modeling 00:25:15.430 --> 00:25:19.070 align:middle line:84% an obstacle coming in place between this node here 00:25:19.070 --> 00:25:22.500 align:middle line:84% and the goal, which means that we have removed that edge. 00:25:22.500 --> 00:25:25.381 align:middle line:84% And that's effectively the same as changing an edge weight. 00:25:25.381 --> 00:25:27.506 align:middle line:84% We're just modeling it as changing this edge weight 00:25:27.506 --> 00:25:31.796 align:middle line:84% to infinity, which means that edge can't be traveled along. 00:25:31.796 --> 00:25:33.820 align:middle line:84% And as I mentioned before, we also 00:25:33.820 --> 00:25:37.470 align:middle line:84% view this as replanning through relaxation of path costs. 00:25:37.470 --> 00:25:39.570 align:middle line:84% So once we have moved to this point, 00:25:39.570 --> 00:25:41.630 align:middle line:84% how do we find the path from here 00:25:41.630 --> 00:25:48.690 align:middle line:84% to here when this edge has been removed, essentially? 00:25:48.690 --> 00:25:51.750 align:middle line:84% So we want to make D* Lite efficient. 00:25:51.750 --> 00:25:54.060 align:middle line:84% And it's a repeated search through the graph 00:25:54.060 --> 00:25:55.800 align:middle line:90% as we traverse it. 00:25:55.800 --> 00:26:00.830 align:middle line:84% So you'll see that since we're using these g values, if we 00:26:00.830 --> 00:26:06.510 align:middle line:84% maintain a formulation where we're measuring the distance 00:26:06.510 --> 00:26:10.320 align:middle line:84% to go or to reach a node from our start node, 00:26:10.320 --> 00:26:13.059 align:middle line:84% this is not preserved when we move our start node. 00:26:13.059 --> 00:26:14.975 align:middle line:84% And we move our start node when we're updating 00:26:14.975 --> 00:26:17.040 align:middle line:90% the position of our vehicle. 00:26:17.040 --> 00:26:19.800 align:middle line:84% So let me show this through an example here. 00:26:19.800 --> 00:26:22.860 align:middle line:84% The g value, or cost to get from one start node 00:26:22.860 --> 00:26:25.380 align:middle line:84% to our current node, s, here is the combination 00:26:25.380 --> 00:26:26.460 align:middle line:90% of these two edges. 00:26:26.460 --> 00:26:28.030 align:middle line:90% And so, it sums to 4. 00:26:28.030 --> 00:26:30.750 align:middle line:84% Now when we move along that path that we've 00:26:30.750 --> 00:26:35.340 align:middle line:84% planned to this next node, the g value to get to that same node 00:26:35.340 --> 00:26:36.630 align:middle line:90% is here too. 00:26:36.630 --> 00:26:37.987 align:middle line:90% So this isn't preserved. 00:26:37.987 --> 00:26:40.320 align:middle line:84% And that means that we'd have to potentially reformulate 00:26:40.320 --> 00:26:43.840 align:middle line:84% things in our cue, which would be very inefficient. 00:26:43.840 --> 00:26:46.230 align:middle line:84% So what we do is we reformulate our search. 00:26:46.230 --> 00:26:48.900 align:middle line:84% And it's perfectly valid to reformulate our search 00:26:48.900 --> 00:26:50.460 align:middle line:84% to search in the opposite direction. 00:26:50.460 --> 00:26:54.120 align:middle line:84% We're essentially measuring for our g values now the cost 00:26:54.120 --> 00:26:56.920 align:middle line:84% to reach the goal from a specific. 00:26:56.920 --> 00:27:00.660 align:middle line:84% And instead of heuristics, then measure the costs 00:27:00.660 --> 00:27:03.812 align:middle line:84% to get from this specific node, or to get from the start 00:27:03.812 --> 00:27:05.060 align:middle line:90% node to our specific node. 00:27:05.060 --> 00:27:08.040 align:middle line:84% And so we search from the goal backwards through the graph. 00:27:08.040 --> 00:27:10.825 align:middle line:84% In this case, we see that regardless of whether our start 00:27:10.825 --> 00:27:15.330 align:middle line:84% node, we preserve our g values, and our cost to travel from 00:27:15.330 --> 00:27:16.865 align:middle line:90% s to that goal. 00:27:16.865 --> 00:27:18.240 align:middle line:84% That's what we like, and we we'll 00:27:18.240 --> 00:27:23.490 align:middle line:84% keep that reformulation to maximize efficiency. 00:27:23.490 --> 00:27:26.640 align:middle line:84% So here, I want to give you a kind of overall understanding 00:27:26.640 --> 00:27:29.510 align:middle line:84% of the D* Lite algorithm before we dive into specifics of what 00:27:29.510 --> 00:27:30.850 align:middle line:90% it's doing. 00:27:30.850 --> 00:27:33.900 align:middle line:84% First we want to initialize all nodes as unexpanded. 00:27:33.900 --> 00:27:36.045 align:middle line:84% And what exactly, it means expand a node neighbor 00:27:36.045 --> 00:27:37.840 align:middle line:90% or cover. 00:27:37.840 --> 00:27:39.920 align:middle line:84% And then we're doing a best-first search 00:27:39.920 --> 00:27:42.210 align:middle line:84% from the goal node to the start node 00:27:42.210 --> 00:27:45.410 align:middle line:84% until the start node is consistent with its neighbors. 00:27:45.410 --> 00:27:48.000 align:middle line:84% And I'll again say what that means later. 00:27:48.000 --> 00:27:51.030 align:middle line:84% We then move our start node to the next best vertex, 00:27:51.030 --> 00:27:55.580 align:middle line:84% essentially our node moving along the plan that we created. 00:27:55.580 --> 00:27:59.220 align:middle line:84% And then we have to see if any of those edge costs change. 00:27:59.220 --> 00:28:01.680 align:middle line:84% We track how our heuristics change, and then update 00:28:01.680 --> 00:28:03.530 align:middle line:84% the source nodes of those changed edges. 00:28:03.530 --> 00:28:06.690 align:middle line:84% And we essentially repeat this process 00:28:06.690 --> 00:28:11.890 align:middle line:84% until, again, we've converged to a solution, and we keep moving. 00:28:11.890 --> 00:28:15.450 align:middle line:84% So this best-first search, the A* Lite part of the algorithm, 00:28:15.450 --> 00:28:18.840 align:middle line:84% is where most of the computation is going to be occurring. 00:28:18.840 --> 00:28:21.540 align:middle line:84% However, the incremental component 00:28:21.540 --> 00:28:25.560 align:middle line:84% is actually how can we adjust our edge costs 00:28:25.560 --> 00:28:28.320 align:middle line:84% to make sure that we're using most of the information 00:28:28.320 --> 00:28:31.570 align:middle line:84% that we previously gathered as efficiently as possible. 00:28:31.570 --> 00:28:34.670 align:middle line:84% So I'm going to walk you through various steps of this. 00:28:34.670 --> 00:28:36.810 align:middle line:84% First, I'd like to walk you through this step 00:28:36.810 --> 00:28:39.050 align:middle line:84% of moving to the next best vertex, 00:28:39.050 --> 00:28:42.050 align:middle line:90% just so you understand that. 00:28:42.050 --> 00:28:45.450 align:middle line:84% So we can extract a path, given the path 00:28:45.450 --> 00:28:48.280 align:middle line:84% costs that have, through this simple argument, 00:28:48.280 --> 00:28:52.080 align:middle line:84% which says that we map the successor, 00:28:52.080 --> 00:28:56.502 align:middle line:84% by which I mean we restate what the start node is, 00:28:56.502 --> 00:28:57.960 align:middle line:84% because we're moving from one state 00:28:57.960 --> 00:29:01.200 align:middle line:84% to the next state, which becomes the new start node, 00:29:01.200 --> 00:29:05.820 align:middle line:84% by this form, which essentially says that we're taking the best 00:29:05.820 --> 00:29:11.490 align:middle line:84% path in the sense that we're minimizing the total cost that 00:29:11.490 --> 00:29:16.210 align:middle line:84% would be gained traveling by that path to this node. 00:29:16.210 --> 00:29:20.940 align:middle line:84% So in a sense, the g value to get to this node, 00:29:20.940 --> 00:29:23.520 align:middle line:84% to the red from the green, or color stretching back 00:29:23.520 --> 00:29:27.620 align:middle line:84% through the graph, by this path here, is 7. 00:29:27.620 --> 00:29:31.310 align:middle line:84% But to get to this node from the lower node is 10. 00:29:31.310 --> 00:29:35.130 align:middle line:84% And so our g value that is optimal is 7 in this case, 00:29:35.130 --> 00:29:38.700 align:middle line:84% and know that we pick the successor as the green node 00:29:38.700 --> 00:29:41.240 align:middle line:90% up here. 00:29:41.240 --> 00:29:44.355 align:middle line:84% So now to walk through some principles that 00:29:44.355 --> 00:29:48.210 align:middle line:90% appear in both of these steps. 00:29:48.210 --> 00:29:51.051 align:middle line:84% I want to show how we handle weight changes locally. 00:29:51.051 --> 00:29:52.550 align:middle line:84% And we do this because we don't want 00:29:52.550 --> 00:29:54.716 align:middle line:84% to track weight changes throughout the entire graph. 00:29:54.716 --> 00:29:57.180 align:middle line:84% We want to handle this efficiently and separate them, 00:29:57.180 --> 00:29:59.280 align:middle line:84% so we only need to handle the changes that 00:29:59.280 --> 00:30:01.380 align:middle line:90% really impact our problem. 00:30:01.380 --> 00:30:03.890 align:middle line:84% So this is the same formula that defines our successor, 00:30:03.890 --> 00:30:07.450 align:middle line:84% or our g values for a given node. 00:30:07.450 --> 00:30:09.210 align:middle line:84% But we can see that this may no longer 00:30:09.210 --> 00:30:10.815 align:middle line:90% hold when edge weights change. 00:30:10.815 --> 00:30:13.890 align:middle line:84% The example that I have here is that I've changed 00:30:13.890 --> 00:30:18.120 align:middle line:90% edge weight cost from 6 to 1. 00:30:18.120 --> 00:30:21.420 align:middle line:84% And so now we can reach the red node, searching back 00:30:21.420 --> 00:30:23.090 align:middle line:84% through graph by a new optimal path, 00:30:23.090 --> 00:30:26.820 align:middle line:84% coming down from this node here, which would make our new g 00:30:26.820 --> 00:30:28.710 align:middle line:90% value five instead. 00:30:28.710 --> 00:30:33.450 align:middle line:90% So this hasn't been preserved. 00:30:33.450 --> 00:30:36.660 align:middle line:84% And then these changes then propagate to our predecessors, 00:30:36.660 --> 00:30:39.510 align:middle line:84% because the costs for the processors 00:30:39.510 --> 00:30:43.140 align:middle line:84% are each dependent on the g value for their successor. 00:30:43.140 --> 00:30:45.180 align:middle line:84% And so when we hit a node, we have 00:30:45.180 --> 00:30:48.300 align:middle line:84% to propagate that change all the way back through the graph. 00:30:48.300 --> 00:30:52.110 align:middle line:84% So to do this efficiently, we take an A* like approach, 00:30:52.110 --> 00:30:55.030 align:middle line:84% where we update the lowest cost nodes first, 00:30:55.030 --> 00:30:59.360 align:middle line:84% and we don't expand a node until it is the next lowest cost. 00:30:59.360 --> 00:31:03.030 align:middle line:90% 00:31:03.030 --> 00:31:05.930 align:middle line:84% To help us do this, we're going to store additional fact. 00:31:05.930 --> 00:31:07.765 align:middle line:90% We'll call this rhs. 00:31:07.765 --> 00:31:11.400 align:middle line:84% Now the meaning behind rhs, it comes 00:31:11.400 --> 00:31:14.400 align:middle line:84% from the term right-hand side, which in the paper world, 00:31:14.400 --> 00:31:16.470 align:middle line:84% was first introduced and made more sense than it 00:31:16.470 --> 00:31:17.880 align:middle line:90% does so here. 00:31:17.880 --> 00:31:21.180 align:middle line:84% What you can think of your rhs value as is essentially 00:31:21.180 --> 00:31:23.180 align:middle line:90% you're corrected your g costs. 00:31:23.180 --> 00:31:26.750 align:middle line:84% When edge costs change, how G should 00:31:26.750 --> 00:31:30.741 align:middle line:84% be signed so that it's correct for a different graph. 00:31:30.741 --> 00:31:35.190 align:middle line:84% And when your rhs value is not equal to your g value, that 00:31:35.190 --> 00:31:38.460 align:middle line:84% means that we have what is called a local inconsistency. 00:31:38.460 --> 00:31:41.730 align:middle line:84% So given the logic that your rhs value should 00:31:41.730 --> 00:31:45.360 align:middle line:84% be what your g value would be corrected to be, 00:31:45.360 --> 00:31:48.344 align:middle line:84% what would your rhs value be for this graph here? 00:31:48.344 --> 00:31:54.420 align:middle line:90% 00:31:54.420 --> 00:31:55.230 align:middle line:90% AUDIENCE: 5? 00:31:55.230 --> 00:31:56.890 align:middle line:90% BEN AYTON: 5, yes. 00:31:56.890 --> 00:32:00.330 align:middle line:84% So this is the same graph that I presented previously, 00:32:00.330 --> 00:32:02.822 align:middle line:84% and I walked through how g should be 5. 00:32:02.822 --> 00:32:05.841 align:middle line:90% rhs should be 5 here. 00:32:05.841 --> 00:32:10.280 align:middle line:84% And what we're tracking is rhs and g differently. 00:32:10.280 --> 00:32:13.560 align:middle line:84% So g is what it should be, but we haven't got the dated g 00:32:13.560 --> 00:32:15.540 align:middle line:90% to be 5 yet. 00:32:15.540 --> 00:32:18.060 align:middle line:84% So we have two different types of local consistencies 00:32:18.060 --> 00:32:20.340 align:middle line:90% that we distinguish between. 00:32:20.340 --> 00:32:23.700 align:middle line:84% We have what we call local overconsistency, where g 00:32:23.700 --> 00:32:26.260 align:middle line:90% is greater than your rhs value. 00:32:26.260 --> 00:32:28.580 align:middle line:84% This is going to behave more similarly 00:32:28.580 --> 00:32:33.270 align:middle line:84% to Dijkstra's algorithm, where the value that we're 00:32:33.270 --> 00:32:35.580 align:middle line:84% associating to any given node now is higher 00:32:35.580 --> 00:32:38.100 align:middle line:84% than it should be, and we're relaxing down 00:32:38.100 --> 00:32:39.880 align:middle line:90% to the true value. 00:32:39.880 --> 00:32:41.910 align:middle line:84% However, we have a lovely underconsistent case 00:32:41.910 --> 00:32:44.620 align:middle line:84% that we have to handle slightly differently. 00:32:44.620 --> 00:32:48.740 align:middle line:84% So now I'm going to walk through updating and expanding nodes. 00:32:48.740 --> 00:32:52.770 align:middle line:84% What I mean by updating a node is recomputing the rhs value, 00:32:52.770 --> 00:32:55.470 align:middle line:84% and then placing that node on the priority queue, 00:32:55.470 --> 00:32:59.130 align:middle line:84% if that node is locally inconsistent. 00:32:59.130 --> 00:33:04.320 align:middle line:84% So to give an example here, I've changed this value, 00:33:04.320 --> 00:33:07.350 align:middle line:84% moving from 6 at the bottom to 1, 00:33:07.350 --> 00:33:10.950 align:middle line:84% and recomputing rhs, as we did beforehand. 00:33:10.950 --> 00:33:13.980 align:middle line:84% Now we see that the node is locally inconsistent. 00:33:13.980 --> 00:33:18.530 align:middle line:84% In this case, it's locally underconsistent. 00:33:18.530 --> 00:33:23.170 align:middle line:84% And so we place that on the priority queue. 00:33:23.170 --> 00:33:28.990 align:middle line:84% The priority queue values are based on this new formula. 00:33:28.990 --> 00:33:34.890 align:middle line:84% This new formula essentially handles how we see G and rhs. 00:33:34.890 --> 00:33:38.970 align:middle line:84% In essence, we want to take the minimum value of one of these, 00:33:38.970 --> 00:33:43.305 align:middle line:84% so that we can handle it the first time that it 00:33:43.305 --> 00:33:45.890 align:middle line:84% will cause changes that propagate through the graph. 00:33:45.890 --> 00:33:49.155 align:middle line:84% And so this is essentially our ordering on our priority queue 00:33:49.155 --> 00:33:54.740 align:middle line:84% which is essentially the same coupling that we saw for A*. 00:33:54.740 --> 00:33:56.875 align:middle line:84% But here, we just have g of s replaced 00:33:56.875 --> 00:34:00.770 align:middle line:84% by the minimum of g of s and rhs of s. 00:34:00.770 --> 00:34:04.480 align:middle line:84% And so, here your minimum of g and rhs 00:34:04.480 --> 00:34:08.550 align:middle line:84% is 5, plus a heuristic, leads to a total cost associated 00:34:08.550 --> 00:34:11.969 align:middle line:90% with this node of 9, 5. 00:34:11.969 --> 00:34:14.280 align:middle line:84% So we expand our five prior nodes 00:34:14.280 --> 00:34:19.260 align:middle line:84% by then taking them off the priority queue and changing g. 00:34:19.260 --> 00:34:22.290 align:middle line:84% Now I said here that we have an inconsistent. 00:34:22.290 --> 00:34:27.449 align:middle line:84% And so, we update this in much the way that we would expect. 00:34:27.449 --> 00:34:30.916 align:middle line:84% Your g values are relaxing down to your rhs values, 00:34:30.916 --> 00:34:35.320 align:middle line:84% and so we just set g to be equal to rhs. 00:34:35.320 --> 00:34:37.920 align:middle line:84% In this case, our node is now locally consistent, 00:34:37.920 --> 00:34:39.780 align:middle line:90% and it's going to stay that way. 00:34:39.780 --> 00:34:42.270 align:middle line:84% So can anyone think of some good reasons 00:34:42.270 --> 00:34:45.570 align:middle line:84% why I go through the process of changing rhs and keeping 00:34:45.570 --> 00:34:48.880 align:middle line:84% track of that new value, only to put on the queue, 00:34:48.880 --> 00:34:50.610 align:middle line:84% and then take it off again, instead 00:34:50.610 --> 00:34:54.953 align:middle line:84% of just recomputing what g of s should be in the first place? 00:34:54.953 --> 00:34:56.429 align:middle line:90% Does anyone have any ideas? 00:34:56.429 --> 00:34:59.873 align:middle line:90% 00:34:59.873 --> 00:35:02.825 align:middle line:90% AUDIENCE: [INAUDIBLE] 00:35:02.825 --> 00:35:05.790 align:middle line:90% 00:35:05.790 --> 00:35:08.660 align:middle line:84% BEN AYTON: What you can think of is that you're 00:35:08.660 --> 00:35:10.960 align:middle line:90% putting something on the queue. 00:35:10.960 --> 00:35:15.630 align:middle line:84% If we update that value immediately, 00:35:15.630 --> 00:35:19.610 align:middle line:84% there are multiple nodes that could change the same node 00:35:19.610 --> 00:35:21.170 align:middle line:90% that we just considered. 00:35:21.170 --> 00:35:25.130 align:middle line:84% By, which I mean more work changes as they propagate back 00:35:25.130 --> 00:35:27.590 align:middle line:84% through the graph, could lead to changes 00:35:27.590 --> 00:35:31.200 align:middle line:84% in rhs value of that specific node. 00:35:31.200 --> 00:35:34.520 align:middle line:84% And so, what we would need to do if we can recompute g of s 00:35:34.520 --> 00:35:39.760 align:middle line:84% every time, is put it on the queue 00:35:39.760 --> 00:35:42.440 align:middle line:84% to signal that we're recomputing g of s, 00:35:42.440 --> 00:35:47.730 align:middle line:84% and do that recomputation, and then new changes we've done, 00:35:47.730 --> 00:35:50.090 align:middle line:84% perform that again, do that again and again 00:35:50.090 --> 00:35:50.930 align:middle line:90% and again and again. 00:35:50.930 --> 00:35:53.414 align:middle line:84% And we want to avoid that situation. 00:35:53.414 --> 00:35:54.830 align:middle line:84% So what we're going to do is we're 00:35:54.830 --> 00:35:57.160 align:middle line:84% going to keep our rhs as essentially a temp 00:35:57.160 --> 00:35:59.990 align:middle line:84% value that can be adjusted when it's in the queue. 00:35:59.990 --> 00:36:02.900 align:middle line:84% So when something is on the queue, it's on there once, 00:36:02.900 --> 00:36:05.570 align:middle line:84% and it can be updated and taken off. 00:36:05.570 --> 00:36:07.320 align:middle line:84% But we know when it's been taken off, 00:36:07.320 --> 00:36:09.070 align:middle line:84% that it's been taken for the correct time, 00:36:09.070 --> 00:36:11.210 align:middle line:84% and won't need to be handled again. 00:36:11.210 --> 00:36:14.130 align:middle line:84% AUDIENCE: So if I understand this correctly, 00:36:14.130 --> 00:36:20.195 align:middle line:84% so if you had a single edge cost that we changed, 00:36:20.195 --> 00:36:22.730 align:middle line:84% then you could just do what you just said, in terms of, 00:36:22.730 --> 00:36:24.880 align:middle line:84% you could just update the thing, and probably 00:36:24.880 --> 00:36:26.215 align:middle line:90% should be predecessor rate. 00:36:26.215 --> 00:36:26.840 align:middle line:90% BEN AYTON: Yes. 00:36:26.840 --> 00:36:29.120 align:middle line:84% So if you knew for sure that nothing else was coming 00:36:29.120 --> 00:36:31.400 align:middle line:84% back and down, then we could just 00:36:31.400 --> 00:36:33.830 align:middle line:84% do that change and propagate to all the predecessors, 00:36:33.830 --> 00:36:36.359 align:middle line:84% because nothing upstream of that node is affected. 00:36:36.359 --> 00:36:38.150 align:middle line:84% AUDIENCE: So am I understanding correctly-- 00:36:38.150 --> 00:36:43.330 align:middle line:84% so you're basically running the programming, because you're 00:36:43.330 --> 00:36:47.840 align:middle line:84% keeping the costs to goal to devote, 00:36:47.840 --> 00:36:50.060 align:middle line:84% and then what you're doing is every time you have 00:36:50.060 --> 00:36:53.870 align:middle line:84% an edge that changes to a value that could potentially make 00:36:53.870 --> 00:36:55.370 align:middle line:84% that node better than it was before, 00:36:55.370 --> 00:36:59.660 align:middle line:84% so if it changes the value of that node to something 00:36:59.660 --> 00:37:05.090 align:middle line:84% better than it was before, you would change that node, 00:37:05.090 --> 00:37:09.260 align:middle line:84% and then queue all the predecessors, 00:37:09.260 --> 00:37:10.940 align:middle line:84% because those be changed as well. 00:37:10.940 --> 00:37:12.145 align:middle line:90% BEN AYTON: Yes. 00:37:12.145 --> 00:37:14.800 align:middle line:84% AUDIENCE: I mean, if those nodes do not change, 00:37:14.800 --> 00:37:15.932 align:middle line:90% you don't do anything. 00:37:15.932 --> 00:37:17.140 align:middle line:90% You're moving from the queue. 00:37:17.140 --> 00:37:18.650 align:middle line:84% They also change a better bell and keep 00:37:18.650 --> 00:37:20.870 align:middle line:84% propagating until you have nothing else to [INAUDIBLE].. 00:37:20.870 --> 00:37:21.536 align:middle line:90% BEN AYTON: Yeah. 00:37:21.536 --> 00:37:26.720 align:middle line:84% But we only do the propagation, or we 00:37:26.720 --> 00:37:29.030 align:middle line:84% queue changes that occur earlier in the graph, 00:37:29.030 --> 00:37:33.500 align:middle line:84% but we only actually perform the change 00:37:33.500 --> 00:37:36.390 align:middle line:84% resetting the g value, when it's expanded. 00:37:36.390 --> 00:37:38.570 align:middle line:84% AUDIENCE: And then the reason for that 00:37:38.570 --> 00:37:43.726 align:middle line:84% is for efficiency when you have more than one edge changing? 00:37:43.726 --> 00:37:44.350 align:middle line:90% BEN AYTON: Yes. 00:37:44.350 --> 00:37:47.490 align:middle line:84% AUDIENCE: So you could basically have multiple paths affecting 00:37:47.490 --> 00:37:48.680 align:middle line:90% the same node. 00:37:48.680 --> 00:37:51.380 align:middle line:84% And in this situation, if you actually 00:37:51.380 --> 00:37:55.030 align:middle line:84% allow your changes will occur and multiple edges, 00:37:55.030 --> 00:37:57.140 align:middle line:84% that's when the importance of the queue comes. 00:37:57.140 --> 00:37:57.810 align:middle line:90% BEN AYTON: That's exactly. 00:37:57.810 --> 00:37:59.643 align:middle line:84% AUDIENCE: Which is actually, it prevents you 00:37:59.643 --> 00:38:01.620 align:middle line:84% from redoing the same work over and over again. 00:38:01.620 --> 00:38:02.300 align:middle line:90% BEN AYTON: That's exactly right. 00:38:02.300 --> 00:38:04.000 align:middle line:84% Because if another change came through, 00:38:04.000 --> 00:38:04.940 align:middle line:84% then we'd put all the predecessors back in the queue 00:38:04.940 --> 00:38:07.380 align:middle line:84% again, and we'd do all of our updates for all of them 00:38:07.380 --> 00:38:07.880 align:middle line:90% as well. 00:38:07.880 --> 00:38:09.520 align:middle line:90% We're avoiding that process. 00:38:09.520 --> 00:38:13.290 align:middle line:84% AUDIENCE: And the queue ensures that we're only updating-- 00:38:13.290 --> 00:38:15.935 align:middle line:90% and I think that'll cover it. 00:38:15.935 --> 00:38:17.810 align:middle line:84% But in the manner in which we terminate, 00:38:17.810 --> 00:38:20.434 align:middle line:84% we don't necessarily-- we don't expand every node in the queue. 00:38:20.434 --> 00:38:23.180 align:middle line:84% We only expand those we need in order to find the shortest, 00:38:23.180 --> 00:38:25.760 align:middle line:84% and then we retain the queue for that search in case 00:38:25.760 --> 00:38:28.942 align:middle line:84% we need to update those in the next round of the search. 00:38:28.942 --> 00:38:29.650 align:middle line:90% BEN AYTON: Right. 00:38:29.650 --> 00:38:41.410 align:middle line:90% 00:38:41.410 --> 00:38:43.650 align:middle line:84% So I have to correct a mistake in my language 00:38:43.650 --> 00:38:45.450 align:middle line:90% that I said before. 00:38:45.450 --> 00:38:48.072 align:middle line:84% I said that the node in my examples that I gave before 00:38:48.072 --> 00:38:50.280 align:middle line:84% was highly consistent, [INAUDIBLE] wasn't consistent. 00:38:50.280 --> 00:38:52.220 align:middle line:90% I apologize for that mistake. 00:38:52.220 --> 00:38:56.800 align:middle line:84% Other consistency is the easy case that we're handling here, 00:38:56.800 --> 00:39:01.440 align:middle line:84% where when we expand that node, we have to take g of s 00:39:01.440 --> 00:39:06.080 align:middle line:84% and set that to be equal to the rhs value for expansion. 00:39:06.080 --> 00:39:09.310 align:middle line:84% And then we propagate that effect 00:39:09.310 --> 00:39:11.170 align:middle line:84% to all of the predecessors at that point, 00:39:11.170 --> 00:39:14.320 align:middle line:84% which means we update the predecessors 00:39:14.320 --> 00:39:20.370 align:middle line:84% and put those on the queue if it hasn't had an impact on them. 00:39:20.370 --> 00:39:23.420 align:middle line:84% This means that the node is not low key inconsistent, 00:39:23.420 --> 00:39:26.650 align:middle line:84% and it's going to remain that way in that fashion. 00:39:26.650 --> 00:39:28.960 align:middle line:84% The underconsistent case, which was 00:39:28.960 --> 00:39:33.890 align:middle line:84% the opposite of what I showed, is the more difficult case. 00:39:33.890 --> 00:39:36.790 align:middle line:84% In this case, the old path cost was better 00:39:36.790 --> 00:39:38.220 align:middle line:90% than the new rhs value. 00:39:38.220 --> 00:39:41.310 align:middle line:84% Rhs value has increased to above [INAUDIBLE].. 00:39:41.310 --> 00:39:46.090 align:middle line:84% In this case, we can no longer relax down to that g value 00:39:46.090 --> 00:39:48.280 align:middle line:84% from our Dijkstra's algorithm respective. 00:39:48.280 --> 00:39:51.890 align:middle line:84% So we have to think of a new way to handle this. 00:39:51.890 --> 00:39:56.100 align:middle line:84% What we do is we essentially set our g value 00:39:56.100 --> 00:39:58.650 align:middle line:90% to be equal to infinity. 00:39:58.650 --> 00:40:01.090 align:middle line:84% And this is an extra step that's going 00:40:01.090 --> 00:40:05.260 align:middle line:84% to cause the node to go back on the queue an additional time. 00:40:05.260 --> 00:40:09.400 align:middle line:84% But what this mechanism assures is that that node goes back 00:40:09.400 --> 00:40:12.000 align:middle line:84% on the queue at most one additional time, 00:40:12.000 --> 00:40:15.120 align:middle line:84% which means that for D* Lite, we only ever examine any node 00:40:15.120 --> 00:40:19.270 align:middle line:90% at most twice on the queue. 00:40:19.270 --> 00:40:22.220 align:middle line:84% Once we've set g of s to be equal to infinity, 00:40:22.220 --> 00:40:24.040 align:middle line:84% then we can essentially relax down 00:40:24.040 --> 00:40:28.120 align:middle line:84% to the rhs value from the perspective that we had before. 00:40:28.120 --> 00:40:31.180 align:middle line:84% So we set g of s to be equal to infinity, 00:40:31.180 --> 00:40:33.180 align:middle line:84% and we update all the predecessors of s 00:40:33.180 --> 00:40:34.674 align:middle line:90% and s itself this time. 00:40:34.674 --> 00:40:37.090 align:middle line:84% As I was saying before, we need to put that node node back 00:40:37.090 --> 00:40:39.010 align:middle line:84% on the cue so that we handle it when 00:40:39.010 --> 00:40:41.500 align:middle line:90% rhs has hit its minimal value. 00:40:41.500 --> 00:40:43.180 align:middle line:84% And this means that that node is now 00:40:43.180 --> 00:40:46.628 align:middle line:84% going to locally consistent or overconsistent. 00:40:46.628 --> 00:40:49.370 align:middle line:84% And we handle overconsistency in the case 00:40:49.370 --> 00:40:51.190 align:middle line:90% that I've shown you before. 00:40:51.190 --> 00:40:54.940 align:middle line:84% So to give you an idea of how exactly this works, 00:40:54.940 --> 00:40:57.910 align:middle line:90% I'm showing you an example here. 00:40:57.910 --> 00:41:02.980 align:middle line:84% We start off with this graph, and we introduce two changes. 00:41:02.980 --> 00:41:06.750 align:middle line:84% We have increased this edge cost to 5. 00:41:06.750 --> 00:41:10.500 align:middle line:84% And so rhs has now increased to a total of 10. 00:41:10.500 --> 00:41:15.340 align:middle line:84% But I've also changed a value here for edges 00:41:15.340 --> 00:41:16.780 align:middle line:90% that you can't can see. 00:41:16.780 --> 00:41:21.260 align:middle line:84% But assume that the rhs value of this node has also changed. 00:41:21.260 --> 00:41:23.450 align:middle line:84% So now we can put all of these nodes 00:41:23.450 --> 00:41:27.670 align:middle line:84% on a cable ordered in this way, computed using the formula 00:41:27.670 --> 00:41:29.037 align:middle line:90% that we've seen for it. 00:41:29.037 --> 00:41:32.020 align:middle line:90% 00:41:32.020 --> 00:41:35.335 align:middle line:90% We expand the node s1 first. 00:41:35.335 --> 00:41:38.430 align:middle line:84% And because it's underconsistent, 00:41:38.430 --> 00:41:42.060 align:middle line:84% we set g to be equal to infinity. 00:41:42.060 --> 00:41:50.800 align:middle line:84% Then we propagate to all predecessors and s1 itself, 00:41:50.800 --> 00:41:53.040 align:middle line:84% meaning those go back on the queue. 00:41:53.040 --> 00:41:55.110 align:middle line:84% Now, assuming that node changes of any relevance 00:41:55.110 --> 00:41:59.910 align:middle line:84% happen in these nodes, we recompute the costs 00:41:59.910 --> 00:42:03.090 align:middle line:84% for s1, which has now changed to 14, 10, 00:42:03.090 --> 00:42:05.555 align:middle line:84% because we're using the minimum of infinity and 10, 00:42:05.555 --> 00:42:12.070 align:middle line:84% which is 10 plus 14, going behind the node s2. 00:42:12.070 --> 00:42:14.320 align:middle line:90% Next, we expand this node s2. 00:42:14.320 --> 00:42:16.970 align:middle line:84% And because that was an overconsistent case, 00:42:16.970 --> 00:42:20.040 align:middle line:84% we can just set g to be equal to our rhs value. 00:42:20.040 --> 00:42:23.080 align:middle line:84% We then have to propagate that to its predecessors, which 00:42:23.080 --> 00:42:27.490 align:middle line:84% includes s1, which has updated its rhs value. 00:42:27.490 --> 00:42:31.210 align:middle line:84% So if we didn't do this, we wouldn't have 00:42:31.210 --> 00:42:34.420 align:middle line:90% handled this effect correctly. 00:42:34.420 --> 00:42:36.642 align:middle line:84% When we set our rhs value here, we then 00:42:36.642 --> 00:42:39.890 align:middle line:84% have to put this node back on the queue. 00:42:39.890 --> 00:42:44.460 align:middle line:84% So previously, s1 was already on the queue, 00:42:44.460 --> 00:42:47.290 align:middle line:84% but the effect has to be propagated to this node, which 00:42:47.290 --> 00:42:49.190 align:middle line:84% puts it on the queue again, which 00:42:49.190 --> 00:42:52.010 align:middle line:84% would mean adjusting the values, because rhs 00:42:52.010 --> 00:42:53.230 align:middle line:90% has been reduced by 1. 00:42:53.230 --> 00:42:56.580 align:middle line:84% Both the costs here have been reduced by 1. 00:42:56.580 --> 00:42:58.930 align:middle line:90% Then we expand that node. 00:42:58.930 --> 00:43:00.660 align:middle line:90% We have an overconsistent case. 00:43:00.660 --> 00:43:03.310 align:middle line:84% We're setting g to be equal to rhs. 00:43:03.310 --> 00:43:05.780 align:middle line:84% And finally, we've completed our graph search problem. 00:43:05.780 --> 00:43:09.120 align:middle line:90% 00:43:09.120 --> 00:43:11.840 align:middle line:84% So now what we have to handle is an additional complication 00:43:11.840 --> 00:43:14.260 align:middle line:84% that occurs between several steps, 00:43:14.260 --> 00:43:17.810 align:middle line:84% tracking how our heuristics have changed. 00:43:17.810 --> 00:43:20.600 align:middle line:84% So we want to carry over prior queue, which 00:43:20.600 --> 00:43:22.420 align:middle line:84% was what was mentioned previously, 00:43:22.420 --> 00:43:25.940 align:middle line:84% between our different searches as our stock node moves. 00:43:25.940 --> 00:43:30.000 align:middle line:84% But the problem is that we're moving from our start node 00:43:30.000 --> 00:43:31.790 align:middle line:90% to a different point. 00:43:31.790 --> 00:43:35.130 align:middle line:84% Recall that our heuristic value is measured from any given 00:43:35.130 --> 00:43:36.960 align:middle line:90% node to the stock node. 00:43:36.960 --> 00:43:38.860 align:middle line:84% So when our stock node has changed, 00:43:38.860 --> 00:43:42.400 align:middle line:84% the heuristic value is not going to be the same value 00:43:42.400 --> 00:43:44.090 align:middle line:90% to the new stock mode. 00:43:44.090 --> 00:43:46.430 align:middle line:84% So we want what's already on the queue 00:43:46.430 --> 00:43:49.556 align:middle line:84% to be comparable to what is being computed 00:43:49.556 --> 00:43:51.930 align:middle line:84% for our new value, so we can use the math and the algebra 00:43:51.930 --> 00:43:54.500 align:middle line:90% that we've already done. 00:43:54.500 --> 00:43:57.170 align:middle line:84% So how we handle this is we introduce something called 00:43:57.170 --> 00:44:00.860 align:middle line:84% the key modifier, where we move from the previous start 00:44:00.860 --> 00:44:05.030 align:middle line:84% node, which here I indicated as s last, to the new start node, 00:44:05.030 --> 00:44:07.840 align:middle line:84% all the heuristics for admissible heuristic 00:44:07.840 --> 00:44:11.370 align:middle line:84% are lowered by at most a heuristic from the last start 00:44:11.370 --> 00:44:13.190 align:middle line:90% node to the new start node. 00:44:13.190 --> 00:44:16.100 align:middle line:84% So now, when we add new nodes to the queue, 00:44:16.100 --> 00:44:19.220 align:middle line:84% instead of subtracting that value from everything that's 00:44:19.220 --> 00:44:21.650 align:middle line:84% already on the queue, what we do is we 00:44:21.650 --> 00:44:23.860 align:middle line:84% just increase for all nodes that we 00:44:23.860 --> 00:44:29.840 align:middle line:84% put on the queue, their values, by this new modifier. 00:44:29.840 --> 00:44:32.360 align:middle line:84% Because essentially when we're taking off of the queue, 00:44:32.360 --> 00:44:35.670 align:middle line:84% all that matters is the relative differences between the two. 00:44:35.670 --> 00:44:38.630 align:middle line:84% And so, instead of subtracting a value from everything 00:44:38.630 --> 00:44:41.500 align:middle line:84% on the queue, adding that value to all new values that 00:44:41.500 --> 00:44:43.840 align:middle line:84% occur on the queue will achieve the same purpose. 00:44:43.840 --> 00:44:47.160 align:middle line:84% And so our key modifier is initialized to 0 00:44:47.160 --> 00:44:50.600 align:middle line:84% at the start of the algorithm, and is increased every time 00:44:50.600 --> 00:44:54.890 align:middle line:84% that we update our start by this new value, 00:44:54.890 --> 00:44:56.607 align:middle line:84% by the heuristic between the last start 00:44:56.607 --> 00:44:58.430 align:middle line:90% node and the new start node. 00:44:58.430 --> 00:45:00.890 align:middle line:84% And then we update our total cost 00:45:00.890 --> 00:45:02.860 align:middle line:84% to include the edition of the key modifier 00:45:02.860 --> 00:45:06.446 align:middle line:90% fire for our f1 term. 00:45:06.446 --> 00:45:11.980 align:middle line:84% So now we've covered all of the D* Lite algorithm basics. 00:45:11.980 --> 00:45:13.840 align:middle line:84% And we can actually walk through the slide 00:45:13.840 --> 00:45:16.490 align:middle line:84% that I showed you before with the math. 00:45:16.490 --> 00:45:19.700 align:middle line:84% So when we start, we initialize all of our g values 00:45:19.700 --> 00:45:21.500 align:middle line:84% to be equal to infinity, and all of our rhs 00:45:21.500 --> 00:45:24.522 align:middle line:84% values to be equal to infinity, except at the goal node. 00:45:24.522 --> 00:45:26.480 align:middle line:84% And that's to ensure that we always have a node 00:45:26.480 --> 00:45:29.740 align:middle line:90% to start from our search. 00:45:29.740 --> 00:45:33.025 align:middle line:84% We best-first search from the goal node to the start node 00:45:33.025 --> 00:45:35.080 align:middle line:84% until that's locally consistent and expanded. 00:45:35.080 --> 00:45:36.780 align:middle line:84% We should now know what that means. 00:45:36.780 --> 00:45:40.030 align:middle line:84% And that signal that we have found our best path, 00:45:40.030 --> 00:45:42.920 align:middle line:84% we move according to our movement rule, 00:45:42.920 --> 00:45:45.410 align:middle line:84% and then if any of our edge costs have changed, 00:45:45.410 --> 00:45:49.070 align:middle line:84% we update our key modifier, and update our rhs and queue 00:45:49.070 --> 00:45:52.790 align:middle line:84% positions for the source nodes of change in edge costs, 00:45:52.790 --> 00:45:56.840 align:middle line:84% meaning the nodes from which those edges originated. 00:45:56.840 --> 00:45:59.240 align:middle line:84% And then, we simply repeat from two, 00:45:59.240 --> 00:46:02.272 align:middle line:84% following best-first through the principles 00:46:02.272 --> 00:46:04.230 align:middle line:84% that I've walked you through for overconsistent 00:46:04.230 --> 00:46:06.390 align:middle line:90% and underconsistent nodes. 00:46:06.390 --> 00:46:08.600 align:middle line:84% And as a reminder down here, we always 00:46:08.600 --> 00:46:15.044 align:middle line:84% search or sort by this very long couplet of two values. 00:46:15.044 --> 00:46:16.960 align:middle line:84% So now, we're going to walk through an example 00:46:16.960 --> 00:46:18.361 align:middle line:90% of [INAUDIBLE]. 00:46:18.361 --> 00:46:23.440 align:middle line:90% 00:46:23.440 --> 00:46:24.298 align:middle line:90% JESS NOSS: OK. 00:46:24.298 --> 00:46:27.434 align:middle line:90% 00:46:27.434 --> 00:46:30.530 align:middle line:84% So again, here's the pseudo code that Ben was just presenting. 00:46:30.530 --> 00:46:34.870 align:middle line:84% And we're going to go through a quick example with five nodes, 00:46:34.870 --> 00:46:37.957 align:middle line:84% so this isn't going to show all of the properties of why you 00:46:37.957 --> 00:46:39.790 align:middle line:84% would want to use incremental path planning, 00:46:39.790 --> 00:46:41.160 align:middle line:84% because when you have only five nodes, 00:46:41.160 --> 00:46:43.760 align:middle line:84% you're going to end updating a lot of them a lot of the time. 00:46:43.760 --> 00:46:45.640 align:middle line:84% But after the example, you'll see 00:46:45.640 --> 00:46:48.510 align:middle line:84% how it makes more of a difference on a larger graph. 00:46:48.510 --> 00:46:50.320 align:middle line:84% So in this graph, we're going to have 00:46:50.320 --> 00:46:53.970 align:middle line:84% prior nodes that go A, B, C, D, and G is our goal node. 00:46:53.970 --> 00:46:56.440 align:middle line:84% The robot will start at node A, and its goal 00:46:56.440 --> 00:46:58.139 align:middle line:84% is to find the shortest path to node G, 00:46:58.139 --> 00:46:59.555 align:middle line:84% except that the graph might change 00:46:59.555 --> 00:47:01.180 align:middle line:84% as the robot goes through, or the robot 00:47:01.180 --> 00:47:03.670 align:middle line:90% might gain new information. 00:47:03.670 --> 00:47:07.285 align:middle line:84% So initially, the robot thinks that every node is available. 00:47:07.285 --> 00:47:09.160 align:middle line:84% And it knows that the path lengths are all 1, 00:47:09.160 --> 00:47:12.676 align:middle line:84% except for from D to G, the path length is 10. 00:47:12.676 --> 00:47:14.230 align:middle line:84% So this is a bi-directional graph. 00:47:14.230 --> 00:47:16.610 align:middle line:90% Every edge goes both directions. 00:47:16.610 --> 00:47:18.580 align:middle line:84% And the heuristic, as Ben described, 00:47:18.580 --> 00:47:22.832 align:middle line:84% will be with respect to the start node. 00:47:22.832 --> 00:47:24.790 align:middle line:84% In this case, we'll just say that the heuristic 00:47:24.790 --> 00:47:26.498 align:middle line:84% is the number of nodes to the start node. 00:47:26.498 --> 00:47:29.825 align:middle line:84% So we're ignoring the fact that this edge length is 10. 00:47:29.825 --> 00:47:34.510 align:middle line:84% So first, we'll initialize all the g and rhs values. 00:47:34.510 --> 00:47:36.560 align:middle line:84% So we have infinity at every node, 00:47:36.560 --> 00:47:40.462 align:middle line:84% except for the goal node, which has an rhs value of 0. 00:47:40.462 --> 00:47:41.920 align:middle line:84% So this means that all of the nodes 00:47:41.920 --> 00:47:43.420 align:middle line:84% are correctly locally consistent, 00:47:43.420 --> 00:47:46.350 align:middle line:84% because their g and rhs values are the same, except the goal 00:47:46.350 --> 00:47:47.270 align:middle line:90% node. 00:47:47.270 --> 00:47:49.630 align:middle line:84% So we put the goal node on the queue 00:47:49.630 --> 00:47:54.228 align:middle line:84% using the formula that is on Ben's slide, which 00:47:54.228 --> 00:47:56.700 align:middle line:90% I'll put here for reference. 00:47:56.700 --> 00:48:04.970 align:middle line:84% So the key is going to be these two terms. 00:48:04.970 --> 00:48:11.380 align:middle line:84% First, we have the minimum of g and rhs 00:48:11.380 --> 00:48:17.700 align:middle line:84% plus the heuristic value plus the key modifier, which 00:48:17.700 --> 00:48:20.220 align:middle line:90% is initially 0. 00:48:20.220 --> 00:48:21.990 align:middle line:84% And then we have the second term, which 00:48:21.990 --> 00:48:24.042 align:middle line:90% is just the minimum, g of rhs. 00:48:24.042 --> 00:48:27.970 align:middle line:90% 00:48:27.970 --> 00:48:30.580 align:middle line:84% So you can see how we would get 3, 0 here, 00:48:30.580 --> 00:48:32.680 align:middle line:84% because the 3 comes from the heuristic, 00:48:32.680 --> 00:48:35.205 align:middle line:84% and everything else was 0, so we just added a bunch of 0's. 00:48:35.205 --> 00:48:38.348 align:middle line:90% 00:48:38.348 --> 00:48:40.930 align:middle line:84% So that's everything what happens for the initialization. 00:48:40.930 --> 00:48:42.950 align:middle line:84% And now we have one node on the queue. 00:48:42.950 --> 00:48:46.549 align:middle line:90% So we'll have to dequeue first. 00:48:46.549 --> 00:48:47.590 align:middle line:90% This is an easy question. 00:48:47.590 --> 00:48:48.170 align:middle line:90% AUDIENCE: The node on the queue. 00:48:48.170 --> 00:48:49.711 align:middle line:84% JESS NOSS: The one node on the queue. 00:48:49.711 --> 00:48:53.400 align:middle line:84% So we dequeue G. So we're going out to update its g value. 00:48:53.400 --> 00:48:57.180 align:middle line:84% So as Ben said, the rhs value is sort of a look ahead. 00:48:57.180 --> 00:49:02.820 align:middle line:84% You can also think of rhs as being the shortest path that we 00:49:02.820 --> 00:49:05.750 align:middle line:84% found so far, and G is the guaranteed shortest path 00:49:05.750 --> 00:49:06.472 align:middle line:90% to that node. 00:49:06.472 --> 00:49:07.430 align:middle line:90% So this one is trivial. 00:49:07.430 --> 00:49:09.420 align:middle line:84% The shortest path from this node to itself 00:49:09.420 --> 00:49:13.140 align:middle line:84% has length 0, because its the same node. 00:49:13.140 --> 00:49:18.970 align:middle line:84% So now we're going to update the rhs values and its neighbors. 00:49:18.970 --> 00:49:22.440 align:middle line:84% So it has two neighbors, C and D, and in each case, 00:49:22.440 --> 00:49:25.640 align:middle line:84% the rhs value will be the minimum-- 00:49:25.640 --> 00:49:30.100 align:middle line:90% 00:49:30.100 --> 00:49:35.420 align:middle line:84% this is not actual math, but its the minimum of the edge cost, 00:49:35.420 --> 00:49:39.230 align:middle line:84% the d value of a neighbor plus the edge cost. 00:49:39.230 --> 00:49:42.590 align:middle line:90% 00:49:42.590 --> 00:49:48.730 align:middle line:84% Which is the same thing as the edge weight or the weight w. 00:49:48.730 --> 00:49:53.180 align:middle line:84% So we rhs is 10 here, because it's the distance from g to do, 00:49:53.180 --> 00:49:57.010 align:middle line:84% and then it's one, which is G to C. 00:49:57.010 --> 00:49:59.684 align:middle line:84% So again, we can calculate keys for those. 00:49:59.684 --> 00:50:01.100 align:middle line:84% So now we'll dequeue another node. 00:50:01.100 --> 00:50:02.714 align:middle line:90% What should we dequeue next? 00:50:02.714 --> 00:50:05.636 align:middle line:90% 00:50:05.636 --> 00:50:06.610 align:middle line:90% AUDIENCE: C. 00:50:06.610 --> 00:50:09.450 align:middle line:84% JESS NOSS: C. Because it has a smaller key. 00:50:09.450 --> 00:50:11.400 align:middle line:84% So we dequeue C. We update its g value. 00:50:11.400 --> 00:50:16.340 align:middle line:84% So this is saying, now we know that the shortest path to C 00:50:16.340 --> 00:50:18.640 align:middle line:90% actually has length 1. 00:50:18.640 --> 00:50:22.150 align:middle line:84% And we can also compare this to A* star algorithm by drawing 00:50:22.150 --> 00:50:23.760 align:middle line:90% a search tree. 00:50:23.760 --> 00:50:27.150 align:middle line:90% So in the beginning, we had A*. 00:50:27.150 --> 00:50:34.010 align:middle line:90% 00:50:34.010 --> 00:50:36.180 align:middle line:84% In the beginning, we just had node G, 00:50:36.180 --> 00:50:39.780 align:middle line:84% because we were searching from G backward through the goal. 00:50:39.780 --> 00:50:47.200 align:middle line:84% And then we expanded that back to get C and D. 00:50:47.200 --> 00:50:52.260 align:middle line:84% And then A* to keep track of the path length to that node plus 00:50:52.260 --> 00:50:53.490 align:middle line:90% the heuristic at each node. 00:50:53.490 --> 00:50:58.920 align:middle line:84% So the pathway to C was 1 plus the heuristic 2, 00:50:58.920 --> 00:51:00.325 align:middle line:90% give us the value of 3. 00:51:00.325 --> 00:51:02.616 align:middle line:84% So if we go back a moment, we can see-- 00:51:02.616 --> 00:51:06.018 align:middle line:90% 00:51:06.018 --> 00:51:06.990 align:middle line:90% is that OK? 00:51:06.990 --> 00:51:09.906 align:middle line:90% 00:51:09.906 --> 00:51:12.900 align:middle line:84% So you can see that this 3 is the same as the first point 00:51:12.900 --> 00:51:16.520 align:middle line:84% of the key, and path length one is the same 00:51:16.520 --> 00:51:18.490 align:middle line:90% as the second part of the key. 00:51:18.490 --> 00:51:22.680 align:middle line:84% And then similarly at D, we have the path length, which 00:51:22.680 --> 00:51:29.685 align:middle line:90% is 10, plus the 2, give us 12. 00:51:29.685 --> 00:51:34.702 align:middle line:84% So this 12 is the 12 here, and then the 10 is the path length. 00:51:34.702 --> 00:51:37.760 align:middle line:84% And so you can see how if we were doing this the A*, 00:51:37.760 --> 00:51:39.912 align:middle line:90% you would also dequeue C next. 00:51:39.912 --> 00:51:46.030 align:middle line:90% 00:51:46.030 --> 00:51:46.530 align:middle line:90% OK. 00:51:46.530 --> 00:51:48.620 align:middle line:84% So now, we dequeue C, and we're going 00:51:48.620 --> 00:51:52.460 align:middle line:84% to update the rhs and its neighbors. 00:51:52.460 --> 00:51:53.520 align:middle line:90% So B is pretty simple. 00:51:53.520 --> 00:51:54.990 align:middle line:84% Previously, it's rhs was infinity. 00:51:54.990 --> 00:51:58.190 align:middle line:84% Now the shortest path to B that we've found so far 00:51:58.190 --> 00:52:00.410 align:middle line:90% has length 2. 00:52:00.410 --> 00:52:05.795 align:middle line:84% So here we have B was a path length 2, plus a heuristic of 1 00:52:05.795 --> 00:52:06.705 align:middle line:90% gives us 3. 00:52:06.705 --> 00:52:09.770 align:middle line:90% 00:52:09.770 --> 00:52:10.746 align:middle line:90% But what about D? 00:52:10.746 --> 00:52:12.120 align:middle line:84% Previously, its rhs value was 10, 00:52:12.120 --> 00:52:13.744 align:middle line:84% but do we now have a shorter path to D, 00:52:13.744 --> 00:52:15.810 align:middle line:90% given that we're expanding C? 00:52:15.810 --> 00:52:19.795 align:middle line:90% 00:52:19.795 --> 00:52:21.170 align:middle line:84% Or is 10 still the shortest path? 00:52:21.170 --> 00:52:23.850 align:middle line:90% 00:52:23.850 --> 00:52:26.094 align:middle line:84% AUDIENCE: That is the shortest path, through C. 00:52:26.094 --> 00:52:26.760 align:middle line:90% JESS NOSS: Yeah. 00:52:26.760 --> 00:52:27.970 align:middle line:84% So now that we're expanding C, we 00:52:27.970 --> 00:52:30.492 align:middle line:84% see that you can actually get to D in only two units instead 00:52:30.492 --> 00:52:30.991 align:middle line:90% of 10 units. 00:52:30.991 --> 00:52:33.940 align:middle line:90% So that's better. 00:52:33.940 --> 00:52:37.780 align:middle line:84% So we'll update the rhs value to 2. 00:52:37.780 --> 00:52:44.190 align:middle line:84% And that means that we're going to change D's key accordingly. 00:52:44.190 --> 00:52:47.640 align:middle line:84% And the way that maps to A* is, so if you already have the path 00:52:47.640 --> 00:52:51.890 align:middle line:84% length is 2, plus the heuristic is 2, gives us 4. 00:52:51.890 --> 00:52:54.692 align:middle line:84% And the fact that we're changing the key to 4, 00:52:54.692 --> 00:52:57.210 align:middle line:84% 2 means that we're never going to expand this D, 00:52:57.210 --> 00:53:01.380 align:middle line:84% because A* uses an extended set, so it would always expand this 00:53:01.380 --> 00:53:04.810 align:middle line:84% D with a shorter cost before this one. 00:53:04.810 --> 00:53:07.430 align:middle line:84% So that's indicated by-- removed by changing 00:53:07.430 --> 00:53:10.400 align:middle line:90% the key on the queue. 00:53:10.400 --> 00:53:12.734 align:middle line:90% So what do we do next? 00:53:12.734 --> 00:53:14.150 align:middle line:90% Which node do we dequeue? 00:53:14.150 --> 00:53:17.454 align:middle line:90% 00:53:17.454 --> 00:53:18.339 align:middle line:90% AUDIENCE: B. 00:53:18.339 --> 00:53:18.880 align:middle line:90% JESS NOSS: B? 00:53:18.880 --> 00:53:22.710 align:middle line:84% Yeah, because it has the smaller key. 00:53:22.710 --> 00:53:26.300 align:middle line:84% So we'll dequeue B. We'll update its g value, which is again 00:53:26.300 --> 00:53:31.940 align:middle line:84% saying that we know now that the shortest path to B is 2. 00:53:31.940 --> 00:53:35.480 align:middle line:84% So we dequeue B, and then we expand it. 00:53:35.480 --> 00:53:38.609 align:middle line:90% How many neighbors does B have? 00:53:38.609 --> 00:53:40.472 align:middle line:90% AUDIENCE: Three. 00:53:40.472 --> 00:53:41.180 align:middle line:90% JESS NOSS: Three. 00:53:41.180 --> 00:53:45.090 align:middle line:84% So we're actually expanding to all three of those neighbors. 00:53:45.090 --> 00:53:51.488 align:middle line:84% We have A, C, and D. Can we still see this? 00:53:51.488 --> 00:53:52.464 align:middle line:90% We can. 00:53:52.464 --> 00:53:55.400 align:middle line:90% 00:53:55.400 --> 00:53:57.150 align:middle line:90% So at A, it's pretty simple. 00:53:57.150 --> 00:53:59.510 align:middle line:84% We have the rhs value is 3, because it's the shortest 00:53:59.510 --> 00:54:09.040 align:middle line:84% path through GCB to A. So that's 3, plus the heuristic 0 plus 3. 00:54:09.040 --> 00:54:14.060 align:middle line:84% What about-- oh, see, it doesn't change, because-- 00:54:14.060 --> 00:54:17.040 align:middle line:84% you could go through B and back to C, but-- 00:54:17.040 --> 00:54:18.740 align:middle line:84% oh, C shouldn't actually be on here. 00:54:18.740 --> 00:54:20.114 align:middle line:90% Sorry. 00:54:20.114 --> 00:54:21.530 align:middle line:84% C shouldn't be on here, because we 00:54:21.530 --> 00:54:22.902 align:middle line:90% don't want a loop in our path. 00:54:22.902 --> 00:54:26.710 align:middle line:90% 00:54:26.710 --> 00:54:27.450 align:middle line:90% OK. 00:54:27.450 --> 00:54:32.204 align:middle line:84% So at D, can we do better than this path length of 2? 00:54:32.204 --> 00:54:35.508 align:middle line:90% 00:54:35.508 --> 00:54:36.797 align:middle line:90% AUDIENCE: No. 00:54:36.797 --> 00:54:37.380 align:middle line:90% JESS NOSS: No. 00:54:37.380 --> 00:54:38.400 align:middle line:84% Because what happened was originally, 00:54:38.400 --> 00:54:40.140 align:middle line:84% we had the path length of 10 through G, 00:54:40.140 --> 00:54:42.516 align:middle line:84% and then we had the path length of 2, 3 C, 00:54:42.516 --> 00:54:44.140 align:middle line:84% and this path through B is not as good. 00:54:44.140 --> 00:54:50.230 align:middle line:84% So we're going to keep the same rhs value and key at D. 00:54:50.230 --> 00:54:53.920 align:middle line:84% And we can see that in a lot of he A*, where we would have, 00:54:53.920 --> 00:54:58.410 align:middle line:84% if we did do GCBD, we would have a pathway of 3, 00:54:58.410 --> 00:55:02.160 align:middle line:90% plus the heuristic of 2 is 5. 00:55:02.160 --> 00:55:05.170 align:middle line:84% But we would never actually want to expand that D, 00:55:05.170 --> 00:55:08.470 align:middle line:84% because it's not as good as this one. 00:55:08.470 --> 00:55:09.330 align:middle line:90% OK. 00:55:09.330 --> 00:55:10.750 align:middle line:90% Now what would we dequeue? 00:55:10.750 --> 00:55:14.944 align:middle line:84% Looks like we have two nodes in our queue currently. 00:55:14.944 --> 00:55:16.402 align:middle line:90% JOE LEAVITT: A. 00:55:16.402 --> 00:55:21.285 align:middle line:84% JESS NOSS: A. So we dequeue A. That's 00:55:21.285 --> 00:55:22.993 align:middle line:84% where we trying to get to, because that's 00:55:22.993 --> 00:55:24.440 align:middle line:90% the current start node. 00:55:24.440 --> 00:55:26.910 align:middle line:84% So we set the g value to 3, which 00:55:26.910 --> 00:55:30.410 align:middle line:84% means that that's actually the shortest path. 00:55:30.410 --> 00:55:31.540 align:middle line:90% And we're done. 00:55:31.540 --> 00:55:32.960 align:middle line:90% We found the shortest path. 00:55:32.960 --> 00:55:35.897 align:middle line:84% Well, we're done, except that the path might have to change. 00:55:35.897 --> 00:55:37.980 align:middle line:84% So one thing you notice here is that we never have 00:55:37.980 --> 00:55:40.550 align:middle line:90% to actually set D's g value. 00:55:40.550 --> 00:55:43.910 align:middle line:84% So it's possible, maybe there's still a shorter path to D, 00:55:43.910 --> 00:55:46.720 align:middle line:90% but it doesn't matter. 00:55:46.720 --> 00:55:50.540 align:middle line:84% So now the robot will move to its next best place, which 00:55:50.540 --> 00:55:53.290 align:middle line:84% is B, and we'll update the heuristics accordingly, 00:55:53.290 --> 00:55:55.480 align:middle line:84% because this is our new start node. 00:55:55.480 --> 00:55:58.430 align:middle line:84% So now, again, our heuristic is the number 00:55:58.430 --> 00:56:01.460 align:middle line:84% of nodes away from that green node. 00:56:01.460 --> 00:56:05.850 align:middle line:84% And we're keeping the same items on the queue from before. 00:56:05.850 --> 00:56:09.890 align:middle line:84% But we also have to update the key modifier. 00:56:09.890 --> 00:56:11.750 align:middle line:84% So now what happens is an obstacle appears. 00:56:11.750 --> 00:56:15.102 align:middle line:84% So there's an obstacle at C that the robot can now 00:56:15.102 --> 00:56:16.780 align:middle line:84% see because it's moved closer to node C, 00:56:16.780 --> 00:56:19.320 align:middle line:90% but it couldn't see it before. 00:56:19.320 --> 00:56:23.650 align:middle line:84% So now, we're going to indicate that by keeping all the edges, 00:56:23.650 --> 00:56:26.070 align:middle line:84% but changing their rates to infinity. 00:56:26.070 --> 00:56:28.350 align:middle line:84% And that means that the robot could try to get to see, 00:56:28.350 --> 00:56:30.840 align:middle line:90% but it wouldn't ever get there. 00:56:30.840 --> 00:56:33.140 align:middle line:84% So what we do next in the algorithm is we're 00:56:33.140 --> 00:56:36.170 align:middle line:84% going to update all of the rhs values associated 00:56:36.170 --> 00:56:38.472 align:middle line:84% with the source nodes of these edges. 00:56:38.472 --> 00:56:40.430 align:middle line:84% And in this case, the edges go both directions, 00:56:40.430 --> 00:56:45.300 align:middle line:84% so all four of these nodes are affected by the infinities. 00:56:45.300 --> 00:56:48.800 align:middle line:84% So let's start with node C. What would be its new rhs value, 00:56:48.800 --> 00:56:54.590 align:middle line:84% given that rhs is the minimum from one of its neighbors to it 00:56:54.590 --> 00:56:58.540 align:middle line:90% of the g plus edge cost? 00:56:58.540 --> 00:56:59.400 align:middle line:90% AUDIENCE: Infinity. 00:56:59.400 --> 00:57:01.941 align:middle line:84% JESS NOSS: Infinity. because all of the edge costs going to C 00:57:01.941 --> 00:57:03.290 align:middle line:90% are infinity. 00:57:03.290 --> 00:57:06.257 align:middle line:90% So what would be the key? 00:57:06.257 --> 00:57:07.840 align:middle line:84% The tricky part here is the first part 00:57:07.840 --> 00:57:09.820 align:middle line:84% of the key is the minimum of g and rhs. 00:57:09.820 --> 00:57:16.184 align:middle line:90% 00:57:16.184 --> 00:57:20.534 align:middle line:84% So the minimum of g or rhs is what? 00:57:20.534 --> 00:57:23.118 align:middle line:90% AUDIENCE: So it'd be 3, 1? 00:57:23.118 --> 00:57:23.986 align:middle line:90% Yeah. 00:57:23.986 --> 00:57:28.940 align:middle line:84% JESS NOSS: Yeah, 3, 1, because the minimum of g and rhs 00:57:28.940 --> 00:57:30.020 align:middle line:90% is g, which is 1. 00:57:30.020 --> 00:57:33.870 align:middle line:84% The heuristic is 1, and the key modifier is 1. 00:57:33.870 --> 00:57:34.370 align:middle line:90% OK. 00:57:34.370 --> 00:57:35.530 align:middle line:90% How about node B? 00:57:35.530 --> 00:57:36.650 align:middle line:90% What's the new rhs value? 00:57:36.650 --> 00:57:40.350 align:middle line:90% 00:57:40.350 --> 00:57:43.077 align:middle line:84% So it has a neighbor here with g as infinity, 00:57:43.077 --> 00:57:45.118 align:middle line:84% a neighbor here of g is 1, except the edge length 00:57:45.118 --> 00:57:46.454 align:middle line:90% is infinity. 00:57:46.454 --> 00:57:48.550 align:middle line:90% And then this guy with g is 3. 00:57:48.550 --> 00:57:52.861 align:middle line:90% 00:57:52.861 --> 00:57:55.740 align:middle line:90% AUDIENCE: So it's infinity, 4? 00:57:55.740 --> 00:57:56.810 align:middle line:90% JESS NOSS: 4. 00:57:56.810 --> 00:57:58.720 align:middle line:84% Yeah, because currently, we still 00:57:58.720 --> 00:58:02.100 align:middle line:84% think that you can get to A and B units. 00:58:02.100 --> 00:58:04.400 align:middle line:84% So this is actually going to be 4. 00:58:04.400 --> 00:58:06.910 align:middle line:84% So this is one of the sillier things that D* does, 00:58:06.910 --> 00:58:08.670 align:middle line:84% but luckily, it doesn't do this very much, 00:58:08.670 --> 00:58:12.750 align:middle line:84% so it doesn't actually really cost too much time. 00:58:12.750 --> 00:58:14.870 align:middle line:90% And then how about D? 00:58:14.870 --> 00:58:16.904 align:middle line:90% What would be the new rhs value? 00:58:16.904 --> 00:58:25.860 align:middle line:90% 00:58:25.860 --> 00:58:27.650 align:middle line:90% AUDIENCE: 5, 3? 00:58:27.650 --> 00:58:29.940 align:middle line:90% rhs. 00:58:29.940 --> 00:58:32.480 align:middle line:84% JESS NOSS: So it has been 3 neighbors. 00:58:32.480 --> 00:58:35.500 align:middle line:84% This edge length is infinity is useless, so that's useless. 00:58:35.500 --> 00:58:38.022 align:middle line:84% This one has g of 0 plus 10 would be 10. 00:58:38.022 --> 00:58:39.230 align:middle line:90% Yes, so this is the best one. 00:58:39.230 --> 00:58:42.490 align:middle line:90% G is 2, plus edge length of 1. 00:58:42.490 --> 00:58:44.285 align:middle line:90% So that would give us 3. 00:58:44.285 --> 00:58:45.510 align:middle line:90% So now what do we dequeue? 00:58:45.510 --> 00:58:50.870 align:middle line:90% 00:58:50.870 --> 00:58:51.460 align:middle line:90% AUDIENCE: C? 00:58:51.460 --> 00:58:54.084 align:middle line:84% JESS NOSS: C. So this is one of those tie breaking cases, where 00:58:54.084 --> 00:58:57.010 align:middle line:84% both of them have the first value is 3, 00:58:57.010 --> 00:58:59.100 align:middle line:84% but C has a better second value in its key, 00:58:59.100 --> 00:59:07.364 align:middle line:84% so we'll dequeue C. Is this over or under consistent? 00:59:07.364 --> 00:59:09.980 align:middle line:84% So the definitions of over and under consistent 00:59:09.980 --> 00:59:20.360 align:middle line:84% that we had earlier, we had if g is greater than rhs, 00:59:20.360 --> 00:59:24.950 align:middle line:84% then that's overconsistent, and we 00:59:24.950 --> 00:59:27.860 align:middle line:84% set g equal to rhs when we're updating g. 00:59:27.860 --> 00:59:30.630 align:middle line:84% So that was the case that we've seen so far. 00:59:30.630 --> 00:59:37.960 align:middle line:84% But then if g is less than rhs, it's underconsistent, 00:59:37.960 --> 00:59:40.180 align:middle line:90% and we set g to infinity. 00:59:40.180 --> 00:59:43.790 align:middle line:90% So what is this? 00:59:43.790 --> 00:59:45.260 align:middle line:90% AUDIENCE: Underconsistent. 00:59:45.260 --> 00:59:46.540 align:middle line:90% JESS NOSS: Underconsistent. 00:59:46.540 --> 00:59:48.127 align:middle line:84% So the reason intuitively why we're 00:59:48.127 --> 00:59:49.930 align:middle line:84% going to set g to infinity is because we're 00:59:49.930 --> 00:59:52.110 align:middle line:84% saying there's some sort of contradiction here. 00:59:52.110 --> 00:59:53.610 align:middle line:90% This one doesn't make any sense. 00:59:53.610 --> 00:59:55.310 align:middle line:84% It's less than the rhs value, so we're 00:59:55.310 --> 00:59:58.760 align:middle line:84% going to just reset g and sort of start over with this node. 00:59:58.760 --> 01:00:00.812 align:middle line:90% So we set that to infinity. 01:00:00.812 --> 01:00:02.270 align:middle line:84% And in this case, we don't actually 01:00:02.270 --> 01:00:03.765 align:middle line:84% need to add the node back onto the queue, 01:00:03.765 --> 01:00:04.931 align:middle line:90% because it's now consistent. 01:00:04.931 --> 01:00:08.730 align:middle line:90% It's now locally consistent. 01:00:08.730 --> 01:00:11.945 align:middle line:84% So next, we'll dequeue the one with the smallest key, which 01:00:11.945 --> 01:00:17.162 align:middle line:84% is B. B is also underconsistent, so we're going to set its g 01:00:17.162 --> 01:00:18.120 align:middle line:90% value back to infinity. 01:00:18.120 --> 01:00:20.980 align:middle line:84% So this is sort of resetting the weird thing we did earlier 01:00:20.980 --> 01:00:23.830 align:middle line:90% that didn't make any sense. 01:00:23.830 --> 01:00:27.830 align:middle line:84% We're going to update its neighbors' rhs values. 01:00:27.830 --> 01:00:30.420 align:middle line:84% So what would be the new rhs value for A, 01:00:30.420 --> 01:00:36.745 align:middle line:84% now that we know that we don't actually have a path B? 01:00:36.745 --> 01:00:37.695 align:middle line:90% AUDIENCE: Infinity? 01:00:37.695 --> 01:00:38.528 align:middle line:90% JESS NOSS: Infinity. 01:00:38.528 --> 01:00:40.550 align:middle line:90% Yeah. 01:00:40.550 --> 01:00:42.770 align:middle line:84% And then, how about rhs value to D? 01:00:42.770 --> 01:00:44.885 align:middle line:84% Because currently, D got its rhs value 01:00:44.885 --> 01:00:53.602 align:middle line:90% by going from A to B to D. 01:00:53.602 --> 01:00:54.600 align:middle line:90% AUDIENCE: 10. 01:00:54.600 --> 01:00:55.260 align:middle line:90% JESS NOSS: 10. 01:00:55.260 --> 01:00:57.950 align:middle line:90% 'Cause now the only way-- 01:00:57.950 --> 01:00:58.710 align:middle line:90% rhs is 10. 01:00:58.710 --> 01:01:03.580 align:middle line:84% The key would have that one also. 01:01:03.580 --> 01:01:05.454 align:middle line:90% Is that right? 01:01:05.454 --> 01:01:06.060 align:middle line:90% Yeah. 01:01:06.060 --> 01:01:08.370 align:middle line:84% Because currently, the best path to D, 01:01:08.370 --> 01:01:11.860 align:middle line:84% and actually, the best path to D you can see 01:01:11.860 --> 01:01:16.936 align:middle line:84% is from G to D, because you can't go through C anymore. 01:01:16.936 --> 01:01:19.310 align:middle line:84% So now, we'll dequeue node A, because it has the smallest 01:01:19.310 --> 01:01:21.130 align:middle line:90% key. 01:01:21.130 --> 01:01:26.020 align:middle line:84% It was also underconsistent, so we would set G to infinity. 01:01:26.020 --> 01:01:29.410 align:middle line:84% So we'll propagate that to its neighbors. 01:01:29.410 --> 01:01:35.510 align:middle line:84% And if we continue doing this, then we find-- 01:01:35.510 --> 01:01:37.980 align:middle line:84% eventually, we dequeue this node, and in this case, 01:01:37.980 --> 01:01:40.470 align:middle line:84% we had to dequeue everything, so now the node's 01:01:40.470 --> 01:01:42.420 align:middle line:90% are locally consistent. 01:01:42.420 --> 01:01:44.920 align:middle line:84% So what's the best path that we've found from B to the goal? 01:01:44.920 --> 01:01:50.860 align:middle line:90% 01:01:50.860 --> 01:01:51.990 align:middle line:90% AUDIENCE: B, D, G? 01:01:51.990 --> 01:01:53.448 align:middle line:84% JESS NOSS: B, D, G. And that's also 01:01:53.448 --> 01:01:55.308 align:middle line:90% the only path in this case. 01:01:55.308 --> 01:01:58.481 align:middle line:84% And we had to update these neighbors. 01:01:58.481 --> 01:01:58.980 align:middle line:90% OK. 01:01:58.980 --> 01:02:00.530 align:middle line:90% So here's our new shortest path. 01:02:00.530 --> 01:02:03.690 align:middle line:84% Now, the robot will move to node D. But now, 01:02:03.690 --> 01:02:04.690 align:middle line:90% what's going to happen-- 01:02:04.690 --> 01:02:06.370 align:middle line:84% I think we updated our heuristics. 01:02:06.370 --> 01:02:08.880 align:middle line:84% But now what happens is it turns out it's a moving obstacle, 01:02:08.880 --> 01:02:10.880 align:middle line:84% and it starts following the robot. 01:02:10.880 --> 01:02:13.230 align:middle line:84% So now the obstacle has moved to there. 01:02:13.230 --> 01:02:15.750 align:middle line:84% So as humans, we can see that, obviously, the shortest 01:02:15.750 --> 01:02:17.580 align:middle line:84% path is through C, but the algorithm 01:02:17.580 --> 01:02:19.949 align:middle line:90% doesn't know this yet. 01:02:19.949 --> 01:02:20.990 align:middle line:90% So what would we do next? 01:02:20.990 --> 01:02:26.990 align:middle line:90% 01:02:26.990 --> 01:02:29.790 align:middle line:84% So we just updated our edge weights. 01:02:29.790 --> 01:02:31.890 align:middle line:84% So now all the edges to B are infinity. 01:02:31.890 --> 01:02:34.720 align:middle line:84% But some of the edges to C are no longer infinity. 01:02:34.720 --> 01:02:39.760 align:middle line:84% So we need to update the rhs values accordingly. 01:02:39.760 --> 01:02:41.970 align:middle line:84% So we'll update all of the rhs values 01:02:41.970 --> 01:02:44.580 align:middle line:84% that were affected by this change. 01:02:44.580 --> 01:02:46.800 align:middle line:84% In this case, G's rhs value never 01:02:46.800 --> 01:02:50.100 align:middle line:84% changes because the distance from G to itself is always 0. 01:02:50.100 --> 01:02:53.810 align:middle line:84% And this one doesn't change because we still 01:02:53.810 --> 01:02:55.070 align:middle line:90% use the G value here. 01:02:55.070 --> 01:02:58.910 align:middle line:90% 01:02:58.910 --> 01:02:59.410 align:middle line:90% OK. 01:02:59.410 --> 01:03:01.451 align:middle line:84% So now again, we can start dequeuing and planning 01:03:01.451 --> 01:03:03.644 align:middle line:84% a new path, so we'll dequeue this node. 01:03:03.644 --> 01:03:06.760 align:middle line:84% We'll propagate to its neighbors. 01:03:06.760 --> 01:03:08.870 align:middle line:90% Dequeue that one. 01:03:08.870 --> 01:03:10.620 align:middle line:90% Update the rhs values. 01:03:10.620 --> 01:03:13.908 align:middle line:90% But in this case, we don't-- 01:03:13.908 --> 01:03:16.685 align:middle line:90% let me go back step. 01:03:16.685 --> 01:03:18.810 align:middle line:84% So we dequeue this node, and we update its G value, 01:03:18.810 --> 01:03:23.910 align:middle line:84% but we don't actually need to continue dequeuing things. 01:03:23.910 --> 01:03:26.960 align:middle line:84% We should-- oh, this rhs value should be updated. 01:03:26.960 --> 01:03:28.730 align:middle line:90% Oh, it is updated. 01:03:28.730 --> 01:03:32.680 align:middle line:90% Why is this rhs infinity? 01:03:32.680 --> 01:03:34.570 align:middle line:84% AUDIENCE: All edges to it are infinity. 01:03:34.570 --> 01:03:34.940 align:middle line:90% JESS NOSS: Yeah. 01:03:34.940 --> 01:03:36.148 align:middle line:90% All edges to it are infinity. 01:03:36.148 --> 01:03:37.405 align:middle line:90% You can't get there. 01:03:37.405 --> 01:03:41.424 align:middle line:84% But we don't actually have to bother expanding B, 01:03:41.424 --> 01:03:43.340 align:middle line:84% because we've already found the shortest path. 01:03:43.340 --> 01:03:46.319 align:middle line:84% So this gives you sort of a hint at when D* might be efficient. 01:03:46.319 --> 01:03:48.360 align:middle line:84% Because even if there were a whole bunch of nodes 01:03:48.360 --> 01:03:50.234 align:middle line:84% over there, I wouldn't need to consider them, 01:03:50.234 --> 01:03:52.360 align:middle line:84% because we found the shortest path. 01:03:52.360 --> 01:03:57.040 align:middle line:84% So now, the robot can move up to C. 01:03:57.040 --> 01:03:59.500 align:middle line:84% And let's say the obstacle chases the robot again. 01:03:59.500 --> 01:04:04.221 align:middle line:90% Do we need to find a new path? 01:04:04.221 --> 01:04:04.720 align:middle line:90% No. 01:04:04.720 --> 01:04:06.886 align:middle line:84% So intuitively, we don't, because the obstacle's not 01:04:06.886 --> 01:04:09.330 align:middle line:84% in the way of our current best path. 01:04:09.330 --> 01:04:11.140 align:middle line:84% But we can also see algorithmically 01:04:11.140 --> 01:04:12.895 align:middle line:90% why that's the case. 01:04:12.895 --> 01:04:15.020 align:middle line:84% So first of all, we'll update all of our rhs values 01:04:15.020 --> 01:04:16.810 align:middle line:84% accordingly, because we've changed 01:04:16.810 --> 01:04:18.120 align:middle line:90% all of the edge weights. 01:04:18.120 --> 01:04:20.788 align:middle line:90% 01:04:20.788 --> 01:04:24.790 align:middle line:84% But the reason algorithmically why we don't actually 01:04:24.790 --> 01:04:27.190 align:middle line:84% need to consider any of these change values 01:04:27.190 --> 01:04:31.180 align:middle line:84% is because this node has the smallest key, which 01:04:31.180 --> 01:04:34.840 align:middle line:84% means that this is the node that has the shortest path to G 01:04:34.840 --> 01:04:36.420 align:middle line:90% anyway. 01:04:36.420 --> 01:04:38.590 align:middle line:84% It is smaller key than all the other nodes 01:04:38.590 --> 01:04:39.810 align:middle line:90% that are on the queue. 01:04:39.810 --> 01:04:41.470 align:middle line:84% So dequeuing things wouldn't actually 01:04:41.470 --> 01:04:44.796 align:middle line:90% help us find the shorter path. 01:04:44.796 --> 01:04:50.008 align:middle line:84% So the robot can move to the goal, and we've succeeded. 01:04:50.008 --> 01:04:51.960 align:middle line:90% Any questions on that? 01:04:51.960 --> 01:04:56.695 align:middle line:90% 01:04:56.695 --> 01:04:57.195 align:middle line:90% OK. 01:04:57.195 --> 01:04:59.840 align:middle line:84% So now then we'll talk about sometimes 01:04:59.840 --> 01:05:02.500 align:middle line:84% when you would want to use incremental path planning, 01:05:02.500 --> 01:05:04.382 align:middle line:90% other than this five node graph. 01:05:04.382 --> 01:05:06.392 align:middle line:90% 01:05:06.392 --> 01:05:07.350 align:middle line:90% ERLEND HARBITZ: Thanks. 01:05:07.350 --> 01:05:11.395 align:middle line:84% So as Jess said, there are sometimes good reasons 01:05:11.395 --> 01:05:12.770 align:middle line:84% to use incremental path planning, 01:05:12.770 --> 01:05:14.353 align:middle line:84% but there are also good reasons not to 01:05:14.353 --> 01:05:15.762 align:middle line:90% in certain circumstances. 01:05:15.762 --> 01:05:17.470 align:middle line:84% In certain circumstances, we can actually 01:05:17.470 --> 01:05:20.180 align:middle line:84% formulate graph problems, where a problem is worse 01:05:20.180 --> 01:05:22.640 align:middle line:90% by using incremental planning. 01:05:22.640 --> 01:05:27.050 align:middle line:84% Remember that I said that D* Lite puts any node on the queue 01:05:27.050 --> 01:05:28.540 align:middle line:90% at most twice. 01:05:28.540 --> 01:05:32.140 align:middle line:84% And so we can compare that to A*, or something like that, 01:05:32.140 --> 01:05:35.660 align:middle line:84% that does full replanning whenever a search occurs. 01:05:35.660 --> 01:05:39.110 align:middle line:84% That pus every node on the search queue at most once. 01:05:39.110 --> 01:05:43.730 align:middle line:84% So D* Lite is most beneficial when putting the nodes 01:05:43.730 --> 01:05:47.116 align:middle line:84% on the queue, potentially more than once, 01:05:47.116 --> 01:05:50.220 align:middle line:84% leads to less nodes being examined overall. 01:05:50.220 --> 01:05:53.300 align:middle line:90% 01:05:53.300 --> 01:05:57.710 align:middle line:84% So here is essentially what I just said. 01:05:57.710 --> 01:06:03.440 align:middle line:84% So A* might perform better for certain problems if we only 01:06:03.440 --> 01:06:08.060 align:middle line:84% have to consider a small amount of nodes anyway. 01:06:08.060 --> 01:06:11.030 align:middle line:84% So putting nodes on the graph twice 01:06:11.030 --> 01:06:13.970 align:middle line:84% would actually lead to more expansions. 01:06:13.970 --> 01:06:19.040 align:middle line:84% And so, to get at an intuition of why that might occur 01:06:19.040 --> 01:06:22.710 align:middle line:84% if changes are close to the start node, 01:06:22.710 --> 01:06:25.130 align:middle line:84% I want to show you an example here. 01:06:25.130 --> 01:06:27.410 align:middle line:84% So here, we're actually showing an example that's 01:06:27.410 --> 01:06:29.340 align:middle line:90% derived from [INAUDIBLE]. 01:06:29.340 --> 01:06:31.832 align:middle line:84% So we're searching forwards here from the start node 01:06:31.832 --> 01:06:32.540 align:middle line:90% to the goal node. 01:06:32.540 --> 01:06:34.560 align:middle line:84% And that's just a different reformulation, 01:06:34.560 --> 01:06:37.730 align:middle line:84% so don't let that confuse you too much. 01:06:37.730 --> 01:06:41.330 align:middle line:84% When we walk forward through A* with a good heuristic, 01:06:41.330 --> 01:06:44.168 align:middle line:84% we might move directly from the start node to goal node, 01:06:44.168 --> 01:06:45.920 align:middle line:90% and find that path. 01:06:45.920 --> 01:06:49.600 align:middle line:84% Now, if a change occurs somewhere upstream 01:06:49.600 --> 01:06:52.520 align:middle line:84% of the start node, when we replanning, 01:06:52.520 --> 01:06:57.320 align:middle line:84% we essentially have to replan the sections that 01:06:57.320 --> 01:07:00.890 align:middle line:84% are beyond the change that's occurred. 01:07:00.890 --> 01:07:05.570 align:middle line:84% So everything that's here, to an extent, it's 01:07:05.570 --> 01:07:08.540 align:middle line:84% relatively insensitive to this change. 01:07:08.540 --> 01:07:11.244 align:middle line:84% It could occur that we would have to take 01:07:11.244 --> 01:07:12.410 align:middle line:90% an entirely different route. 01:07:12.410 --> 01:07:15.110 align:middle line:84% But for the most part, it's likely 01:07:15.110 --> 01:07:18.570 align:middle line:84% that changes are going to couple to our existing path 01:07:18.570 --> 01:07:19.836 align:middle line:90% and so forth. 01:07:19.836 --> 01:07:22.700 align:middle line:84% And so we have an efficient search 01:07:22.700 --> 01:07:27.500 align:middle line:84% that will just go around from the path 01:07:27.500 --> 01:07:31.140 align:middle line:84% that we've already planned to our goal node. 01:07:31.140 --> 01:07:33.200 align:middle line:84% So in this sense, we have an example 01:07:33.200 --> 01:07:38.960 align:middle line:84% where incremental path planning would be very efficient. 01:07:38.960 --> 01:07:41.180 align:middle line:84% In this case, which is a different one, 01:07:41.180 --> 01:07:44.660 align:middle line:84% we introduce an obstacle very near our start node. 01:07:44.660 --> 01:07:49.320 align:middle line:84% This means that most of our existing path is to charge. 01:07:49.320 --> 01:07:55.880 align:middle line:84% So where we start out from our start node to our goal node, 01:07:55.880 --> 01:07:59.160 align:middle line:84% we have to do a lot of recomputation from scratch. 01:07:59.160 --> 01:08:02.007 align:middle line:84% And so an incremental searching algorithm 01:08:02.007 --> 01:08:04.340 align:middle line:84% is going to have to undo all searches that we've already 01:08:04.340 --> 01:08:07.940 align:middle line:84% done, and then search through almost the entirety 01:08:07.940 --> 01:08:09.890 align:middle line:90% of the graph again. 01:08:09.890 --> 01:08:13.960 align:middle line:84% So intuitively, in this case, just restarting with A* from 01:08:13.960 --> 01:08:17.420 align:middle line:84% a blank slate, which we would search directly down 01:08:17.420 --> 01:08:20.770 align:middle line:84% to the goal, instead of doing reparations that would send us 01:08:20.770 --> 01:08:25.660 align:middle line:84% in one path, would be a better idea in this case. 01:08:25.660 --> 01:08:29.250 align:middle line:84% So since we're powering from the start to the goal, 01:08:29.250 --> 01:08:31.490 align:middle line:84% in this example that we're showing, 01:08:31.490 --> 01:08:34.910 align:middle line:84% it's worse to use incremental path planning algorithms 01:08:34.910 --> 01:08:38.720 align:middle line:84% when your changes happen near your start node. 01:08:38.720 --> 01:08:43.399 align:middle line:84% For something like D* Lite, we search back from the goal node 01:08:43.399 --> 01:08:47.130 align:middle line:84% to the start node, it's better if changes occur nearer 01:08:47.130 --> 01:08:50.720 align:middle line:84% to the start node, which for our vehicles that we normally see 01:08:50.720 --> 01:08:53.160 align:middle line:84% that have limited sensor horizons, 01:08:53.160 --> 01:08:55.740 align:middle line:84% is what's going to occur in the most case. 01:08:55.740 --> 01:08:58.455 align:middle line:84% So in those type of situations, incremental path planning 01:08:58.455 --> 01:09:00.842 align:middle line:90% is going to be very efficient. 01:09:00.842 --> 01:09:03.050 align:middle line:84% AUDIENCE: So I understand how this could be a problem 01:09:03.050 --> 01:09:07.122 align:middle line:84% for LPA* and D* Lite over finding a single path, 01:09:07.122 --> 01:09:10.670 align:middle line:84% where you have like a single source or destination 01:09:10.670 --> 01:09:11.740 align:middle line:90% or something like that. 01:09:11.740 --> 01:09:14.300 align:middle line:84% But what if it's incremental, all-pairs shortest-paths, 01:09:14.300 --> 01:09:17.479 align:middle line:90% where you've computed-- 01:09:17.479 --> 01:09:19.750 align:middle line:84% I'd assume you wouldn't experience the same problems 01:09:19.750 --> 01:09:22.490 align:middle line:84% if your algorithm was computing choice paths between all pairs 01:09:22.490 --> 01:09:23.759 align:middle line:90% of nodes indirect. 01:09:23.759 --> 01:09:24.634 align:middle line:90% ERLEND HARBITZ: Yeah. 01:09:24.634 --> 01:09:25.882 align:middle line:90% It already exists. 01:09:25.882 --> 01:09:29.189 align:middle line:90% 01:09:29.189 --> 01:09:30.160 align:middle line:90% Well, that depends. 01:09:30.160 --> 01:09:33.720 align:middle line:90% 01:09:33.720 --> 01:09:39.470 align:middle line:84% When you're redoing recomputation, 01:09:39.470 --> 01:09:41.760 align:middle line:84% between all the nodes in the path, 01:09:41.760 --> 01:09:45.726 align:middle line:84% it depends if our obstacle has changed 01:09:45.726 --> 01:09:47.850 align:middle line:84% [INAUDIBLE] of something there that we to conceive. 01:09:47.850 --> 01:09:49.790 align:middle line:84% And by that I mean imagining this graph 01:09:49.790 --> 01:09:53.540 align:middle line:84% we've computed, D, the paths and path costs, 01:09:53.540 --> 01:09:57.060 align:middle line:84% to get from every single node to every single other node. 01:09:57.060 --> 01:09:59.520 align:middle line:84% Now, if introducing an obstacle here 01:09:59.520 --> 01:10:02.940 align:middle line:84% only changed the path cost to get from our stock node 01:10:02.940 --> 01:10:06.580 align:middle line:84% to our goal node from example, then 01:10:06.580 --> 01:10:10.680 align:middle line:84% replanning that specific pair of paths from start node 01:10:10.680 --> 01:10:12.720 align:middle line:84% to the goal node would use data that 01:10:12.720 --> 01:10:15.000 align:middle line:84% already exists from the other nodes to the goal node. 01:10:15.000 --> 01:10:16.155 align:middle line:90% We already have that. 01:10:16.155 --> 01:10:18.450 align:middle line:84% But if introducing an obstacle would cause changes 01:10:18.450 --> 01:10:20.460 align:middle line:84% throughout the entire graph, then we 01:10:20.460 --> 01:10:23.090 align:middle line:84% can potentially create what would 01:10:23.090 --> 01:10:26.170 align:middle line:84% have to be a very well-connected graph, where again, 01:10:26.170 --> 01:10:28.420 align:middle line:84% incremental path planning would probably perform worse 01:10:28.420 --> 01:10:28.962 align:middle line:90% in that case. 01:10:28.962 --> 01:10:29.586 align:middle line:90% AUDIENCE: Yeah. 01:10:29.586 --> 01:10:32.255 align:middle line:84% So you could still end up in the same situation [INAUDIBLE].. 01:10:32.255 --> 01:10:35.580 align:middle line:90% 01:10:35.580 --> 01:10:37.080 align:middle line:84% ERLEND HARBITZ: Now these algorithms 01:10:37.080 --> 01:10:40.050 align:middle line:84% are interesting, because they allow for some extensions that 01:10:40.050 --> 01:10:45.850 align:middle line:84% allow us to do some some very different things quite easily. 01:10:45.850 --> 01:10:49.080 align:middle line:84% One example is greedy mapping, in terms 01:10:49.080 --> 01:10:51.960 align:middle line:84% of how can we design algorithm using 01:10:51.960 --> 01:10:54.100 align:middle line:84% what we know about incremental path planning, 01:10:54.100 --> 01:10:57.810 align:middle line:84% to hit every single node on our graph. 01:10:57.810 --> 01:11:02.080 align:middle line:84% And what we do is then we introduce a faux goal node, 01:11:02.080 --> 01:11:06.610 align:middle line:84% essentially a node that doesn't exist that we can never reach. 01:11:06.610 --> 01:11:12.570 align:middle line:84% And we add to our initial beliefs, 01:11:12.570 --> 01:11:15.510 align:middle line:84% our initial understanding of the graph 01:11:15.510 --> 01:11:18.670 align:middle line:84% connections between all nodes to this faux goal node. 01:11:18.670 --> 01:11:22.230 align:middle line:84% Now, it's false in the sense that whenever we move our start 01:11:22.230 --> 01:11:25.530 align:middle line:84% node to one of the nodes that we've examined before, 01:11:25.530 --> 01:11:27.930 align:middle line:84% what happens in terms of the changes of the edge 01:11:27.930 --> 01:11:33.415 align:middle line:84% costs is removing that edge that connected that node to the goal 01:11:33.415 --> 01:11:34.200 align:middle line:90% node. 01:11:34.200 --> 01:11:36.090 align:middle line:84% So what this means in principle is 01:11:36.090 --> 01:11:38.470 align:middle line:84% that our vehicle, our poor vehicle, 01:11:38.470 --> 01:11:40.820 align:middle line:90% can never reach the goal node. 01:11:40.820 --> 01:11:42.330 align:middle line:84% Whenever it reaches a node that it 01:11:42.330 --> 01:11:44.700 align:middle line:84% thinks it can get to the goal node from, 01:11:44.700 --> 01:11:45.960 align:middle line:90% that path disappears. 01:11:45.960 --> 01:11:49.680 align:middle line:84% But what that causes it to do is when it's replanning, 01:11:49.680 --> 01:11:53.460 align:middle line:84% it finds a path to a node that we haven't visited before, 01:11:53.460 --> 01:11:56.702 align:middle line:84% because it thinks there is a path that leads from that node 01:11:56.702 --> 01:11:57.410 align:middle line:90% to the goal node. 01:11:57.410 --> 01:11:59.025 align:middle line:84% And so this means that it's efficient, 01:11:59.025 --> 01:12:03.160 align:middle line:84% but replanning a root from his current understanding to a node 01:12:03.160 --> 01:12:04.990 align:middle line:90% that it hasn't examined before. 01:12:04.990 --> 01:12:09.930 align:middle line:84% And so it can apply something like the D* Lite or LPA* with 01:12:09.930 --> 01:12:14.430 align:middle line:84% this formalism in order to search through the graph in its 01:12:14.430 --> 01:12:19.110 align:middle line:84% entirety and make sure that in a greedily efficient manner, 01:12:19.110 --> 01:12:23.590 align:middle line:84% we hit every single node in the graph. 01:12:23.590 --> 01:12:26.700 align:middle line:84% AUDIENCE: Is this [INAUDIBLE] TSP 01:12:26.700 --> 01:12:29.300 align:middle line:84% problem, or the traveling solutions problem? 01:12:29.300 --> 01:12:32.955 align:middle line:84% ERLEND HARBITZ: I'm not very familiar with the TSP problem. 01:12:32.955 --> 01:12:34.320 align:middle line:90% AUDIENCE: You try to-- 01:12:34.320 --> 01:12:37.260 align:middle line:84% and you try to cover all the Cs, and try 01:12:37.260 --> 01:12:39.980 align:middle line:84% to come up with a route that is the most efficient, 01:12:39.980 --> 01:12:43.639 align:middle line:84% and try to visit every known C value once. 01:12:43.639 --> 01:12:44.430 align:middle line:90% ERLEND HARBITZ: OK. 01:12:44.430 --> 01:12:45.388 align:middle line:90% Then in that case, yes. 01:12:45.388 --> 01:12:48.610 align:middle line:84% It's not going to be globally efficient. 01:12:48.610 --> 01:12:50.910 align:middle line:84% As you said, it's going to be an approximation for it. 01:12:50.910 --> 01:12:52.970 align:middle line:84% In a sense, from each node that you visited, 01:12:52.970 --> 01:12:56.030 align:middle line:84% you're searching for the next best choice. 01:12:56.030 --> 01:12:58.540 align:middle line:84% So it's possible you can corner yourself. 01:12:58.540 --> 01:13:01.884 align:middle line:84% But it's a way that we use what we already implemented 01:13:01.884 --> 01:13:03.300 align:middle line:84% and the methods that we've already 01:13:03.300 --> 01:13:06.430 align:middle line:84% established to tackle a different type of problem. 01:13:06.430 --> 01:13:08.960 align:middle line:84% AUDIENCE: And all this [INAUDIBLE] usually if 01:13:08.960 --> 01:13:12.962 align:middle line:84% you have a strategy which is visit the closest unvisited 01:13:12.962 --> 01:13:15.810 align:middle line:84% node, instead of making this whole incremental path 01:13:15.810 --> 01:13:17.130 align:middle line:90% from this study. 01:13:17.130 --> 01:13:21.000 align:middle line:84% If your policy is, always move to the closest unvisited node, 01:13:21.000 --> 01:13:23.753 align:middle line:84% how worse does that get compared to this? 01:13:23.753 --> 01:13:28.850 align:middle line:90% 01:13:28.850 --> 01:13:31.222 align:middle line:84% ERLEND HARBITZ: I think it would behave quite similarly. 01:13:31.222 --> 01:13:32.680 align:middle line:84% What we're just trying to show here 01:13:32.680 --> 01:13:36.560 align:middle line:84% is that we can implement that problem as an extension 01:13:36.560 --> 01:13:37.360 align:middle line:90% if this happened. 01:13:37.360 --> 01:13:40.950 align:middle line:84% It may be an efficient way to do it. 01:13:40.950 --> 01:13:43.960 align:middle line:84% AUDIENCE: [INAUDIBLE] probably like, zigzag. 01:13:43.960 --> 01:13:46.680 align:middle line:84% ERLEND HARBITZ: Also, if you're just 01:13:46.680 --> 01:13:51.060 align:middle line:84% going to the nearest connected node, 01:13:51.060 --> 01:13:55.440 align:middle line:84% if you corner yourself as we showed here, 01:13:55.440 --> 01:13:57.200 align:middle line:84% then you could potentially run into issues 01:13:57.200 --> 01:14:00.180 align:middle line:84% if your algorithm doesn't then do an extended search for it. 01:14:00.180 --> 01:14:03.080 align:middle line:84% If it only considers nodes in its immediate neighborhood 01:14:03.080 --> 01:14:05.470 align:middle line:84% that hasn't visited, once it corners itself 01:14:05.470 --> 01:14:07.209 align:middle line:90% and doesn't know where to go. 01:14:07.209 --> 01:14:09.000 align:middle line:84% AUDIENCE: Just keep going through the graph 01:14:09.000 --> 01:14:11.460 align:middle line:84% until you hit the next inhibited node, 01:14:11.460 --> 01:14:15.400 align:middle line:84% assuming that you know how many invocations you have to do. 01:14:15.400 --> 01:14:18.100 align:middle line:84% AUDIENCE: Sort of like the random walk type approach? 01:14:18.100 --> 01:14:20.683 align:middle line:84% AUDIENCE: Well, I mean, kind of like the greedy version of it. 01:14:20.683 --> 01:14:22.320 align:middle line:84% You kind of start from a node, and then 01:14:22.320 --> 01:14:23.820 align:middle line:84% you keep searching, you'll see, what 01:14:23.820 --> 01:14:27.690 align:middle line:84% is the next unvisited node, and you pick that. 01:14:27.690 --> 01:14:28.700 align:middle line:90% Kind of like-- 01:14:28.700 --> 01:14:32.140 align:middle line:84% [INAUDIBLE] some you see what you have covered. 01:14:32.140 --> 01:14:33.960 align:middle line:84% You see what's the next best move. 01:14:33.960 --> 01:14:35.486 align:middle line:90% You make that move. 01:14:35.486 --> 01:14:36.826 align:middle line:90% And you're done. 01:14:36.826 --> 01:14:39.950 align:middle line:90% 01:14:39.950 --> 01:14:50.230 align:middle line:84% [INAUDIBLE] So you probably don't have this [INAUDIBLE].. 01:14:50.230 --> 01:14:57.587 align:middle line:90% 01:14:57.587 --> 01:14:59.170 align:middle line:84% ERLEND HARBITZ: This basically ensures 01:14:59.170 --> 01:15:00.961 align:middle line:84% you're going to find the shortest path back 01:15:00.961 --> 01:15:02.603 align:middle line:84% to the next unvisited node, I guess. 01:15:02.603 --> 01:15:08.990 align:middle line:90% 01:15:08.990 --> 01:15:10.726 align:middle line:84% We also can use these incremental path 01:15:10.726 --> 01:15:14.540 align:middle line:84% planning algorithms for anytime planners. 01:15:14.540 --> 01:15:16.350 align:middle line:84% And the benefit of an anytime planner 01:15:16.350 --> 01:15:22.730 align:middle line:84% is that we quickly reach a plan which is suboptimal. 01:15:22.730 --> 01:15:26.660 align:middle line:84% And then from there, we are repairing this plan 01:15:26.660 --> 01:15:28.480 align:middle line:90% to approach optimality. 01:15:28.480 --> 01:15:31.430 align:middle line:84% And this is useful in the real world 01:15:31.430 --> 01:15:34.100 align:middle line:84% where you want to get a solution that comes out quickly, 01:15:34.100 --> 01:15:37.050 align:middle line:84% and we want to start along our solution, 01:15:37.050 --> 01:15:38.900 align:middle line:90% and then repair it as we go. 01:15:38.900 --> 01:15:41.390 align:middle line:84% And once we start it, we need o save some more time 01:15:41.390 --> 01:15:44.707 align:middle line:84% if your reparations might lead to small changes and that type 01:15:44.707 --> 01:15:45.206 align:middle line:90% of thing. 01:15:45.206 --> 01:15:47.630 align:middle line:84% And this can be handled quite efficiently 01:15:47.630 --> 01:15:50.450 align:middle line:84% using an incremental path planning. 01:15:50.450 --> 01:15:56.500 align:middle line:84% So what happens is we apply something, idea, called 01:15:56.500 --> 01:15:57.940 align:middle line:90% inflated heuristics. 01:15:57.940 --> 01:16:00.830 align:middle line:84% And so these inflated heuristics are essentially 01:16:00.830 --> 01:16:02.600 align:middle line:84% increasing our heuristic values that 01:16:02.600 --> 01:16:05.150 align:middle line:84% exist on our graph, in our true graph, 01:16:05.150 --> 01:16:07.130 align:middle line:90% by a cost of scale factor. 01:16:07.130 --> 01:16:08.570 align:middle line:84% And this is typically very large, 01:16:08.570 --> 01:16:13.290 align:middle line:84% so differences in heuristics become very large. 01:16:13.290 --> 01:16:17.630 align:middle line:84% This means that once we start along a path, 01:16:17.630 --> 01:16:20.030 align:middle line:90% we continue along it. 01:16:20.030 --> 01:16:23.690 align:middle line:84% And that causes us to very quickly find the first path 01:16:23.690 --> 01:16:25.550 align:middle line:90% that actually works. 01:16:25.550 --> 01:16:28.870 align:middle line:84% Then, we can view reducing those heuristics back 01:16:28.870 --> 01:16:34.460 align:middle line:84% to their original values as essentially a form of changing 01:16:34.460 --> 01:16:37.640 align:middle line:90% our [INAUDIBLE] causes or-- 01:16:37.640 --> 01:16:39.920 align:middle line:90% so it's a form of replanning. 01:16:39.920 --> 01:16:41.510 align:middle line:84% And then by applying the principles 01:16:41.510 --> 01:16:44.900 align:middle line:84% of an incremental planner, we can efficiently use the data 01:16:44.900 --> 01:16:49.090 align:middle line:84% that we already have in order to perform the search 01:16:49.090 --> 01:16:53.640 align:middle line:84% and repair our solution as we go. 01:16:53.640 --> 01:16:56.240 align:middle line:84% Finally, we want to make explicit to you 01:16:56.240 --> 01:17:00.470 align:middle line:84% guys how we can actually apply these algorithms 01:17:00.470 --> 01:17:02.930 align:middle line:90% to mobile robotics. 01:17:02.930 --> 01:17:05.300 align:middle line:84% Because it may not immediately clear how 01:17:05.300 --> 01:17:09.680 align:middle line:84% we go from a full space to a graph. 01:17:09.680 --> 01:17:12.760 align:middle line:84% Joe on this kind of briefly when he talked about we 01:17:12.760 --> 01:17:13.884 align:middle line:90% discretize the world. 01:17:13.884 --> 01:17:15.800 align:middle line:84% But we want to give you guys some ideas of how 01:17:15.800 --> 01:17:18.500 align:middle line:90% we might do this cleverly. 01:17:18.500 --> 01:17:24.050 align:middle line:84% So in order to build a graph system out of our world state, 01:17:24.050 --> 01:17:26.390 align:middle line:84% we differentiate between holonomic systems 01:17:26.390 --> 01:17:29.630 align:middle line:84% and nonholonomic systems, which their difference is just 01:17:29.630 --> 01:17:32.440 align:middle line:84% in terms of whether your constraints are differential 01:17:32.440 --> 01:17:35.412 align:middle line:90% in nature or not. 01:17:35.412 --> 01:17:37.370 align:middle line:84% Then we are going to outline three methods here 01:17:37.370 --> 01:17:39.700 align:middle line:84% very briefly, cell decomposition, 01:17:39.700 --> 01:17:42.500 align:middle line:84% using a visibility graph, and sampling-based construction 01:17:42.500 --> 01:17:44.280 align:middle line:90% methods. 01:17:44.280 --> 01:17:46.970 align:middle line:84% So for cell decomposition methods, 01:17:46.970 --> 01:17:51.350 align:middle line:84% we might to do what you would expect, take the free space 01:17:51.350 --> 01:17:54.655 align:middle line:84% in this region and split it up into various different cells. 01:17:54.655 --> 01:17:56.530 align:middle line:84% And each of those cells is going to represent 01:17:56.530 --> 01:17:58.910 align:middle line:90% a node in our graph. 01:17:58.910 --> 01:18:02.630 align:middle line:84% And how we can do this is we split the region up 01:18:02.630 --> 01:18:06.680 align:middle line:84% based on each of the vertices of obstacles. 01:18:06.680 --> 01:18:13.940 align:middle line:84% So we draw a line vertically from each of our vertices 01:18:13.940 --> 01:18:17.060 align:middle line:84% that goes in the direction away from the obstacle. 01:18:17.060 --> 01:18:20.560 align:middle line:84% So at the top here, it's going up and out of the obstacle. 01:18:20.560 --> 01:18:25.351 align:middle line:84% But in the lower left corner there, both up and down, 01:18:25.351 --> 01:18:26.600 align:middle line:90% don't intercept the obstacles. 01:18:26.600 --> 01:18:28.017 align:middle line:90% We draw them in its entirely. 01:18:28.017 --> 01:18:29.975 align:middle line:84% So we can continue this throughout the entirety 01:18:29.975 --> 01:18:32.420 align:middle line:84% of the state space, and then we can 01:18:32.420 --> 01:18:35.490 align:middle line:84% attribute to each essential cell that we've 01:18:35.490 --> 01:18:38.920 align:middle line:84% defined through these regions, a single point that represents 01:18:38.920 --> 01:18:44.090 align:middle line:84% the midpoint of that cell, which is going to be our graph 01:18:44.090 --> 01:18:47.360 align:middle line:84% node, because we attribute a single set of coordinates 01:18:47.360 --> 01:18:50.370 align:middle line:90% to each of our graph nodes. 01:18:50.370 --> 01:18:52.730 align:middle line:84% And we also, to make sure that our transitions 01:18:52.730 --> 01:18:55.790 align:middle line:84% between these cells are consistent, 01:18:55.790 --> 01:18:59.660 align:middle line:84% additional nodes at the midpoint of each of these lines 01:18:59.660 --> 01:19:03.110 align:middle line:84% that we've drawn, where we make the distinction that from here, 01:19:03.110 --> 01:19:04.680 align:middle line:90% we've actually drawn two lines. 01:19:04.680 --> 01:19:07.580 align:middle line:84% So we draw the midpoint between all of those. 01:19:07.580 --> 01:19:09.575 align:middle line:84% This allows us to essentially create a graph 01:19:09.575 --> 01:19:15.230 align:middle line:84% through the entire system, and we connect each node 01:19:15.230 --> 01:19:18.145 align:middle line:84% at the center of any given cell to nodes 01:19:18.145 --> 01:19:21.530 align:middle line:84% that are on the boundary of any given cell. 01:19:21.530 --> 01:19:25.000 align:middle line:84% This allows us to create a full graph through the system, 01:19:25.000 --> 01:19:26.810 align:middle line:84% that we can move through, guaranteeing 01:19:26.810 --> 01:19:30.760 align:middle line:84% that none of these paths, if we follow them exactly 01:19:30.760 --> 01:19:33.439 align:middle line:90% collide with the obstacles. 01:19:33.439 --> 01:19:35.730 align:middle line:84% Now, this works in environments where the obstacles are 01:19:35.730 --> 01:19:39.460 align:middle line:84% 2D polygons, and the path is far from optimal, as we can see, 01:19:39.460 --> 01:19:42.285 align:middle line:84% but we can plan an optimal path over the graph 01:19:42.285 --> 01:19:44.810 align:middle line:90% that we have defined. 01:19:44.810 --> 01:19:48.300 align:middle line:84% And this only works on holonomic systems. 01:19:48.300 --> 01:19:50.675 align:middle line:84% We can also define what is called a visibility 01:19:50.675 --> 01:19:54.050 align:middle line:84% graph, where we essentially draw a boundary between both 01:19:54.050 --> 01:19:56.210 align:middle line:84% our vehicle down there at that bottom left, 01:19:56.210 --> 01:19:58.470 align:middle line:84% and boundaries around our obstacles. 01:19:58.470 --> 01:20:00.770 align:middle line:84% These boundaries are sufficiently sized 01:20:00.770 --> 01:20:06.500 align:middle line:84% so that if the center of the vehicle that we're defining 01:20:06.500 --> 01:20:10.130 align:middle line:84% is along the extended edge of the obstacle, 01:20:10.130 --> 01:20:15.090 align:middle line:84% that the vehicle will not collide with that obstacle. 01:20:15.090 --> 01:20:19.890 align:middle line:84% Then for each of the vertices of the extended boundary 01:20:19.890 --> 01:20:23.780 align:middle line:84% that we've defined, we add in a new node, 01:20:23.780 --> 01:20:27.260 align:middle line:84% and we draw all lines between all nodes, which 01:20:27.260 --> 01:20:29.830 align:middle line:84% can be connected without intersecting 01:20:29.830 --> 01:20:33.920 align:middle line:84% the extended obstacles, which we've drawn here. 01:20:33.920 --> 01:20:39.710 align:middle line:84% And this leads to a graph that we can, again, plan over. 01:20:39.710 --> 01:20:41.520 align:middle line:84% This, again, works in environments where 01:20:41.520 --> 01:20:42.810 align:middle line:90% the obstacles are 2D polygons. 01:20:42.810 --> 01:20:44.930 align:middle line:84% But what's nice about this system is 01:20:44.930 --> 01:20:47.200 align:middle line:84% that we do get an optimal path, provided 01:20:47.200 --> 01:20:50.220 align:middle line:84% that we assume that we assume that our extension 01:20:50.220 --> 01:20:56.735 align:middle line:84% of the robot itself is valid, or doesn't act too much-- 01:20:56.735 --> 01:20:59.280 align:middle line:90% 01:20:59.280 --> 01:21:03.560 align:middle line:84% isn't too excessive for constraint. 01:21:03.560 --> 01:21:07.970 align:middle line:84% Finally, going back to some of the random sampling methods 01:21:07.970 --> 01:21:10.830 align:middle line:84% that we mentioned at the very beginning of this lecture, 01:21:10.830 --> 01:21:14.690 align:middle line:84% we've introduced sampling-based roadmap construction, wherein 01:21:14.690 --> 01:21:18.440 align:middle line:84% we choose nodes randomly or deterministically 01:21:18.440 --> 01:21:19.685 align:middle line:90% throughout our environment. 01:21:19.685 --> 01:21:21.830 align:middle line:84% The difference here, though, is that we then 01:21:21.830 --> 01:21:23.850 align:middle line:84% create a graph out of this system, 01:21:23.850 --> 01:21:26.610 align:middle line:84% and that we reason over that graph using something 01:21:26.610 --> 01:21:29.921 align:middle line:84% like an incremental planning algorithm. 01:21:29.921 --> 01:21:33.680 align:middle line:84% And so when we create that graph, after we've sampled, 01:21:33.680 --> 01:21:37.160 align:middle line:84% we can use the [INAUDIBLE] neighbors of each node, 01:21:37.160 --> 01:21:41.620 align:middle line:84% or we can connect each node to every node 01:21:41.620 --> 01:21:46.650 align:middle line:84% within a certain radius, which is what we've drawn here. 01:21:46.650 --> 01:21:50.880 align:middle line:84% Finally, in order to actually move between nodes, 01:21:50.880 --> 01:21:54.060 align:middle line:84% we need to have some knowledge about the dynamics 01:21:54.060 --> 01:21:56.460 align:middle line:84% of the vehicle, which is what we're discussing here. 01:21:56.460 --> 01:22:00.660 align:middle line:84% And one method that we can do this for nonholonomic systems 01:22:00.660 --> 01:22:05.750 align:middle line:84% is we can distinguish between different paths that 01:22:05.750 --> 01:22:07.670 align:middle line:84% use turning right, turning left, and traveling 01:22:07.670 --> 01:22:09.260 align:middle line:90% in straight lines. 01:22:09.260 --> 01:22:11.720 align:middle line:84% It turns out that the shortest path between two points 01:22:11.720 --> 01:22:16.160 align:middle line:84% can be defined using one of these systems. 01:22:16.160 --> 01:22:19.510 align:middle line:84% And so we just have to search over a finite number of options 01:22:19.510 --> 01:22:23.390 align:middle line:84% in order to find the best path that can connect any two nodes. 01:22:23.390 --> 01:22:26.720 align:middle line:90% 01:22:26.720 --> 01:22:29.450 align:middle line:84% Hopefully, that has given you some intuition 01:22:29.450 --> 01:22:34.490 align:middle line:84% into how we can apply these incremental path planning 01:22:34.490 --> 01:22:35.910 align:middle line:90% [INAUDIBLE] systems. 01:22:35.910 --> 01:22:39.810 align:middle line:90% Are there any questions? 01:22:39.810 --> 01:22:43.460 align:middle line:90% [APPLAUSE] 01:22:43.460 --> 01:22:45.888 align:middle line:90%