1
00:00:01,580 --> 00:00:03,920
The following content is
provided under a Creative

2
00:00:03,920 --> 00:00:05,340
Commons License.

3
00:00:05,340 --> 00:00:07,550
Your support will help
MIT OpenCourseWare

4
00:00:07,550 --> 00:00:11,640
continue to offer high quality
educational resources for free.

5
00:00:11,640 --> 00:00:14,180
To make a donation or to
view additional materials

6
00:00:14,180 --> 00:00:18,110
from hundreds of MIT courses,
visit MIT OpenCourseWare

7
00:00:18,110 --> 00:00:19,080
at ocw.mit.edu.

8
00:00:22,414 --> 00:00:23,580
JOE LEAVITT: Good afternoon.

9
00:00:23,580 --> 00:00:24,770
My name is Joe Leavitt.

10
00:00:24,770 --> 00:00:27,390
My group members are Ben Ayton,
Jess Noss, Erlend Harbitz,

11
00:00:27,390 --> 00:00:29,870
Jake Barnwell, and
Sam Pinto, and we're

12
00:00:29,870 --> 00:00:32,290
going to introduce you to
the topic incremental path

13
00:00:32,290 --> 00:00:34,850
planning.

14
00:00:34,850 --> 00:00:37,160
Listed on screen are just
some of the references

15
00:00:37,160 --> 00:00:39,620
we used in putting
together this lecture.

16
00:00:39,620 --> 00:00:42,332
The top three are the
main references we used

17
00:00:42,332 --> 00:00:46,320
in developing our material
on the D* Lite algorithm,

18
00:00:46,320 --> 00:00:48,980
but also take note of
the bottom reference,

19
00:00:48,980 --> 00:00:52,530
which is a paper done within
Professor William's group here

20
00:00:52,530 --> 00:00:53,180
at MIT.

21
00:00:53,180 --> 00:00:56,180
It talks about
all-pairs, shortest paths

22
00:00:56,180 --> 00:00:57,797
incremental search method.

23
00:00:57,797 --> 00:00:59,880
So if you're interested
in more incremental search

24
00:00:59,880 --> 00:01:01,740
methods than those we're
going to talk about today,

25
00:01:01,740 --> 00:01:02,614
please get into this.

26
00:01:02,614 --> 00:01:05,540
Especially if you're going
to go ahead and do our P set,

27
00:01:05,540 --> 00:01:10,570
these references will
be very helpful to you.

28
00:01:10,570 --> 00:01:13,719
So today, we're going
to first Monday problem,

29
00:01:13,719 --> 00:01:16,260
then we're going to introduce
the idea of incremental search.

30
00:01:16,260 --> 00:01:18,480
Then we're going to go
into detail on the D* Lite

31
00:01:18,480 --> 00:01:21,285
algorithm, which is type of
incremental path planning,

32
00:01:21,285 --> 00:01:23,790
an algorithm used for
incremental path planning,

33
00:01:23,790 --> 00:01:25,540
run through an
example of D* Lite,

34
00:01:25,540 --> 00:01:28,600
talk about when it is good to
use incremental path planning

35
00:01:28,600 --> 00:01:32,152
when other path planners
might be better,

36
00:01:32,152 --> 00:01:34,110
and then I'll go into
some algorithm extensions

37
00:01:34,110 --> 00:01:36,600
and related topics, and
some applications in mobile

38
00:01:36,600 --> 00:01:38,250
robotics.

39
00:01:38,250 --> 00:01:40,480
So in order to
motivate the problem,

40
00:01:40,480 --> 00:01:43,900
we're going to use a robot
living in a grid world.

41
00:01:43,900 --> 00:01:47,640
It's at a start location trying
to get to a goal location.

42
00:01:47,640 --> 00:01:50,520
It can move up, down, left,
or right, or diagonally.

43
00:01:50,520 --> 00:01:54,130
And it can only see the
eight cells around it,

44
00:01:54,130 --> 00:01:56,010
as it's moving through
this grid world.

45
00:01:56,010 --> 00:01:59,100
And we're going to give some
pretty fine map of the world.

46
00:01:59,100 --> 00:02:02,270
This is a map that has the
world before it starts moving.

47
00:02:02,270 --> 00:02:05,520
And it's possibly moving
through some of these,

48
00:02:05,520 --> 00:02:08,650
new obstacles could appear,
or obstacles could go away.

49
00:02:08,650 --> 00:02:11,614
And the idea is that it's
going to make observations

50
00:02:11,614 --> 00:02:13,655
as it's moving to this
world and update its plane

51
00:02:13,655 --> 00:02:17,400
to get to the goal based on what
it sees as it's moving along.

52
00:02:20,750 --> 00:02:23,822
So I must correct myself.

53
00:02:23,822 --> 00:02:25,530
The map that you're
seeing now is the map

54
00:02:25,530 --> 00:02:26,904
that the robot
has ahead of time.

55
00:02:26,904 --> 00:02:29,180
The previous map is what
the environment actually

56
00:02:29,180 --> 00:02:30,510
looks like.

57
00:02:30,510 --> 00:02:32,420
So as it starts
moving, it's going

58
00:02:32,420 --> 00:02:34,750
to make a plan to get from
the start to the goal,

59
00:02:34,750 --> 00:02:36,470
and that's what
we want it to do.

60
00:02:36,470 --> 00:02:38,262
And it's going to move
to the next position

61
00:02:38,262 --> 00:02:40,470
and then realize that there's
an obstacle in the path

62
00:02:40,470 --> 00:02:42,110
that it previously
planned, and now it

63
00:02:42,110 --> 00:02:47,250
must replan in order to find
a valid path to the goal.

64
00:02:47,250 --> 00:02:49,360
And so now, it
replans, and continues

65
00:02:49,360 --> 00:02:52,130
to move on the new plan, and
now it finds another obstacle

66
00:02:52,130 --> 00:02:54,810
that it has to replan around,
and it does the same thing.

67
00:02:54,810 --> 00:02:56,990
And then it continues
to walk through

68
00:02:56,990 --> 00:02:59,660
until it gets to a
point where we have

69
00:02:59,660 --> 00:03:01,140
a change in the environment.

70
00:03:01,140 --> 00:03:03,740
The change is represented
by these two cells

71
00:03:03,740 --> 00:03:06,200
going black here, and
now its original plane

72
00:03:06,200 --> 00:03:07,420
is no longer valid again.

73
00:03:07,420 --> 00:03:09,003
And what we want it
to do is to replan

74
00:03:09,003 --> 00:03:13,580
to find a new valid
optimal path to the goal.

75
00:03:13,580 --> 00:03:17,190
This is just showing here what
happened to our environment.

76
00:03:17,190 --> 00:03:21,740
And then the robot
replans and continues

77
00:03:21,740 --> 00:03:23,477
to march towards
the goal So this

78
00:03:23,477 --> 00:03:25,310
is the behavior we want
from a mobile robot,

79
00:03:25,310 --> 00:03:27,518
whether it's a two-dimensional
path-planning problem,

80
00:03:27,518 --> 00:03:32,160
or we're talking about
multi-dimensional robot arms,

81
00:03:32,160 --> 00:03:35,510
planning, working with
humans on a manufacturing

82
00:03:35,510 --> 00:03:39,124
environment, or an
activity-planning process.

83
00:03:39,124 --> 00:03:40,540
The idea is that
we want the robot

84
00:03:40,540 --> 00:03:42,890
to be able to account for
new obstacles or changes

85
00:03:42,890 --> 00:03:45,280
in its representation
of the environment

86
00:03:45,280 --> 00:03:50,150
and continue to plan on the
fly quickly and optimally.

87
00:03:50,150 --> 00:03:53,510
So there's methods
that accomplish this.

88
00:03:53,510 --> 00:03:56,330
One method used to
in path planning

89
00:03:56,330 --> 00:04:00,620
is rapidly exploring
random trees, or RRTs.

90
00:04:00,620 --> 00:04:03,930
RRTs are basically
going to plan, replan,

91
00:04:03,930 --> 00:04:06,680
on every time step based on
changes in the environment.

92
00:04:06,680 --> 00:04:09,420
And one problem in
my experience here,

93
00:04:09,420 --> 00:04:13,850
though, is that RRTs are
suboptimal, even though fast.

94
00:04:22,198 --> 00:04:24,830
See, here you see the robot
has come up with a plan.

95
00:04:24,830 --> 00:04:27,080
It's seeing new obstacles,
the table in front of it.

96
00:04:27,080 --> 00:04:27,705
The coffee cup.

97
00:04:27,705 --> 00:04:30,771
And its planning for those
things, so the plan it came up

98
00:04:30,771 --> 00:04:33,541
with is clearly not optimal.

99
00:04:38,730 --> 00:04:41,400
So we can perform some
additional computation.

100
00:04:41,400 --> 00:04:45,360
We can use a method that
builds upon the RRT, RRT*,

101
00:04:45,360 --> 00:04:47,920
that it finds an optimal path,
but that requires a pretty

102
00:04:47,920 --> 00:04:50,360
significant amount of
offline computation time,

103
00:04:50,360 --> 00:04:53,190
and then you can accomplish
something that looks a little

104
00:04:53,190 --> 00:04:54,900
more normal.

105
00:04:54,900 --> 00:04:58,154
Again, we have to
allow the robot

106
00:04:58,154 --> 00:05:00,549
to perform a lot of the
computation off-line.

107
00:05:04,390 --> 00:05:08,820
So how do we accomplish
what we want to?

108
00:05:08,820 --> 00:05:10,990
We want to compute quickly.

109
00:05:10,990 --> 00:05:12,700
We want to compute
optimally, and then

110
00:05:12,700 --> 00:05:14,560
have the robot continue
to move as it's

111
00:05:14,560 --> 00:05:17,890
received new information
coming to the environment.

112
00:05:17,890 --> 00:05:19,660
Some problems we're
facing, there's

113
00:05:19,660 --> 00:05:21,266
changing environmental
conditions.

114
00:05:21,266 --> 00:05:22,390
There's sensor limitations.

115
00:05:22,390 --> 00:05:23,765
You might not be able
to see everything

116
00:05:23,765 --> 00:05:25,570
in the environment
around you, and then we

117
00:05:25,570 --> 00:05:27,005
have limited computation time.

118
00:05:29,860 --> 00:05:32,090
And the solution that
we're going to explore here

119
00:05:32,090 --> 00:05:35,510
is to reuse that from
the previous search,

120
00:05:35,510 --> 00:05:37,580
in order to speed up
the search process,

121
00:05:37,580 --> 00:05:40,516
and continue to plan optimally.

122
00:05:40,516 --> 00:05:42,710
And the method we're going
to use to accomplish this

123
00:05:42,710 --> 00:05:44,487
is incremental search.

124
00:05:44,487 --> 00:05:46,070
So now, we're going
to talk about what

125
00:05:46,070 --> 00:05:49,170
incremental search means.

126
00:05:49,170 --> 00:05:52,370
First to show you kind of how
powerful incremental search can

127
00:05:52,370 --> 00:05:55,290
be, this is one example of
an incremental search method,

128
00:05:55,290 --> 00:05:57,030
incremental all-pairs
shortest paths,

129
00:05:57,030 --> 00:06:01,190
which allows you to online
compute paths between any start

130
00:06:01,190 --> 00:06:03,042
and goal location.

131
00:06:03,042 --> 00:06:05,000
The idea is that you have
a robot that's trying

132
00:06:05,000 --> 00:06:06,590
to move around one obstacle.

133
00:06:06,590 --> 00:06:09,380
That's what you see in the
upper sequence of events there.

134
00:06:09,380 --> 00:06:11,240
And another obstacle
is added, and now it

135
00:06:11,240 --> 00:06:13,120
has to plan around
this new obstacle.

136
00:06:13,120 --> 00:06:16,220
In using this algorithm, you
have a significant increase

137
00:06:16,220 --> 00:06:18,950
in performance and
computation time

138
00:06:18,950 --> 00:06:22,550
using the all-pairs shortest
paths versus an RRT.

139
00:06:22,550 --> 00:06:24,380
RRT can act as another
version of an RRT,

140
00:06:24,380 --> 00:06:26,880
where you're planning basically
from both the goal and start

141
00:06:26,880 --> 00:06:30,080
and connecting your paths.

142
00:06:30,080 --> 00:06:33,230
It's on the order of
what you get with RRT,

143
00:06:33,230 --> 00:06:37,890
but this RRT connect
algorithm doesn't give you

144
00:06:37,890 --> 00:06:39,740
an optimal result.
And then it's even

145
00:06:39,740 --> 00:06:41,987
fast than another
algorithm probablistic road

146
00:06:41,987 --> 00:06:44,320
mapping, where you're randomly
sampling the environment,

147
00:06:44,320 --> 00:06:48,380
and connecting points
to produce a path.

148
00:06:48,380 --> 00:06:50,540
So the idea here is
that we can really

149
00:06:50,540 --> 00:06:53,810
leverage this idea of reusing
results from previous searches

150
00:06:53,810 --> 00:06:57,005
to accomplish fast
replanning online.

151
00:06:59,956 --> 00:07:01,580
So the whole idea of
incremental search

152
00:07:01,580 --> 00:07:05,290
is that we're going to
form a normal graph search,

153
00:07:05,290 --> 00:07:08,150
just like anybody's used to.

154
00:07:08,150 --> 00:07:11,620
And then we're going to
repeat as the robot moves

155
00:07:11,620 --> 00:07:14,610
through the environment, or
you're executing your plan,

156
00:07:14,610 --> 00:07:16,830
you're going to execute
the next point in the plan,

157
00:07:16,830 --> 00:07:19,540
or the next action in the plan.

158
00:07:19,540 --> 00:07:23,130
We're going to receive
changes from the environment,

159
00:07:23,130 --> 00:07:27,470
either through our sensors or
information from the outside.

160
00:07:27,470 --> 00:07:29,510
And then we're going
to use that information

161
00:07:29,510 --> 00:07:32,320
to update our previous search
results instead of completely

162
00:07:32,320 --> 00:07:36,190
replanning, and then
continue to form that loop,

163
00:07:36,190 --> 00:07:38,950
until you reach your goal.

164
00:07:38,950 --> 00:07:41,650
So before we get into too many
of the details of how we're

165
00:07:41,650 --> 00:07:43,130
going to accomplish
this, first, we

166
00:07:43,130 --> 00:07:44,963
want to review the
graph search problem.

167
00:07:44,963 --> 00:07:49,300
So in graph search, you
have a graph consisting

168
00:07:49,300 --> 00:07:51,030
of vertices or nodes.

169
00:07:51,030 --> 00:07:53,780
We're going to use nodes
throughout this lecture

170
00:07:53,780 --> 00:07:57,670
to talk about the vertices
or nodes in the graph that

171
00:07:57,670 --> 00:07:59,530
are connected by edges.

172
00:07:59,530 --> 00:08:01,510
And then, there's an
edge weighting function

173
00:08:01,510 --> 00:08:04,800
that describes the cost going
from one node to another.

174
00:08:04,800 --> 00:08:07,930
And then, a heuristic
function is an estimate

175
00:08:07,930 --> 00:08:11,576
of the cost to get from a
current node to the goal node.

176
00:08:11,576 --> 00:08:13,450
And then we a start
vertex and a gold vertex.

177
00:08:18,108 --> 00:08:19,570
In the graph search
problem, we're

178
00:08:19,570 --> 00:08:23,310
also going to have this idea
of a g value, or a cost so far.

179
00:08:23,310 --> 00:08:26,500
We're going to use term g
value, which is essentially

180
00:08:26,500 --> 00:08:29,070
the cost that it's taken to
get to some predecessor node,

181
00:08:29,070 --> 00:08:32,200
plus the cost of the node that
you're actually expanding.

182
00:08:32,200 --> 00:08:37,059
So you would have this
W start to S1 cost

183
00:08:37,059 --> 00:08:38,549
to get from start to S1.

184
00:08:38,549 --> 00:08:40,570
And then the g value
for S goal would

185
00:08:40,570 --> 00:08:46,300
be the weight from S1 of the
goal plus the S1's g value.

186
00:08:46,300 --> 00:08:46,940
Yes, go ahead.

187
00:08:46,940 --> 00:08:48,025
AUDIENCE: What is W?

188
00:08:48,025 --> 00:08:50,140
JOE LEAVITT: W is the
weighting function

189
00:08:50,140 --> 00:08:51,640
that I talked about
previously here.

190
00:08:51,640 --> 00:08:54,280
So the weighting function
describes the cost

191
00:08:54,280 --> 00:09:00,510
to get from one node to another
along any edge in a graph.

192
00:09:00,510 --> 00:09:02,260
And then we'd use
those waiting functions

193
00:09:02,260 --> 00:09:05,410
to develop the idea
of the G value.

194
00:09:05,410 --> 00:09:10,840
So W is just the edge cost.

195
00:09:10,840 --> 00:09:15,740
And then our
heuristic value, like,

196
00:09:15,740 --> 00:09:17,990
we use our heuristic function
to establish a heuristic

197
00:09:17,990 --> 00:09:20,120
value from one goal to
another, and we're going

198
00:09:20,120 --> 00:09:22,540
to call that our h value, or h.

199
00:09:22,540 --> 00:09:25,241
And then the total estimated
cost to get from a node

200
00:09:25,241 --> 00:09:27,240
that we're currently
expanding in a graph search

201
00:09:27,240 --> 00:09:34,556
is f, which is your cost so far,
plus your estimated cost to go.

202
00:09:34,556 --> 00:09:36,430
I think I might have
messed that up in there.

203
00:09:36,430 --> 00:09:38,894
Call g value associated
cost so far, not cost to go,

204
00:09:38,894 --> 00:09:40,560
and then h is an
estimate of cost to go.

205
00:09:44,730 --> 00:09:48,087
So in order to reason about
a real-world environment,

206
00:09:48,087 --> 00:09:49,670
we first have to
take that environment

207
00:09:49,670 --> 00:09:51,961
and convert it to a graph,
so we can form graph search,

208
00:09:51,961 --> 00:09:54,330
and then do everything
we're talking about.

209
00:09:54,330 --> 00:09:55,650
Some of the examples,
we're going to talk about,

210
00:09:55,650 --> 00:09:57,650
and some of the ones we've
already talked about,

211
00:09:57,650 --> 00:09:59,510
we're using this
notion of a grid world.

212
00:09:59,510 --> 00:10:02,440
You can have a four
connected graph

213
00:10:02,440 --> 00:10:05,670
or a grid, where a robot can
move up, down, left, or right,

214
00:10:05,670 --> 00:10:08,540
basically in x or y directions,
or an eight connected graph,

215
00:10:08,540 --> 00:10:09,920
where you can also move
along the diagonals.

216
00:10:09,920 --> 00:10:11,370
And then had here,
we just showed

217
00:10:11,370 --> 00:10:13,860
the graphs associated
with those, with the nodes

218
00:10:13,860 --> 00:10:16,120
and edges.

219
00:10:16,120 --> 00:10:19,510
And then you would have an
associated admissible heuristic

220
00:10:19,510 --> 00:10:23,650
with those if you're going
to form a heuristic search.

221
00:10:23,650 --> 00:10:26,450
So just know up front
that any time we're

222
00:10:26,450 --> 00:10:28,239
moving from a real
world environment,

223
00:10:28,239 --> 00:10:30,530
that we're trying to reason
about these search methods,

224
00:10:30,530 --> 00:10:34,690
we have to develop
an associated graph.

225
00:10:34,690 --> 00:10:37,297
And one more thing that
is important as far

226
00:10:37,297 --> 00:10:38,880
as understanding
incremental searches,

227
00:10:38,880 --> 00:10:42,360
is the notion or
idea of relaxation.

228
00:10:42,360 --> 00:10:45,320
We're going use
Dijkstra's algorithm.

229
00:10:45,320 --> 00:10:47,445
Everybody here familiar
with Dijkstra's algorithm,

230
00:10:47,445 --> 00:10:49,550
I'm assuming, in the past.

231
00:10:49,550 --> 00:10:51,176
We're going to use
Dijkstra's algorithm

232
00:10:51,176 --> 00:10:53,008
to kind of illustrate
the idea of relaxation

233
00:10:53,008 --> 00:10:55,380
and remind everybody of what
that means, because it's

234
00:10:55,380 --> 00:10:57,190
an important concept
when we're talking

235
00:10:57,190 --> 00:10:58,972
about incremental
search algorithms.

236
00:10:58,972 --> 00:11:01,130
So essentially, in
Dijkstra's algorithm,

237
00:11:01,130 --> 00:11:03,980
it's a single source, shortest
path algorithm, a best

238
00:11:03,980 --> 00:11:04,800
first search.

239
00:11:04,800 --> 00:11:07,050
So we're going to make eight
expanding nodes according

240
00:11:07,050 --> 00:11:10,830
to their best g
value, or cost so far.

241
00:11:10,830 --> 00:11:13,170
And we have our start
node initialized

242
00:11:13,170 --> 00:11:17,670
to zero cost so far,
because at your start,

243
00:11:17,670 --> 00:11:18,727
haven't accrued any cost.

244
00:11:18,727 --> 00:11:20,185
And then, every
other node is going

245
00:11:20,185 --> 00:11:21,731
to be initialized to infinity.

246
00:11:21,731 --> 00:11:23,730
And the idea is we're
going to find the shortest

247
00:11:23,730 --> 00:11:25,190
distance to all
of the other nodes

248
00:11:25,190 --> 00:11:30,810
by relaxing the cost to
those nodes from infinity

249
00:11:30,810 --> 00:11:35,610
down to their actual shortest
or lowest cost value.

250
00:11:35,610 --> 00:11:38,770
So we start by expanding
the start node,

251
00:11:38,770 --> 00:11:41,560
and then you find
the cost so far

252
00:11:41,560 --> 00:11:44,640
to each of the children
nodes of the start node.

253
00:11:44,640 --> 00:11:48,260
So you have an edge way
to one, and so the node

254
00:11:48,260 --> 00:11:50,170
on the bottom as a value of 1.

255
00:11:50,170 --> 00:11:53,580
Similarly for the middle node
at 5, and the top node for 3.

256
00:11:53,580 --> 00:11:55,289
Now, the next node
we're going to expand,

257
00:11:55,289 --> 00:11:56,871
can anybody tell me
what the next node

258
00:11:56,871 --> 00:11:59,370
we're going to expand based on
performing best first search

259
00:11:59,370 --> 00:12:02,970
here using cost so far?

260
00:12:02,970 --> 00:12:04,410
AUDIENCE: The 1?

261
00:12:04,410 --> 00:12:05,232
1?

262
00:12:05,232 --> 00:12:05,940
JOE LEAVITT: Yes.

263
00:12:05,940 --> 00:12:08,820
So the node labeled 1 here,
that has a g value of 1

264
00:12:08,820 --> 00:12:12,090
is the lowest out of 1,
3, and 5, and infinity.

265
00:12:12,090 --> 00:12:15,110
So we're going to go
ahead and expand 1 next.

266
00:12:15,110 --> 00:12:20,370
And now, we find that using
this value of 1 plus 3

267
00:12:20,370 --> 00:12:22,830
equals 4, which is less
than what we previously

268
00:12:22,830 --> 00:12:23,646
had here as 4.

269
00:12:23,646 --> 00:12:26,020
So now the center node has
been relaxed from a value of 5

270
00:12:26,020 --> 00:12:27,060
down to 4.

271
00:12:27,060 --> 00:12:32,480
So now the shortest path we
found to the center node is 4.

272
00:12:32,480 --> 00:12:35,220
And we're going to
continue to perform this

273
00:12:35,220 --> 00:12:40,200
by going through each node
in order of the lowest cost.

274
00:12:40,200 --> 00:12:42,570
And similarly, you
saw how the top node

275
00:12:42,570 --> 00:12:46,400
was relaxed to 6 by going
through 1, 3, and 2, where

276
00:12:46,400 --> 00:12:48,730
as previously, it had a
lowest cost of 7 going

277
00:12:48,730 --> 00:12:50,920
through 3 and 4.

278
00:12:50,920 --> 00:12:52,790
And if we format
through the whole graph

279
00:12:52,790 --> 00:12:54,790
until all nodes have
been unexpanded,

280
00:12:54,790 --> 00:12:56,780
then we've relaxed
every node in the graph

281
00:12:56,780 --> 00:12:59,460
to its shortest path value.

282
00:12:59,460 --> 00:13:02,600
And then, we can just
do a greedy search

283
00:13:02,600 --> 00:13:06,800
from a chosen goal node,
back through the graph

284
00:13:06,800 --> 00:13:07,810
to find shortest path.

285
00:13:10,680 --> 00:13:13,770
AUDIENCE: Is the term
relaxation standard?

286
00:13:13,770 --> 00:13:17,880
Because the fact that you're
bringing numbers from infinity

287
00:13:17,880 --> 00:13:20,300
double to small
value sounds more

288
00:13:20,300 --> 00:13:22,040
like tightening as
opposed to relaxation.

289
00:13:22,040 --> 00:13:24,090
So I was just wondering
if people use it

290
00:13:24,090 --> 00:13:25,830
as like, a standard--

291
00:13:25,830 --> 00:13:28,440
JOE LEAVITT: I'm not sure
if it is in all areas,

292
00:13:28,440 --> 00:13:33,230
but I have seen it
in multiple places.

293
00:13:33,230 --> 00:13:35,080
When you're talking
about graph search,

294
00:13:35,080 --> 00:13:38,940
specifically with respect
to Dijkstra's algorithm,

295
00:13:38,940 --> 00:13:41,670
that is where the term
relaxation is used.

296
00:13:41,670 --> 00:13:43,690
And we're going to use
that as we go throughout

297
00:13:43,690 --> 00:13:45,660
and talk about
incremental search.

298
00:13:45,660 --> 00:13:48,282
We're kind of repairing the
results because of new edge

299
00:13:48,282 --> 00:13:49,490
weights and things like that.

300
00:13:49,490 --> 00:13:51,031
We're going to figure
out which nodes

301
00:13:51,031 --> 00:13:53,243
we have to relax back down
to their shortest value

302
00:13:53,243 --> 00:13:56,571
to be able to find
the new shortest path.

303
00:13:56,571 --> 00:13:58,320
So that's the term
we're going to use here

304
00:13:58,320 --> 00:14:00,590
if it's not used universally.

305
00:14:00,590 --> 00:14:02,730
I can't say for certain.

306
00:14:02,730 --> 00:14:07,920
But we will use relaxation
to talk about reducing--

307
00:14:07,920 --> 00:14:10,860
it's kind of related to--

308
00:14:10,860 --> 00:14:13,830
think about like,
a tension spring.

309
00:14:13,830 --> 00:14:18,340
As you reduce the
constraints on it,

310
00:14:18,340 --> 00:14:21,122
it'll slowly relax to its
completely relaxed position.

311
00:14:21,122 --> 00:14:22,330
That's kind of the idea here.

312
00:14:25,704 --> 00:14:29,510
So now that we have talked
about the ideas of relaxation

313
00:14:29,510 --> 00:14:33,860
and graph search, how
do we reuse the results

314
00:14:33,860 --> 00:14:36,110
from a previous search
in a new search field

315
00:14:36,110 --> 00:14:40,850
to employ an incremental
search method

316
00:14:40,850 --> 00:14:43,430
where you have done a search,
you found the shortest path,

317
00:14:43,430 --> 00:14:45,760
and now, you had some
changes to your graph,

318
00:14:45,760 --> 00:14:50,110
and you want to use your
previous results to come up

319
00:14:50,110 --> 00:14:51,470
with your new shortest path.

320
00:14:51,470 --> 00:14:53,060
So the way we're
going to do that,

321
00:14:53,060 --> 00:14:54,851
is we're going to store
our optimal results

322
00:14:54,851 --> 00:14:58,219
from a previous search.

323
00:14:58,219 --> 00:14:59,760
This can be done in
a number of ways.

324
00:14:59,760 --> 00:15:02,460
It's going to depend
on the algorithm.

325
00:15:02,460 --> 00:15:06,020
But you can soar like,
a list of shortest paths

326
00:15:06,020 --> 00:15:09,210
as an incremental all-pairs
shortest paths problem.

327
00:15:09,210 --> 00:15:12,620
The one we're going to talk
most about is storing g values,

328
00:15:12,620 --> 00:15:16,090
and that's what's done
in a D* Lite algorithm,

329
00:15:16,090 --> 00:15:18,690
where you're finding the
shortest distance from any

330
00:15:18,690 --> 00:15:22,815
given node that you've
expanded so far in your search

331
00:15:22,815 --> 00:15:25,160
to the goal node.

332
00:15:25,160 --> 00:15:27,560
And then, you'll use that
data to find out, find

333
00:15:27,560 --> 00:15:28,710
the shortest path.

334
00:15:28,710 --> 00:15:31,520
And then when you go to
perform your next iteration,

335
00:15:31,520 --> 00:15:34,280
you'll look for
inconsistencies in that graph.

336
00:15:34,280 --> 00:15:37,030
And then in order to
find a new shortest path,

337
00:15:37,030 --> 00:15:40,540
we'll leverage that previous
data and the inconsistencies

338
00:15:40,540 --> 00:15:45,464
for relaxations to come up
with a new optimal solution.

339
00:15:45,464 --> 00:15:46,880
And that's all
we're showing here.

340
00:15:46,880 --> 00:15:48,629
On the right side of
these two graphics is

341
00:15:48,629 --> 00:15:50,290
you have the initial search.

342
00:15:50,290 --> 00:15:53,000
You have an obstacle that
is now in a place that

343
00:15:53,000 --> 00:15:54,965
was wasn't there
previously, which

344
00:15:54,965 --> 00:15:58,100
causes you to update edge
weights in the graph associated

345
00:15:58,100 --> 00:15:59,470
with this grid world.

346
00:15:59,470 --> 00:16:01,580
And then using
these values which

347
00:16:01,580 --> 00:16:05,690
represent the g values
in the graph search,

348
00:16:05,690 --> 00:16:08,420
you only update
the ones you need

349
00:16:08,420 --> 00:16:11,870
to update to find
a new optimal path.

350
00:16:11,870 --> 00:16:16,404
And so completely
reperforming our search.

351
00:16:16,404 --> 00:16:17,820
So what incremental
search methods

352
00:16:17,820 --> 00:16:22,850
exist, now that we've kind
of talked about what they are

353
00:16:22,850 --> 00:16:25,350
and what we can do with them?

354
00:16:25,350 --> 00:16:27,890
So the whole idea
of this slide here

355
00:16:27,890 --> 00:16:30,670
is to show you that
incremental search can

356
00:16:30,670 --> 00:16:32,090
be used across many domains.

357
00:16:32,090 --> 00:16:34,810
It's not just specific
to path planning.

358
00:16:34,810 --> 00:16:37,520
It's used in temporal
planning to determine

359
00:16:37,520 --> 00:16:38,840
temporal consistency.

360
00:16:38,840 --> 00:16:41,100
It can be used propositional
satisfiability.

361
00:16:44,130 --> 00:16:48,870
Instead of every time you're
updating another algorithm,

362
00:16:48,870 --> 00:16:50,960
you can use your
previous results.

363
00:16:50,960 --> 00:16:54,860
And the one we're going to
focus on today is D* Lite,

364
00:16:54,860 --> 00:16:57,300
which is specific to
mobile and robots.

365
00:16:57,300 --> 00:16:59,850
But the ideas and concepts
we're going to talk about there

366
00:16:59,850 --> 00:17:01,760
are applicable
elsewhere, and you

367
00:17:01,760 --> 00:17:05,660
can use to help you learn
about and expand your knowledge

368
00:17:05,660 --> 00:17:09,175
in other incremental search.

369
00:17:09,175 --> 00:17:11,609
So the D* incremental
path planning approach.

370
00:17:11,609 --> 00:17:13,319
The whole idea behind
it is that we're

371
00:17:13,319 --> 00:17:16,349
going to take the idea
of incremental search

372
00:17:16,349 --> 00:17:18,180
that we just talked
it about, and we're

373
00:17:18,180 --> 00:17:20,880
going to combine it with
heuristic search, which

374
00:17:20,880 --> 00:17:22,599
is an optimal solution.

375
00:17:22,599 --> 00:17:26,314
And the idea is
that two things are

376
00:17:26,314 --> 00:17:28,560
orthogonal so we
can combine them,

377
00:17:28,560 --> 00:17:30,690
and then we can form
efficient incremental path

378
00:17:30,690 --> 00:17:33,030
planning, getting both
an optimal result,

379
00:17:33,030 --> 00:17:35,452
and quickly getting
a new path when

380
00:17:35,452 --> 00:17:37,410
things change in the
environment without having

381
00:17:37,410 --> 00:17:38,650
to completely replan.

382
00:17:38,650 --> 00:17:42,330
And so what that
looks like, when

383
00:17:42,330 --> 00:17:45,410
we have a grid world, which is
shown in four instances here up

384
00:17:45,410 --> 00:17:48,690
on the board, and we're trying
to plan from a start node

385
00:17:48,690 --> 00:17:51,510
to a goal node, and showing
each of these sections.

386
00:17:51,510 --> 00:17:55,522
And in the upper left or
on the left axis, we have--

387
00:17:55,522 --> 00:17:58,959
whether we're performing
a complete search

388
00:17:58,959 --> 00:18:00,000
or an incremental search.

389
00:18:00,000 --> 00:18:02,280
And on the horizontal
axis, whether it's

390
00:18:02,280 --> 00:18:04,320
an uninformed search
or a heuristic search.

391
00:18:04,320 --> 00:18:07,680
So the upper left is
a complete search,

392
00:18:07,680 --> 00:18:10,136
just using best-first
search and no heuristics.

393
00:18:10,136 --> 00:18:12,510
And what it's showing is all
the nodes that are expanded,

394
00:18:12,510 --> 00:18:16,046
and trying to find a path
from the start to the goal.

395
00:18:16,046 --> 00:18:19,035
In the upper right is
a heuristic search, A*.

396
00:18:19,035 --> 00:18:21,330
Employing the heuristic,
we're able to collapse

397
00:18:21,330 --> 00:18:24,200
the number of nodes
expanded by quite a bit.

398
00:18:24,200 --> 00:18:28,620
In the bottom left is an
incremental search method

399
00:18:28,620 --> 00:18:32,380
that's using uninformed
search or best-first

400
00:18:32,380 --> 00:18:35,070
search as an underlying
search algorithm,

401
00:18:35,070 --> 00:18:38,650
so it's going to expand the same
nodes as the best-first search.

402
00:18:38,650 --> 00:18:42,330
And then in the bottom
right, you see an incremental

403
00:18:42,330 --> 00:18:46,780
heuristic search, the lifelong
planning A* algorithm,

404
00:18:46,780 --> 00:18:49,447
which is kind of the baseline
for the D* Lite we're going

405
00:18:49,447 --> 00:18:53,050
to talk about, that the initial
search will expand the same

406
00:18:53,050 --> 00:18:55,510
nodes as the heuristic search,
since it's using A* as its

407
00:18:55,510 --> 00:18:58,210
baseline.

408
00:18:58,210 --> 00:19:00,990
So now let's say
the robot moves.

409
00:19:00,990 --> 00:19:02,490
We get an update
to the environment.

410
00:19:02,490 --> 00:19:05,730
There's a bunch
of changed edges.

411
00:19:05,730 --> 00:19:08,280
And now best-first
search, as you can see,

412
00:19:08,280 --> 00:19:11,760
is going to have to expand
pretty much the same number,

413
00:19:11,760 --> 00:19:14,820
if not more nodes, then
our previous search.

414
00:19:14,820 --> 00:19:17,850
And A* also is going to expand
about the same number of nodes

415
00:19:17,850 --> 00:19:18,960
as it did previously.

416
00:19:18,960 --> 00:19:22,920
The incremental search method
using best-first search,

417
00:19:22,920 --> 00:19:25,290
by using previous results,
reduces significantly

418
00:19:25,290 --> 00:19:27,420
the number of nodes that
needs to be expanded.

419
00:19:27,420 --> 00:19:30,195
And then, lifelong planning
A*, the incremental search plus

420
00:19:30,195 --> 00:19:35,400
the heuristic search, reduces
that number by even more.

421
00:19:35,400 --> 00:19:37,776
Even when there's quite a few
changes in the environment,

422
00:19:37,776 --> 00:19:39,316
you have very few
nodes that you need

423
00:19:39,316 --> 00:19:41,010
to expand on the
next search in order

424
00:19:41,010 --> 00:19:45,384
to find a new optimal path.

425
00:19:45,384 --> 00:19:49,872
With that, I'll turn it over to
Ben to talk about the D* Lite

426
00:19:49,872 --> 00:19:52,824
algorithm.

427
00:19:52,824 --> 00:19:53,790
BEN AYTON: Thanks.

428
00:19:53,790 --> 00:19:56,800
So I'll be walking you through
the D* Light algorithm today.

429
00:19:56,800 --> 00:19:59,315
Now as we're emphasizing,
the D* Lite is not the only

430
00:19:59,315 --> 00:20:02,190
incremental algorithm, as
we mentioned previously.

431
00:20:02,190 --> 00:20:05,042
But it is a widely used
incremental algorithm,

432
00:20:05,042 --> 00:20:09,180
and you'll see it's quite
efficient in what it does.

433
00:20:09,180 --> 00:20:12,440
D* Lite is an algorithm that
searches from a single start

434
00:20:12,440 --> 00:20:14,480
node to a single goal node.

435
00:20:14,480 --> 00:20:17,950
But as we see changes occur
in the path along the way,

436
00:20:17,950 --> 00:20:20,020
we'll adjust that.

437
00:20:20,020 --> 00:20:24,370
So it's nice to think about
D* Lite from two perspectives.

438
00:20:24,370 --> 00:20:26,860
One is to think about it as a
modification of the principle

439
00:20:26,860 --> 00:20:28,170
space.

440
00:20:28,170 --> 00:20:30,910
Now, you'll see that D* Lite
in its first iteration or its

441
00:20:30,910 --> 00:20:33,750
first run through before any
modifications behaves very

442
00:20:33,750 --> 00:20:36,737
similar to A*, and that's where
it's nice to keep that main

443
00:20:36,737 --> 00:20:37,510
model in mind.

444
00:20:37,510 --> 00:20:41,040
And you'll see that in
an example further along.

445
00:20:41,040 --> 00:20:44,730
The second perspective to look
at A* is from the perspective

446
00:20:44,730 --> 00:20:47,730
of Dijkstra's algorithm,
from which we see many

447
00:20:47,730 --> 00:20:51,690
of the principles of relaxation
that we covered beforehand.

448
00:20:51,690 --> 00:20:54,600
From this perspective, we see
that when we do reparations

449
00:20:54,600 --> 00:20:58,710
to the graph or changes to the
graph, our methods of repairs

450
00:20:58,710 --> 00:21:02,000
the graph is essentially to
relax down our changed edge

451
00:21:02,000 --> 00:21:04,080
weights until they
reach their truth.

452
00:21:04,080 --> 00:21:06,510
Now, where we differ from
Dijkstra's algorithm with

453
00:21:06,510 --> 00:21:11,010
something like D* Lite is
selecting only the nodes that

454
00:21:11,010 --> 00:21:12,570
we want to hit efficiently.

455
00:21:12,570 --> 00:21:17,160
So whereas Dijkstra's algorithm
tried to fit the entire graph,

456
00:21:17,160 --> 00:21:20,250
we only want to travel
from a single start node

457
00:21:20,250 --> 00:21:21,750
to a single goal node.

458
00:21:21,750 --> 00:21:24,860
And so D* Lite behaves like
Dijkstra's, but a guided

459
00:21:24,860 --> 00:21:26,970
Dijkstra's.

460
00:21:26,970 --> 00:21:30,400
So I'd like to just remind you
of several concepts from A*

461
00:21:30,400 --> 00:21:32,950
first, because it's easier to
introduce them in that context.

462
00:21:32,950 --> 00:21:35,070
And then we'll modify
them a little bit.

463
00:21:35,070 --> 00:21:38,340
So Joe mentioned
before that for A*,

464
00:21:38,340 --> 00:21:41,370
we're searching from a
start node to a goal node.

465
00:21:41,370 --> 00:21:43,380
We're sorting the
nodes in our queue

466
00:21:43,380 --> 00:21:47,130
by total cost, which is the sum
of what we're calling g, which

467
00:21:47,130 --> 00:21:50,495
are costs to get
to a single node,

468
00:21:50,495 --> 00:21:53,640
and our heuristic, which is
the cost to get from a node,

469
00:21:53,640 --> 00:21:55,546
s, to the goal node, your s.

470
00:21:58,740 --> 00:22:01,320
Now, what we don't
usually consider with A*,

471
00:22:01,320 --> 00:22:05,580
but is very important for D*, is
how to distinguish between ties

472
00:22:05,580 --> 00:22:08,880
when two different nodes on the
search cube have the same total

473
00:22:08,880 --> 00:22:09,792
cost.

474
00:22:09,792 --> 00:22:11,250
So here's what
we're doing is we're

475
00:22:11,250 --> 00:22:14,770
introducing a couplet, which
consists of two values.

476
00:22:14,770 --> 00:22:17,940
The first, which I'm calling
f1 here for any given state,

477
00:22:17,940 --> 00:22:20,370
is the same as what
we're usually used to.

478
00:22:20,370 --> 00:22:22,500
It's the linear
combination of your cost

479
00:22:22,500 --> 00:22:25,000
to reach the goal node
and heuristic function.

480
00:22:25,000 --> 00:22:28,770
But we also introduced
a second value

481
00:22:28,770 --> 00:22:31,140
for this couplet that's used
in the case for only those

482
00:22:31,140 --> 00:22:32,250
to draw.

483
00:22:32,250 --> 00:22:35,290
And that is simply the
cost to go, that g value.

484
00:22:35,290 --> 00:22:38,155
So the order of expansion
of nodes from the q

485
00:22:38,155 --> 00:22:41,880
will order them in
terms of f1 first,

486
00:22:41,880 --> 00:22:44,050
and if two nodes at
the front of the cube

487
00:22:44,050 --> 00:22:46,120
tie in terms of
their f1 value, we

488
00:22:46,120 --> 00:22:49,110
select the node with
the lowest g value.

489
00:22:49,110 --> 00:22:52,200
So looking at our
graph down here,

490
00:22:52,200 --> 00:22:56,250
we're looking at our
states s1 here and s2 here.

491
00:22:56,250 --> 00:23:01,350
Can anyone tell me the
total cost for our node s1

492
00:23:01,350 --> 00:23:03,990
which, remember, is a couplet
of two different values?

493
00:23:11,230 --> 00:23:13,220
5, 3.

494
00:23:13,220 --> 00:23:16,790
So five, yes, is
our cost to reach

495
00:23:16,790 --> 00:23:21,170
this node one, plus the
heuristic, which is 2 here.

496
00:23:21,170 --> 00:23:24,804
And also, 3 is our second value,
because it's only our g value

497
00:23:24,804 --> 00:23:25,640
there.

498
00:23:25,640 --> 00:23:28,470
And so what would the value
be by the same logic for s2

499
00:23:28,470 --> 00:23:30,960
down here?

500
00:23:30,960 --> 00:23:31,994
AUDIENCE: 5, 2.

501
00:23:31,994 --> 00:23:32,660
BEN AYTON: 5, 2.

502
00:23:32,660 --> 00:23:35,050
That's exactly right.

503
00:23:35,050 --> 00:23:37,540
And so, bearing in mind
what I told you before,

504
00:23:37,540 --> 00:23:40,852
where should we take
off the q first?

505
00:23:40,852 --> 00:23:42,230
AUDIENCE: S2?

506
00:23:42,230 --> 00:23:45,260
BEN AYTON: That's exactly right,
because the first value, f1,

507
00:23:45,260 --> 00:23:47,060
for these two nodes
is exactly the same.

508
00:23:47,060 --> 00:23:52,210
But the second value
is lower is f(s)2.

509
00:23:52,210 --> 00:23:55,230
I'd also like to introduce
the concept of successors

510
00:23:55,230 --> 00:23:58,390
and predecessors
of any given node.

511
00:23:58,390 --> 00:24:01,071
In general, a graph
consists of directed edges.

512
00:24:01,071 --> 00:24:03,070
And we could have a graph
with undirected edges,

513
00:24:03,070 --> 00:24:05,890
but we can think of
as just any edge being

514
00:24:05,890 --> 00:24:07,960
directed in both direction.

515
00:24:07,960 --> 00:24:11,080
So here, we define the concept
of a successor of a node, which

516
00:24:11,080 --> 00:24:15,810
is every node can be reached
from a given node, along edges

517
00:24:15,810 --> 00:24:17,430
that can be found.

518
00:24:17,430 --> 00:24:19,200
So the successors of
this given red node

519
00:24:19,200 --> 00:24:22,770
are these two blue nodes here.

520
00:24:22,770 --> 00:24:25,600
We also introduced the
concept of predecessors,

521
00:24:25,600 --> 00:24:28,990
which is every node from which
that node can be reached,

522
00:24:28,990 --> 00:24:30,820
meaning nodes from
which we can follow

523
00:24:30,820 --> 00:24:33,170
our directed edge to our node.

524
00:24:33,170 --> 00:24:35,690
So for our red node,
the predecessors

525
00:24:35,690 --> 00:24:37,260
are these two nodes.

526
00:24:37,260 --> 00:24:38,920
What I'd like to
emphasize here is

527
00:24:38,920 --> 00:24:41,500
comparing the two,
that this node here

528
00:24:41,500 --> 00:24:45,120
was both the processor and
the successor of our red node.

529
00:24:45,120 --> 00:24:46,395
And that's fine.

530
00:24:46,395 --> 00:24:49,460
That occurs when we have
undirected edges or edges

531
00:24:49,460 --> 00:24:53,030
that direct in both directions.

532
00:24:53,030 --> 00:24:56,660
So think about D* Lite in
the sense that is a repeated

533
00:24:56,660 --> 00:24:59,370
best-first search, which is
where the A* principles come

534
00:24:59,370 --> 00:25:03,970
in through a graph with changing
edge weights as that graph is

535
00:25:03,970 --> 00:25:07,280
traversed, meaning that as
we travel from our start node

536
00:25:07,280 --> 00:25:11,780
to our goal node along a
path that we are planning,

537
00:25:11,780 --> 00:25:14,056
we can see that edge
weights are changing.

538
00:25:14,056 --> 00:25:15,430
And in this example,
I'm modeling

539
00:25:15,430 --> 00:25:19,070
an obstacle coming in place
between this node here

540
00:25:19,070 --> 00:25:22,500
and the goal, which means that
we have removed that edge.

541
00:25:22,500 --> 00:25:25,381
And that's effectively the same
as changing an edge weight.

542
00:25:25,381 --> 00:25:27,506
We're just modeling it as
changing this edge weight

543
00:25:27,506 --> 00:25:31,796
to infinity, which means that
edge can't be traveled along.

544
00:25:31,796 --> 00:25:33,820
And as I mentioned
before, we also

545
00:25:33,820 --> 00:25:37,470
view this as replanning through
relaxation of path costs.

546
00:25:37,470 --> 00:25:39,570
So once we have
moved to this point,

547
00:25:39,570 --> 00:25:41,630
how do we find
the path from here

548
00:25:41,630 --> 00:25:48,690
to here when this edge has
been removed, essentially?

549
00:25:48,690 --> 00:25:51,750
So we want to make
D* Lite efficient.

550
00:25:51,750 --> 00:25:54,060
And it's a repeated
search through the graph

551
00:25:54,060 --> 00:25:55,800
as we traverse it.

552
00:25:55,800 --> 00:26:00,830
So you'll see that since we're
using these g values, if we

553
00:26:00,830 --> 00:26:06,510
maintain a formulation where
we're measuring the distance

554
00:26:06,510 --> 00:26:10,320
to go or to reach a node
from our start node,

555
00:26:10,320 --> 00:26:13,059
this is not preserved when
we move our start node.

556
00:26:13,059 --> 00:26:14,975
And we move our start
node when we're updating

557
00:26:14,975 --> 00:26:17,040
the position of our vehicle.

558
00:26:17,040 --> 00:26:19,800
So let me show this
through an example here.

559
00:26:19,800 --> 00:26:22,860
The g value, or cost to
get from one start node

560
00:26:22,860 --> 00:26:25,380
to our current node, s,
here is the combination

561
00:26:25,380 --> 00:26:26,460
of these two edges.

562
00:26:26,460 --> 00:26:28,030
And so, it sums to 4.

563
00:26:28,030 --> 00:26:30,750
Now when we move along
that path that we've

564
00:26:30,750 --> 00:26:35,340
planned to this next node, the
g value to get to that same node

565
00:26:35,340 --> 00:26:36,630
is here too.

566
00:26:36,630 --> 00:26:37,987
So this isn't preserved.

567
00:26:37,987 --> 00:26:40,320
And that means that we'd have
to potentially reformulate

568
00:26:40,320 --> 00:26:43,840
things in our cue, which
would be very inefficient.

569
00:26:43,840 --> 00:26:46,230
So what we do is we
reformulate our search.

570
00:26:46,230 --> 00:26:48,900
And it's perfectly valid
to reformulate our search

571
00:26:48,900 --> 00:26:50,460
to search in the
opposite direction.

572
00:26:50,460 --> 00:26:54,120
We're essentially measuring
for our g values now the cost

573
00:26:54,120 --> 00:26:56,920
to reach the goal
from a specific.

574
00:26:56,920 --> 00:27:00,660
And instead of heuristics,
then measure the costs

575
00:27:00,660 --> 00:27:03,812
to get from this specific
node, or to get from the start

576
00:27:03,812 --> 00:27:05,060
node to our specific node.

577
00:27:05,060 --> 00:27:08,040
And so we search from the goal
backwards through the graph.

578
00:27:08,040 --> 00:27:10,825
In this case, we see that
regardless of whether our start

579
00:27:10,825 --> 00:27:15,330
node, we preserve our g values,
and our cost to travel from

580
00:27:15,330 --> 00:27:16,865
s to that goal.

581
00:27:16,865 --> 00:27:18,240
That's what we
like, and we we'll

582
00:27:18,240 --> 00:27:23,490
keep that reformulation
to maximize efficiency.

583
00:27:23,490 --> 00:27:26,640
So here, I want to give you a
kind of overall understanding

584
00:27:26,640 --> 00:27:29,510
of the D* Lite algorithm before
we dive into specifics of what

585
00:27:29,510 --> 00:27:30,850
it's doing.

586
00:27:30,850 --> 00:27:33,900
First we want to initialize
all nodes as unexpanded.

587
00:27:33,900 --> 00:27:36,045
And what exactly, it means
expand a node neighbor

588
00:27:36,045 --> 00:27:37,840
or cover.

589
00:27:37,840 --> 00:27:39,920
And then we're doing
a best-first search

590
00:27:39,920 --> 00:27:42,210
from the goal node
to the start node

591
00:27:42,210 --> 00:27:45,410
until the start node is
consistent with its neighbors.

592
00:27:45,410 --> 00:27:48,000
And I'll again say
what that means later.

593
00:27:48,000 --> 00:27:51,030
We then move our start node
to the next best vertex,

594
00:27:51,030 --> 00:27:55,580
essentially our node moving
along the plan that we created.

595
00:27:55,580 --> 00:27:59,220
And then we have to see if any
of those edge costs change.

596
00:27:59,220 --> 00:28:01,680
We track how our heuristics
change, and then update

597
00:28:01,680 --> 00:28:03,530
the source nodes of
those changed edges.

598
00:28:03,530 --> 00:28:06,690
And we essentially
repeat this process

599
00:28:06,690 --> 00:28:11,890
until, again, we've converged to
a solution, and we keep moving.

600
00:28:11,890 --> 00:28:15,450
So this best-first search, the
A* Lite part of the algorithm,

601
00:28:15,450 --> 00:28:18,840
is where most of the computation
is going to be occurring.

602
00:28:18,840 --> 00:28:21,540
However, the
incremental component

603
00:28:21,540 --> 00:28:25,560
is actually how can we
adjust our edge costs

604
00:28:25,560 --> 00:28:28,320
to make sure that we're
using most of the information

605
00:28:28,320 --> 00:28:31,570
that we previously gathered
as efficiently as possible.

606
00:28:31,570 --> 00:28:34,670
So I'm going to walk you
through various steps of this.

607
00:28:34,670 --> 00:28:36,810
First, I'd like to walk
you through this step

608
00:28:36,810 --> 00:28:39,050
of moving to the
next best vertex,

609
00:28:39,050 --> 00:28:42,050
just so you understand that.

610
00:28:42,050 --> 00:28:45,450
So we can extract a
path, given the path

611
00:28:45,450 --> 00:28:48,280
costs that have, through
this simple argument,

612
00:28:48,280 --> 00:28:52,080
which says that we
map the successor,

613
00:28:52,080 --> 00:28:56,502
by which I mean we restate
what the start node is,

614
00:28:56,502 --> 00:28:57,960
because we're moving
from one state

615
00:28:57,960 --> 00:29:01,200
to the next state, which
becomes the new start node,

616
00:29:01,200 --> 00:29:05,820
by this form, which essentially
says that we're taking the best

617
00:29:05,820 --> 00:29:11,490
path in the sense that we're
minimizing the total cost that

618
00:29:11,490 --> 00:29:16,210
would be gained traveling
by that path to this node.

619
00:29:16,210 --> 00:29:20,940
So in a sense, the g
value to get to this node,

620
00:29:20,940 --> 00:29:23,520
to the red from the green,
or color stretching back

621
00:29:23,520 --> 00:29:27,620
through the graph, by
this path here, is 7.

622
00:29:27,620 --> 00:29:31,310
But to get to this node
from the lower node is 10.

623
00:29:31,310 --> 00:29:35,130
And so our g value that is
optimal is 7 in this case,

624
00:29:35,130 --> 00:29:38,700
and know that we pick the
successor as the green node

625
00:29:38,700 --> 00:29:41,240
up here.

626
00:29:41,240 --> 00:29:44,355
So now to walk through
some principles that

627
00:29:44,355 --> 00:29:48,210
appear in both of these steps.

628
00:29:48,210 --> 00:29:51,051
I want to show how we handle
weight changes locally.

629
00:29:51,051 --> 00:29:52,550
And we do this
because we don't want

630
00:29:52,550 --> 00:29:54,716
to track weight changes
throughout the entire graph.

631
00:29:54,716 --> 00:29:57,180
We want to handle this
efficiently and separate them,

632
00:29:57,180 --> 00:29:59,280
so we only need to
handle the changes that

633
00:29:59,280 --> 00:30:01,380
really impact our problem.

634
00:30:01,380 --> 00:30:03,890
So this is the same formula
that defines our successor,

635
00:30:03,890 --> 00:30:07,450
or our g values
for a given node.

636
00:30:07,450 --> 00:30:09,210
But we can see that
this may no longer

637
00:30:09,210 --> 00:30:10,815
hold when edge weights change.

638
00:30:10,815 --> 00:30:13,890
The example that I have
here is that I've changed

639
00:30:13,890 --> 00:30:18,120
edge weight cost from 6 to 1.

640
00:30:18,120 --> 00:30:21,420
And so now we can reach the
red node, searching back

641
00:30:21,420 --> 00:30:23,090
through graph by a
new optimal path,

642
00:30:23,090 --> 00:30:26,820
coming down from this node
here, which would make our new g

643
00:30:26,820 --> 00:30:28,710
value five instead.

644
00:30:28,710 --> 00:30:33,450
So this hasn't been preserved.

645
00:30:33,450 --> 00:30:36,660
And then these changes then
propagate to our predecessors,

646
00:30:36,660 --> 00:30:39,510
because the costs
for the processors

647
00:30:39,510 --> 00:30:43,140
are each dependent on the g
value for their successor.

648
00:30:43,140 --> 00:30:45,180
And so when we hit
a node, we have

649
00:30:45,180 --> 00:30:48,300
to propagate that change all
the way back through the graph.

650
00:30:48,300 --> 00:30:52,110
So to do this efficiently,
we take an A* like approach,

651
00:30:52,110 --> 00:30:55,030
where we update the
lowest cost nodes first,

652
00:30:55,030 --> 00:30:59,360
and we don't expand a node until
it is the next lowest cost.

653
00:31:03,030 --> 00:31:05,930
To help us do this, we're
going to store additional fact.

654
00:31:05,930 --> 00:31:07,765
We'll call this rhs.

655
00:31:07,765 --> 00:31:11,400
Now the meaning
behind rhs, it comes

656
00:31:11,400 --> 00:31:14,400
from the term right-hand side,
which in the paper world,

657
00:31:14,400 --> 00:31:16,470
was first introduced and
made more sense than it

658
00:31:16,470 --> 00:31:17,880
does so here.

659
00:31:17,880 --> 00:31:21,180
What you can think of your
rhs value as is essentially

660
00:31:21,180 --> 00:31:23,180
you're corrected your g costs.

661
00:31:23,180 --> 00:31:26,750
When edge costs
change, how G should

662
00:31:26,750 --> 00:31:30,741
be signed so that it's
correct for a different graph.

663
00:31:30,741 --> 00:31:35,190
And when your rhs value is not
equal to your g value, that

664
00:31:35,190 --> 00:31:38,460
means that we have what is
called a local inconsistency.

665
00:31:38,460 --> 00:31:41,730
So given the logic that
your rhs value should

666
00:31:41,730 --> 00:31:45,360
be what your g value
would be corrected to be,

667
00:31:45,360 --> 00:31:48,344
what would your rhs value
be for this graph here?

668
00:31:54,420 --> 00:31:55,230
AUDIENCE: 5?

669
00:31:55,230 --> 00:31:56,890
BEN AYTON: 5, yes.

670
00:31:56,890 --> 00:32:00,330
So this is the same graph
that I presented previously,

671
00:32:00,330 --> 00:32:02,822
and I walked through
how g should be 5.

672
00:32:02,822 --> 00:32:05,841
rhs should be 5 here.

673
00:32:05,841 --> 00:32:10,280
And what we're tracking
is rhs and g differently.

674
00:32:10,280 --> 00:32:13,560
So g is what it should be,
but we haven't got the dated g

675
00:32:13,560 --> 00:32:15,540
to be 5 yet.

676
00:32:15,540 --> 00:32:18,060
So we have two different
types of local consistencies

677
00:32:18,060 --> 00:32:20,340
that we distinguish between.

678
00:32:20,340 --> 00:32:23,700
We have what we call local
overconsistency, where g

679
00:32:23,700 --> 00:32:26,260
is greater than your rhs value.

680
00:32:26,260 --> 00:32:28,580
This is going to
behave more similarly

681
00:32:28,580 --> 00:32:33,270
to Dijkstra's algorithm,
where the value that we're

682
00:32:33,270 --> 00:32:35,580
associating to any
given node now is higher

683
00:32:35,580 --> 00:32:38,100
than it should be, and
we're relaxing down

684
00:32:38,100 --> 00:32:39,880
to the true value.

685
00:32:39,880 --> 00:32:41,910
However, we have a lovely
underconsistent case

686
00:32:41,910 --> 00:32:44,620
that we have to handle
slightly differently.

687
00:32:44,620 --> 00:32:48,740
So now I'm going to walk through
updating and expanding nodes.

688
00:32:48,740 --> 00:32:52,770
What I mean by updating a node
is recomputing the rhs value,

689
00:32:52,770 --> 00:32:55,470
and then placing that node
on the priority queue,

690
00:32:55,470 --> 00:32:59,130
if that node is
locally inconsistent.

691
00:32:59,130 --> 00:33:04,320
So to give an example here,
I've changed this value,

692
00:33:04,320 --> 00:33:07,350
moving from 6 at
the bottom to 1,

693
00:33:07,350 --> 00:33:10,950
and recomputing rhs,
as we did beforehand.

694
00:33:10,950 --> 00:33:13,980
Now we see that the node
is locally inconsistent.

695
00:33:13,980 --> 00:33:18,530
In this case, it's
locally underconsistent.

696
00:33:18,530 --> 00:33:23,170
And so we place that
on the priority queue.

697
00:33:23,170 --> 00:33:28,990
The priority queue values are
based on this new formula.

698
00:33:28,990 --> 00:33:34,890
This new formula essentially
handles how we see G and rhs.

699
00:33:34,890 --> 00:33:38,970
In essence, we want to take the
minimum value of one of these,

700
00:33:38,970 --> 00:33:43,305
so that we can handle it
the first time that it

701
00:33:43,305 --> 00:33:45,890
will cause changes that
propagate through the graph.

702
00:33:45,890 --> 00:33:49,155
And so this is essentially our
ordering on our priority queue

703
00:33:49,155 --> 00:33:54,740
which is essentially the same
coupling that we saw for A*.

704
00:33:54,740 --> 00:33:56,875
But here, we just
have g of s replaced

705
00:33:56,875 --> 00:34:00,770
by the minimum of g
of s and rhs of s.

706
00:34:00,770 --> 00:34:04,480
And so, here your
minimum of g and rhs

707
00:34:04,480 --> 00:34:08,550
is 5, plus a heuristic, leads
to a total cost associated

708
00:34:08,550 --> 00:34:11,969
with this node of 9, 5.

709
00:34:11,969 --> 00:34:14,280
So we expand our
five prior nodes

710
00:34:14,280 --> 00:34:19,260
by then taking them off the
priority queue and changing g.

711
00:34:19,260 --> 00:34:22,290
Now I said here that we
have an inconsistent.

712
00:34:22,290 --> 00:34:27,449
And so, we update this in much
the way that we would expect.

713
00:34:27,449 --> 00:34:30,916
Your g values are relaxing
down to your rhs values,

714
00:34:30,916 --> 00:34:35,320
and so we just set g
to be equal to rhs.

715
00:34:35,320 --> 00:34:37,920
In this case, our node is
now locally consistent,

716
00:34:37,920 --> 00:34:39,780
and it's going to stay that way.

717
00:34:39,780 --> 00:34:42,270
So can anyone think
of some good reasons

718
00:34:42,270 --> 00:34:45,570
why I go through the process
of changing rhs and keeping

719
00:34:45,570 --> 00:34:48,880
track of that new value,
only to put on the queue,

720
00:34:48,880 --> 00:34:50,610
and then take it
off again, instead

721
00:34:50,610 --> 00:34:54,953
of just recomputing what g of
s should be in the first place?

722
00:34:54,953 --> 00:34:56,429
Does anyone have any ideas?

723
00:34:59,873 --> 00:35:02,825
AUDIENCE: [INAUDIBLE]

724
00:35:05,790 --> 00:35:08,660
BEN AYTON: What you can
think of is that you're

725
00:35:08,660 --> 00:35:10,960
putting something on the queue.

726
00:35:10,960 --> 00:35:15,630
If we update that
value immediately,

727
00:35:15,630 --> 00:35:19,610
there are multiple nodes that
could change the same node

728
00:35:19,610 --> 00:35:21,170
that we just considered.

729
00:35:21,170 --> 00:35:25,130
By, which I mean more work
changes as they propagate back

730
00:35:25,130 --> 00:35:27,590
through the graph,
could lead to changes

731
00:35:27,590 --> 00:35:31,200
in rhs value of
that specific node.

732
00:35:31,200 --> 00:35:34,520
And so, what we would need to
do if we can recompute g of s

733
00:35:34,520 --> 00:35:39,760
every time, is put
it on the queue

734
00:35:39,760 --> 00:35:42,440
to signal that we're
recomputing g of s,

735
00:35:42,440 --> 00:35:47,730
and do that recomputation, and
then new changes we've done,

736
00:35:47,730 --> 00:35:50,090
perform that again, do
that again and again

737
00:35:50,090 --> 00:35:50,930
and again and again.

738
00:35:50,930 --> 00:35:53,414
And we want to avoid
that situation.

739
00:35:53,414 --> 00:35:54,830
So what we're going
to do is we're

740
00:35:54,830 --> 00:35:57,160
going to keep our rhs
as essentially a temp

741
00:35:57,160 --> 00:35:59,990
value that can be adjusted
when it's in the queue.

742
00:35:59,990 --> 00:36:02,900
So when something is on the
queue, it's on there once,

743
00:36:02,900 --> 00:36:05,570
and it can be updated
and taken off.

744
00:36:05,570 --> 00:36:07,320
But we know when
it's been taken off,

745
00:36:07,320 --> 00:36:09,070
that it's been taken
for the correct time,

746
00:36:09,070 --> 00:36:11,210
and won't need to
be handled again.

747
00:36:11,210 --> 00:36:14,130
AUDIENCE: So if I
understand this correctly,

748
00:36:14,130 --> 00:36:20,195
so if you had a single
edge cost that we changed,

749
00:36:20,195 --> 00:36:22,730
then you could just do what
you just said, in terms of,

750
00:36:22,730 --> 00:36:24,880
you could just update
the thing, and probably

751
00:36:24,880 --> 00:36:26,215
should be predecessor rate.

752
00:36:26,215 --> 00:36:26,840
BEN AYTON: Yes.

753
00:36:26,840 --> 00:36:29,120
So if you knew for sure
that nothing else was coming

754
00:36:29,120 --> 00:36:31,400
back and down,
then we could just

755
00:36:31,400 --> 00:36:33,830
do that change and propagate
to all the predecessors,

756
00:36:33,830 --> 00:36:36,359
because nothing upstream
of that node is affected.

757
00:36:36,359 --> 00:36:38,150
AUDIENCE: So am I
understanding correctly--

758
00:36:38,150 --> 00:36:43,330
so you're basically running
the programming, because you're

759
00:36:43,330 --> 00:36:47,840
keeping the costs
to goal to devote,

760
00:36:47,840 --> 00:36:50,060
and then what you're doing
is every time you have

761
00:36:50,060 --> 00:36:53,870
an edge that changes to a value
that could potentially make

762
00:36:53,870 --> 00:36:55,370
that node better
than it was before,

763
00:36:55,370 --> 00:36:59,660
so if it changes the value
of that node to something

764
00:36:59,660 --> 00:37:05,090
better than it was before,
you would change that node,

765
00:37:05,090 --> 00:37:09,260
and then queue all
the predecessors,

766
00:37:09,260 --> 00:37:10,940
because those be
changed as well.

767
00:37:10,940 --> 00:37:12,145
BEN AYTON: Yes.

768
00:37:12,145 --> 00:37:14,800
AUDIENCE: I mean, if
those nodes do not change,

769
00:37:14,800 --> 00:37:15,932
you don't do anything.

770
00:37:15,932 --> 00:37:17,140
You're moving from the queue.

771
00:37:17,140 --> 00:37:18,650
They also change a
better bell and keep

772
00:37:18,650 --> 00:37:20,870
propagating until you have
nothing else to [INAUDIBLE]..

773
00:37:20,870 --> 00:37:21,536
BEN AYTON: Yeah.

774
00:37:21,536 --> 00:37:26,720
But we only do the
propagation, or we

775
00:37:26,720 --> 00:37:29,030
queue changes that occur
earlier in the graph,

776
00:37:29,030 --> 00:37:33,500
but we only actually
perform the change

777
00:37:33,500 --> 00:37:36,390
resetting the g value,
when it's expanded.

778
00:37:36,390 --> 00:37:38,570
AUDIENCE: And then
the reason for that

779
00:37:38,570 --> 00:37:43,726
is for efficiency when you have
more than one edge changing?

780
00:37:43,726 --> 00:37:44,350
BEN AYTON: Yes.

781
00:37:44,350 --> 00:37:47,490
AUDIENCE: So you could basically
have multiple paths affecting

782
00:37:47,490 --> 00:37:48,680
the same node.

783
00:37:48,680 --> 00:37:51,380
And in this situation,
if you actually

784
00:37:51,380 --> 00:37:55,030
allow your changes will
occur and multiple edges,

785
00:37:55,030 --> 00:37:57,140
that's when the importance
of the queue comes.

786
00:37:57,140 --> 00:37:57,810
BEN AYTON: That's exactly.

787
00:37:57,810 --> 00:37:59,643
AUDIENCE: Which is
actually, it prevents you

788
00:37:59,643 --> 00:38:01,620
from redoing the same
work over and over again.

789
00:38:01,620 --> 00:38:02,300
BEN AYTON: That's exactly right.

790
00:38:02,300 --> 00:38:04,000
Because if another
change came through,

791
00:38:04,000 --> 00:38:04,940
then we'd put all the
predecessors back in the queue

792
00:38:04,940 --> 00:38:07,380
again, and we'd do all of
our updates for all of them

793
00:38:07,380 --> 00:38:07,880
as well.

794
00:38:07,880 --> 00:38:09,520
We're avoiding that process.

795
00:38:09,520 --> 00:38:13,290
AUDIENCE: And the queue ensures
that we're only updating--

796
00:38:13,290 --> 00:38:15,935
and I think that'll cover it.

797
00:38:15,935 --> 00:38:17,810
But in the manner in
which we terminate,

798
00:38:17,810 --> 00:38:20,434
we don't necessarily-- we don't
expand every node in the queue.

799
00:38:20,434 --> 00:38:23,180
We only expand those we need
in order to find the shortest,

800
00:38:23,180 --> 00:38:25,760
and then we retain the queue
for that search in case

801
00:38:25,760 --> 00:38:28,942
we need to update those in
the next round of the search.

802
00:38:28,942 --> 00:38:29,650
BEN AYTON: Right.

803
00:38:41,410 --> 00:38:43,650
So I have to correct a
mistake in my language

804
00:38:43,650 --> 00:38:45,450
that I said before.

805
00:38:45,450 --> 00:38:48,072
I said that the node in my
examples that I gave before

806
00:38:48,072 --> 00:38:50,280
was highly consistent,
[INAUDIBLE] wasn't consistent.

807
00:38:50,280 --> 00:38:52,220
I apologize for that mistake.

808
00:38:52,220 --> 00:38:56,800
Other consistency is the easy
case that we're handling here,

809
00:38:56,800 --> 00:39:01,440
where when we expand that
node, we have to take g of s

810
00:39:01,440 --> 00:39:06,080
and set that to be equal to
the rhs value for expansion.

811
00:39:06,080 --> 00:39:09,310
And then we
propagate that effect

812
00:39:09,310 --> 00:39:11,170
to all of the predecessors
at that point,

813
00:39:11,170 --> 00:39:14,320
which means we update
the predecessors

814
00:39:14,320 --> 00:39:20,370
and put those on the queue if
it hasn't had an impact on them.

815
00:39:20,370 --> 00:39:23,420
This means that the node is
not low key inconsistent,

816
00:39:23,420 --> 00:39:26,650
and it's going to remain
that way in that fashion.

817
00:39:26,650 --> 00:39:28,960
The underconsistent
case, which was

818
00:39:28,960 --> 00:39:33,890
the opposite of what I showed,
is the more difficult case.

819
00:39:33,890 --> 00:39:36,790
In this case, the old
path cost was better

820
00:39:36,790 --> 00:39:38,220
than the new rhs value.

821
00:39:38,220 --> 00:39:41,310
Rhs value has increased
to above [INAUDIBLE]..

822
00:39:41,310 --> 00:39:46,090
In this case, we can no longer
relax down to that g value

823
00:39:46,090 --> 00:39:48,280
from our Dijkstra's
algorithm respective.

824
00:39:48,280 --> 00:39:51,890
So we have to think of a
new way to handle this.

825
00:39:51,890 --> 00:39:56,100
What we do is we
essentially set our g value

826
00:39:56,100 --> 00:39:58,650
to be equal to infinity.

827
00:39:58,650 --> 00:40:01,090
And this is an extra
step that's going

828
00:40:01,090 --> 00:40:05,260
to cause the node to go back on
the queue an additional time.

829
00:40:05,260 --> 00:40:09,400
But what this mechanism assures
is that that node goes back

830
00:40:09,400 --> 00:40:12,000
on the queue at most
one additional time,

831
00:40:12,000 --> 00:40:15,120
which means that for D* Lite,
we only ever examine any node

832
00:40:15,120 --> 00:40:19,270
at most twice on the queue.

833
00:40:19,270 --> 00:40:22,220
Once we've set g of s
to be equal to infinity,

834
00:40:22,220 --> 00:40:24,040
then we can
essentially relax down

835
00:40:24,040 --> 00:40:28,120
to the rhs value from the
perspective that we had before.

836
00:40:28,120 --> 00:40:31,180
So we set g of s to
be equal to infinity,

837
00:40:31,180 --> 00:40:33,180
and we update all
the predecessors of s

838
00:40:33,180 --> 00:40:34,674
and s itself this time.

839
00:40:34,674 --> 00:40:37,090
As I was saying before, we
need to put that node node back

840
00:40:37,090 --> 00:40:39,010
on the cue so that
we handle it when

841
00:40:39,010 --> 00:40:41,500
rhs has hit its minimal value.

842
00:40:41,500 --> 00:40:43,180
And this means that
that node is now

843
00:40:43,180 --> 00:40:46,628
going to locally consistent
or overconsistent.

844
00:40:46,628 --> 00:40:49,370
And we handle
overconsistency in the case

845
00:40:49,370 --> 00:40:51,190
that I've shown you before.

846
00:40:51,190 --> 00:40:54,940
So to give you an idea of
how exactly this works,

847
00:40:54,940 --> 00:40:57,910
I'm showing you an example here.

848
00:40:57,910 --> 00:41:02,980
We start off with this graph,
and we introduce two changes.

849
00:41:02,980 --> 00:41:06,750
We have increased
this edge cost to 5.

850
00:41:06,750 --> 00:41:10,500
And so rhs has now
increased to a total of 10.

851
00:41:10,500 --> 00:41:15,340
But I've also changed
a value here for edges

852
00:41:15,340 --> 00:41:16,780
that you can't can see.

853
00:41:16,780 --> 00:41:21,260
But assume that the rhs value
of this node has also changed.

854
00:41:21,260 --> 00:41:23,450
So now we can put
all of these nodes

855
00:41:23,450 --> 00:41:27,670
on a cable ordered in this
way, computed using the formula

856
00:41:27,670 --> 00:41:29,037
that we've seen for it.

857
00:41:32,020 --> 00:41:35,335
We expand the node s1 first.

858
00:41:35,335 --> 00:41:38,430
And because it's
underconsistent,

859
00:41:38,430 --> 00:41:42,060
we set g to be
equal to infinity.

860
00:41:42,060 --> 00:41:50,800
Then we propagate to all
predecessors and s1 itself,

861
00:41:50,800 --> 00:41:53,040
meaning those go
back on the queue.

862
00:41:53,040 --> 00:41:55,110
Now, assuming that node
changes of any relevance

863
00:41:55,110 --> 00:41:59,910
happen in these nodes,
we recompute the costs

864
00:41:59,910 --> 00:42:03,090
for s1, which has now
changed to 14, 10,

865
00:42:03,090 --> 00:42:05,555
because we're using the
minimum of infinity and 10,

866
00:42:05,555 --> 00:42:12,070
which is 10 plus 14,
going behind the node s2.

867
00:42:12,070 --> 00:42:14,320
Next, we expand this node s2.

868
00:42:14,320 --> 00:42:16,970
And because that was
an overconsistent case,

869
00:42:16,970 --> 00:42:20,040
we can just set g to be
equal to our rhs value.

870
00:42:20,040 --> 00:42:23,080
We then have to propagate that
to its predecessors, which

871
00:42:23,080 --> 00:42:27,490
includes s1, which has
updated its rhs value.

872
00:42:27,490 --> 00:42:31,210
So if we didn't do
this, we wouldn't have

873
00:42:31,210 --> 00:42:34,420
handled this effect correctly.

874
00:42:34,420 --> 00:42:36,642
When we set our rhs
value here, we then

875
00:42:36,642 --> 00:42:39,890
have to put this node
back on the queue.

876
00:42:39,890 --> 00:42:44,460
So previously, s1 was
already on the queue,

877
00:42:44,460 --> 00:42:47,290
but the effect has to be
propagated to this node, which

878
00:42:47,290 --> 00:42:49,190
puts it on the
queue again, which

879
00:42:49,190 --> 00:42:52,010
would mean adjusting
the values, because rhs

880
00:42:52,010 --> 00:42:53,230
has been reduced by 1.

881
00:42:53,230 --> 00:42:56,580
Both the costs here
have been reduced by 1.

882
00:42:56,580 --> 00:42:58,930
Then we expand that node.

883
00:42:58,930 --> 00:43:00,660
We have an overconsistent case.

884
00:43:00,660 --> 00:43:03,310
We're setting g to
be equal to rhs.

885
00:43:03,310 --> 00:43:05,780
And finally, we've completed
our graph search problem.

886
00:43:09,120 --> 00:43:11,840
So now what we have to handle
is an additional complication

887
00:43:11,840 --> 00:43:14,260
that occurs between
several steps,

888
00:43:14,260 --> 00:43:17,810
tracking how our
heuristics have changed.

889
00:43:17,810 --> 00:43:20,600
So we want to carry
over prior queue, which

890
00:43:20,600 --> 00:43:22,420
was what was
mentioned previously,

891
00:43:22,420 --> 00:43:25,940
between our different searches
as our stock node moves.

892
00:43:25,940 --> 00:43:30,000
But the problem is that we're
moving from our start node

893
00:43:30,000 --> 00:43:31,790
to a different point.

894
00:43:31,790 --> 00:43:35,130
Recall that our heuristic value
is measured from any given

895
00:43:35,130 --> 00:43:36,960
node to the stock node.

896
00:43:36,960 --> 00:43:38,860
So when our stock
node has changed,

897
00:43:38,860 --> 00:43:42,400
the heuristic value is not
going to be the same value

898
00:43:42,400 --> 00:43:44,090
to the new stock mode.

899
00:43:44,090 --> 00:43:46,430
So we want what's
already on the queue

900
00:43:46,430 --> 00:43:49,556
to be comparable to
what is being computed

901
00:43:49,556 --> 00:43:51,930
for our new value, so we can
use the math and the algebra

902
00:43:51,930 --> 00:43:54,500
that we've already done.

903
00:43:54,500 --> 00:43:57,170
So how we handle this is we
introduce something called

904
00:43:57,170 --> 00:44:00,860
the key modifier, where we
move from the previous start

905
00:44:00,860 --> 00:44:05,030
node, which here I indicated as
s last, to the new start node,

906
00:44:05,030 --> 00:44:07,840
all the heuristics for
admissible heuristic

907
00:44:07,840 --> 00:44:11,370
are lowered by at most a
heuristic from the last start

908
00:44:11,370 --> 00:44:13,190
node to the new start node.

909
00:44:13,190 --> 00:44:16,100
So now, when we add
new nodes to the queue,

910
00:44:16,100 --> 00:44:19,220
instead of subtracting that
value from everything that's

911
00:44:19,220 --> 00:44:21,650
already on the queue,
what we do is we

912
00:44:21,650 --> 00:44:23,860
just increase for
all nodes that we

913
00:44:23,860 --> 00:44:29,840
put on the queue, their
values, by this new modifier.

914
00:44:29,840 --> 00:44:32,360
Because essentially when
we're taking off of the queue,

915
00:44:32,360 --> 00:44:35,670
all that matters is the relative
differences between the two.

916
00:44:35,670 --> 00:44:38,630
And so, instead of subtracting
a value from everything

917
00:44:38,630 --> 00:44:41,500
on the queue, adding that
value to all new values that

918
00:44:41,500 --> 00:44:43,840
occur on the queue will
achieve the same purpose.

919
00:44:43,840 --> 00:44:47,160
And so our key modifier
is initialized to 0

920
00:44:47,160 --> 00:44:50,600
at the start of the algorithm,
and is increased every time

921
00:44:50,600 --> 00:44:54,890
that we update our
start by this new value,

922
00:44:54,890 --> 00:44:56,607
by the heuristic
between the last start

923
00:44:56,607 --> 00:44:58,430
node and the new start node.

924
00:44:58,430 --> 00:45:00,890
And then we update
our total cost

925
00:45:00,890 --> 00:45:02,860
to include the edition
of the key modifier

926
00:45:02,860 --> 00:45:06,446
fire for our f1 term.

927
00:45:06,446 --> 00:45:11,980
So now we've covered all of
the D* Lite algorithm basics.

928
00:45:11,980 --> 00:45:13,840
And we can actually
walk through the slide

929
00:45:13,840 --> 00:45:16,490
that I showed you
before with the math.

930
00:45:16,490 --> 00:45:19,700
So when we start, we
initialize all of our g values

931
00:45:19,700 --> 00:45:21,500
to be equal to infinity,
and all of our rhs

932
00:45:21,500 --> 00:45:24,522
values to be equal to infinity,
except at the goal node.

933
00:45:24,522 --> 00:45:26,480
And that's to ensure that
we always have a node

934
00:45:26,480 --> 00:45:29,740
to start from our search.

935
00:45:29,740 --> 00:45:33,025
We best-first search from the
goal node to the start node

936
00:45:33,025 --> 00:45:35,080
until that's locally
consistent and expanded.

937
00:45:35,080 --> 00:45:36,780
We should now know
what that means.

938
00:45:36,780 --> 00:45:40,030
And that signal that we
have found our best path,

939
00:45:40,030 --> 00:45:42,920
we move according to
our movement rule,

940
00:45:42,920 --> 00:45:45,410
and then if any of our
edge costs have changed,

941
00:45:45,410 --> 00:45:49,070
we update our key modifier,
and update our rhs and queue

942
00:45:49,070 --> 00:45:52,790
positions for the source
nodes of change in edge costs,

943
00:45:52,790 --> 00:45:56,840
meaning the nodes from which
those edges originated.

944
00:45:56,840 --> 00:45:59,240
And then, we simply
repeat from two,

945
00:45:59,240 --> 00:46:02,272
following best-first
through the principles

946
00:46:02,272 --> 00:46:04,230
that I've walked you
through for overconsistent

947
00:46:04,230 --> 00:46:06,390
and underconsistent nodes.

948
00:46:06,390 --> 00:46:08,600
And as a reminder
down here, we always

949
00:46:08,600 --> 00:46:15,044
search or sort by this very
long couplet of two values.

950
00:46:15,044 --> 00:46:16,960
So now, we're going to
walk through an example

951
00:46:16,960 --> 00:46:18,361
of [INAUDIBLE].

952
00:46:23,440 --> 00:46:24,298
JESS NOSS: OK.

953
00:46:27,434 --> 00:46:30,530
So again, here's the pseudo code
that Ben was just presenting.

954
00:46:30,530 --> 00:46:34,870
And we're going to go through a
quick example with five nodes,

955
00:46:34,870 --> 00:46:37,957
so this isn't going to show all
of the properties of why you

956
00:46:37,957 --> 00:46:39,790
would want to use
incremental path planning,

957
00:46:39,790 --> 00:46:41,160
because when you
have only five nodes,

958
00:46:41,160 --> 00:46:43,760
you're going to end updating a
lot of them a lot of the time.

959
00:46:43,760 --> 00:46:45,640
But after the
example, you'll see

960
00:46:45,640 --> 00:46:48,510
how it makes more of a
difference on a larger graph.

961
00:46:48,510 --> 00:46:50,320
So in this graph,
we're going to have

962
00:46:50,320 --> 00:46:53,970
prior nodes that go A, B, C,
D, and G is our goal node.

963
00:46:53,970 --> 00:46:56,440
The robot will start
at node A, and its goal

964
00:46:56,440 --> 00:46:58,139
is to find the shortest
path to node G,

965
00:46:58,139 --> 00:46:59,555
except that the
graph might change

966
00:46:59,555 --> 00:47:01,180
as the robot goes
through, or the robot

967
00:47:01,180 --> 00:47:03,670
might gain new information.

968
00:47:03,670 --> 00:47:07,285
So initially, the robot thinks
that every node is available.

969
00:47:07,285 --> 00:47:09,160
And it knows that the
path lengths are all 1,

970
00:47:09,160 --> 00:47:12,676
except for from D to G,
the path length is 10.

971
00:47:12,676 --> 00:47:14,230
So this is a
bi-directional graph.

972
00:47:14,230 --> 00:47:16,610
Every edge goes both directions.

973
00:47:16,610 --> 00:47:18,580
And the heuristic,
as Ben described,

974
00:47:18,580 --> 00:47:22,832
will be with respect
to the start node.

975
00:47:22,832 --> 00:47:24,790
In this case, we'll just
say that the heuristic

976
00:47:24,790 --> 00:47:26,498
is the number of nodes
to the start node.

977
00:47:26,498 --> 00:47:29,825
So we're ignoring the fact
that this edge length is 10.

978
00:47:29,825 --> 00:47:34,510
So first, we'll initialize
all the g and rhs values.

979
00:47:34,510 --> 00:47:36,560
So we have infinity
at every node,

980
00:47:36,560 --> 00:47:40,462
except for the goal node,
which has an rhs value of 0.

981
00:47:40,462 --> 00:47:41,920
So this means that
all of the nodes

982
00:47:41,920 --> 00:47:43,420
are correctly
locally consistent,

983
00:47:43,420 --> 00:47:46,350
because their g and rhs values
are the same, except the goal

984
00:47:46,350 --> 00:47:47,270
node.

985
00:47:47,270 --> 00:47:49,630
So we put the goal
node on the queue

986
00:47:49,630 --> 00:47:54,228
using the formula that
is on Ben's slide, which

987
00:47:54,228 --> 00:47:56,700
I'll put here for reference.

988
00:47:56,700 --> 00:48:04,970
So the key is going
to be these two terms.

989
00:48:04,970 --> 00:48:11,380
First, we have the
minimum of g and rhs

990
00:48:11,380 --> 00:48:17,700
plus the heuristic value
plus the key modifier, which

991
00:48:17,700 --> 00:48:20,220
is initially 0.

992
00:48:20,220 --> 00:48:21,990
And then we have the
second term, which

993
00:48:21,990 --> 00:48:24,042
is just the minimum, g of rhs.

994
00:48:27,970 --> 00:48:30,580
So you can see how we
would get 3, 0 here,

995
00:48:30,580 --> 00:48:32,680
because the 3 comes
from the heuristic,

996
00:48:32,680 --> 00:48:35,205
and everything else was 0, so
we just added a bunch of 0's.

997
00:48:38,348 --> 00:48:40,930
So that's everything what
happens for the initialization.

998
00:48:40,930 --> 00:48:42,950
And now we have one
node on the queue.

999
00:48:42,950 --> 00:48:46,549
So we'll have to dequeue first.

1000
00:48:46,549 --> 00:48:47,590
This is an easy question.

1001
00:48:47,590 --> 00:48:48,170
AUDIENCE: The node on the queue.

1002
00:48:48,170 --> 00:48:49,711
JESS NOSS: The one
node on the queue.

1003
00:48:49,711 --> 00:48:53,400
So we dequeue G. So we're going
out to update its g value.

1004
00:48:53,400 --> 00:48:57,180
So as Ben said, the rhs value
is sort of a look ahead.

1005
00:48:57,180 --> 00:49:02,820
You can also think of rhs as
being the shortest path that we

1006
00:49:02,820 --> 00:49:05,750
found so far, and G is the
guaranteed shortest path

1007
00:49:05,750 --> 00:49:06,472
to that node.

1008
00:49:06,472 --> 00:49:07,430
So this one is trivial.

1009
00:49:07,430 --> 00:49:09,420
The shortest path from
this node to itself

1010
00:49:09,420 --> 00:49:13,140
has length 0, because
its the same node.

1011
00:49:13,140 --> 00:49:18,970
So now we're going to update the
rhs values and its neighbors.

1012
00:49:18,970 --> 00:49:22,440
So it has two neighbors,
C and D, and in each case,

1013
00:49:22,440 --> 00:49:25,640
the rhs value will
be the minimum--

1014
00:49:30,100 --> 00:49:35,420
this is not actual math, but its
the minimum of the edge cost,

1015
00:49:35,420 --> 00:49:39,230
the d value of a neighbor
plus the edge cost.

1016
00:49:42,590 --> 00:49:48,730
Which is the same thing as the
edge weight or the weight w.

1017
00:49:48,730 --> 00:49:53,180
So we rhs is 10 here, because
it's the distance from g to do,

1018
00:49:53,180 --> 00:49:57,010
and then it's one,
which is G to C.

1019
00:49:57,010 --> 00:49:59,684
So again, we can
calculate keys for those.

1020
00:49:59,684 --> 00:50:01,100
So now we'll dequeue
another node.

1021
00:50:01,100 --> 00:50:02,714
What should we dequeue next?

1022
00:50:05,636 --> 00:50:06,610
AUDIENCE: C.

1023
00:50:06,610 --> 00:50:09,450
JESS NOSS: C. Because
it has a smaller key.

1024
00:50:09,450 --> 00:50:11,400
So we dequeue C. We
update its g value.

1025
00:50:11,400 --> 00:50:16,340
So this is saying, now we know
that the shortest path to C

1026
00:50:16,340 --> 00:50:18,640
actually has length 1.

1027
00:50:18,640 --> 00:50:22,150
And we can also compare this
to A* star algorithm by drawing

1028
00:50:22,150 --> 00:50:23,760
a search tree.

1029
00:50:23,760 --> 00:50:27,150
So in the beginning, we had A*.

1030
00:50:34,010 --> 00:50:36,180
In the beginning,
we just had node G,

1031
00:50:36,180 --> 00:50:39,780
because we were searching from
G backward through the goal.

1032
00:50:39,780 --> 00:50:47,200
And then we expanded
that back to get C and D.

1033
00:50:47,200 --> 00:50:52,260
And then A* to keep track of the
path length to that node plus

1034
00:50:52,260 --> 00:50:53,490
the heuristic at each node.

1035
00:50:53,490 --> 00:50:58,920
So the pathway to C was
1 plus the heuristic 2,

1036
00:50:58,920 --> 00:51:00,325
give us the value of 3.

1037
00:51:00,325 --> 00:51:02,616
So if we go back a
moment, we can see--

1038
00:51:06,018 --> 00:51:06,990
is that OK?

1039
00:51:09,906 --> 00:51:12,900
So you can see that this 3 is
the same as the first point

1040
00:51:12,900 --> 00:51:16,520
of the key, and path
length one is the same

1041
00:51:16,520 --> 00:51:18,490
as the second part of the key.

1042
00:51:18,490 --> 00:51:22,680
And then similarly at D, we
have the path length, which

1043
00:51:22,680 --> 00:51:29,685
is 10, plus the 2, give us 12.

1044
00:51:29,685 --> 00:51:34,702
So this 12 is the 12 here, and
then the 10 is the path length.

1045
00:51:34,702 --> 00:51:37,760
And so you can see how if
we were doing this the A*,

1046
00:51:37,760 --> 00:51:39,912
you would also dequeue C next.

1047
00:51:46,030 --> 00:51:46,530
OK.

1048
00:51:46,530 --> 00:51:48,620
So now, we dequeue
C, and we're going

1049
00:51:48,620 --> 00:51:52,460
to update the rhs
and its neighbors.

1050
00:51:52,460 --> 00:51:53,520
So B is pretty simple.

1051
00:51:53,520 --> 00:51:54,990
Previously, it's
rhs was infinity.

1052
00:51:54,990 --> 00:51:58,190
Now the shortest path to
B that we've found so far

1053
00:51:58,190 --> 00:52:00,410
has length 2.

1054
00:52:00,410 --> 00:52:05,795
So here we have B was a path
length 2, plus a heuristic of 1

1055
00:52:05,795 --> 00:52:06,705
gives us 3.

1056
00:52:09,770 --> 00:52:10,746
But what about D?

1057
00:52:10,746 --> 00:52:12,120
Previously, its
rhs value was 10,

1058
00:52:12,120 --> 00:52:13,744
but do we now have
a shorter path to D,

1059
00:52:13,744 --> 00:52:15,810
given that we're expanding C?

1060
00:52:19,795 --> 00:52:21,170
Or is 10 still
the shortest path?

1061
00:52:23,850 --> 00:52:26,094
AUDIENCE: That is the
shortest path, through C.

1062
00:52:26,094 --> 00:52:26,760
JESS NOSS: Yeah.

1063
00:52:26,760 --> 00:52:27,970
So now that we're
expanding C, we

1064
00:52:27,970 --> 00:52:30,492
see that you can actually get
to D in only two units instead

1065
00:52:30,492 --> 00:52:30,991
of 10 units.

1066
00:52:30,991 --> 00:52:33,940
So that's better.

1067
00:52:33,940 --> 00:52:37,780
So we'll update
the rhs value to 2.

1068
00:52:37,780 --> 00:52:44,190
And that means that we're going
to change D's key accordingly.

1069
00:52:44,190 --> 00:52:47,640
And the way that maps to A* is,
so if you already have the path

1070
00:52:47,640 --> 00:52:51,890
length is 2, plus the
heuristic is 2, gives us 4.

1071
00:52:51,890 --> 00:52:54,692
And the fact that we're
changing the key to 4,

1072
00:52:54,692 --> 00:52:57,210
2 means that we're never
going to expand this D,

1073
00:52:57,210 --> 00:53:01,380
because A* uses an extended set,
so it would always expand this

1074
00:53:01,380 --> 00:53:04,810
D with a shorter
cost before this one.

1075
00:53:04,810 --> 00:53:07,430
So that's indicated
by-- removed by changing

1076
00:53:07,430 --> 00:53:10,400
the key on the queue.

1077
00:53:10,400 --> 00:53:12,734
So what do we do next?

1078
00:53:12,734 --> 00:53:14,150
Which node do we dequeue?

1079
00:53:17,454 --> 00:53:18,339
AUDIENCE: B.

1080
00:53:18,339 --> 00:53:18,880
JESS NOSS: B?

1081
00:53:18,880 --> 00:53:22,710
Yeah, because it
has the smaller key.

1082
00:53:22,710 --> 00:53:26,300
So we'll dequeue B. We'll update
its g value, which is again

1083
00:53:26,300 --> 00:53:31,940
saying that we know now that
the shortest path to B is 2.

1084
00:53:31,940 --> 00:53:35,480
So we dequeue B, and
then we expand it.

1085
00:53:35,480 --> 00:53:38,609
How many neighbors does B have?

1086
00:53:38,609 --> 00:53:40,472
AUDIENCE: Three.

1087
00:53:40,472 --> 00:53:41,180
JESS NOSS: Three.

1088
00:53:41,180 --> 00:53:45,090
So we're actually expanding to
all three of those neighbors.

1089
00:53:45,090 --> 00:53:51,488
We have A, C, and D.
Can we still see this?

1090
00:53:51,488 --> 00:53:52,464
We can.

1091
00:53:55,400 --> 00:53:57,150
So at A, it's pretty simple.

1092
00:53:57,150 --> 00:53:59,510
We have the rhs value is 3,
because it's the shortest

1093
00:53:59,510 --> 00:54:09,040
path through GCB to A. So that's
3, plus the heuristic 0 plus 3.

1094
00:54:09,040 --> 00:54:14,060
What about-- oh, see, it
doesn't change, because--

1095
00:54:14,060 --> 00:54:17,040
you could go through
B and back to C, but--

1096
00:54:17,040 --> 00:54:18,740
oh, C shouldn't
actually be on here.

1097
00:54:18,740 --> 00:54:20,114
Sorry.

1098
00:54:20,114 --> 00:54:21,530
C shouldn't be on
here, because we

1099
00:54:21,530 --> 00:54:22,902
don't want a loop in our path.

1100
00:54:26,710 --> 00:54:27,450
OK.

1101
00:54:27,450 --> 00:54:32,204
So at D, can we do better
than this path length of 2?

1102
00:54:35,508 --> 00:54:36,797
AUDIENCE: No.

1103
00:54:36,797 --> 00:54:37,380
JESS NOSS: No.

1104
00:54:37,380 --> 00:54:38,400
Because what happened
was originally,

1105
00:54:38,400 --> 00:54:40,140
we had the path length
of 10 through G,

1106
00:54:40,140 --> 00:54:42,516
and then we had the
path length of 2, 3 C,

1107
00:54:42,516 --> 00:54:44,140
and this path through
B is not as good.

1108
00:54:44,140 --> 00:54:50,230
So we're going to keep the
same rhs value and key at D.

1109
00:54:50,230 --> 00:54:53,920
And we can see that in a lot
of he A*, where we would have,

1110
00:54:53,920 --> 00:54:58,410
if we did do GCBD, we
would have a pathway of 3,

1111
00:54:58,410 --> 00:55:02,160
plus the heuristic of 2 is 5.

1112
00:55:02,160 --> 00:55:05,170
But we would never actually
want to expand that D,

1113
00:55:05,170 --> 00:55:08,470
because it's not as
good as this one.

1114
00:55:08,470 --> 00:55:09,330
OK.

1115
00:55:09,330 --> 00:55:10,750
Now what would we dequeue?

1116
00:55:10,750 --> 00:55:14,944
Looks like we have two nodes
in our queue currently.

1117
00:55:14,944 --> 00:55:16,402
JOE LEAVITT: A.

1118
00:55:16,402 --> 00:55:21,285
JESS NOSS: A. So we
dequeue A. That's

1119
00:55:21,285 --> 00:55:22,993
where we trying to
get to, because that's

1120
00:55:22,993 --> 00:55:24,440
the current start node.

1121
00:55:24,440 --> 00:55:26,910
So we set the g
value to 3, which

1122
00:55:26,910 --> 00:55:30,410
means that that's actually
the shortest path.

1123
00:55:30,410 --> 00:55:31,540
And we're done.

1124
00:55:31,540 --> 00:55:32,960
We found the shortest path.

1125
00:55:32,960 --> 00:55:35,897
Well, we're done, except that
the path might have to change.

1126
00:55:35,897 --> 00:55:37,980
So one thing you notice
here is that we never have

1127
00:55:37,980 --> 00:55:40,550
to actually set D's g value.

1128
00:55:40,550 --> 00:55:43,910
So it's possible, maybe there's
still a shorter path to D,

1129
00:55:43,910 --> 00:55:46,720
but it doesn't matter.

1130
00:55:46,720 --> 00:55:50,540
So now the robot will move
to its next best place, which

1131
00:55:50,540 --> 00:55:53,290
is B, and we'll update the
heuristics accordingly,

1132
00:55:53,290 --> 00:55:55,480
because this is
our new start node.

1133
00:55:55,480 --> 00:55:58,430
So now, again, our
heuristic is the number

1134
00:55:58,430 --> 00:56:01,460
of nodes away from
that green node.

1135
00:56:01,460 --> 00:56:05,850
And we're keeping the same
items on the queue from before.

1136
00:56:05,850 --> 00:56:09,890
But we also have to
update the key modifier.

1137
00:56:09,890 --> 00:56:11,750
So now what happens is
an obstacle appears.

1138
00:56:11,750 --> 00:56:15,102
So there's an obstacle at
C that the robot can now

1139
00:56:15,102 --> 00:56:16,780
see because it's moved
closer to node C,

1140
00:56:16,780 --> 00:56:19,320
but it couldn't see it before.

1141
00:56:19,320 --> 00:56:23,650
So now, we're going to indicate
that by keeping all the edges,

1142
00:56:23,650 --> 00:56:26,070
but changing their
rates to infinity.

1143
00:56:26,070 --> 00:56:28,350
And that means that the robot
could try to get to see,

1144
00:56:28,350 --> 00:56:30,840
but it wouldn't ever get there.

1145
00:56:30,840 --> 00:56:33,140
So what we do next in
the algorithm is we're

1146
00:56:33,140 --> 00:56:36,170
going to update all of
the rhs values associated

1147
00:56:36,170 --> 00:56:38,472
with the source
nodes of these edges.

1148
00:56:38,472 --> 00:56:40,430
And in this case, the
edges go both directions,

1149
00:56:40,430 --> 00:56:45,300
so all four of these nodes are
affected by the infinities.

1150
00:56:45,300 --> 00:56:48,800
So let's start with node C.
What would be its new rhs value,

1151
00:56:48,800 --> 00:56:54,590
given that rhs is the minimum
from one of its neighbors to it

1152
00:56:54,590 --> 00:56:58,540
of the g plus edge cost?

1153
00:56:58,540 --> 00:56:59,400
AUDIENCE: Infinity.

1154
00:56:59,400 --> 00:57:01,941
JESS NOSS: Infinity. because
all of the edge costs going to C

1155
00:57:01,941 --> 00:57:03,290
are infinity.

1156
00:57:03,290 --> 00:57:06,257
So what would be the key?

1157
00:57:06,257 --> 00:57:07,840
The tricky part here
is the first part

1158
00:57:07,840 --> 00:57:09,820
of the key is the
minimum of g and rhs.

1159
00:57:16,184 --> 00:57:20,534
So the minimum of
g or rhs is what?

1160
00:57:20,534 --> 00:57:23,118
AUDIENCE: So it'd be 3, 1?

1161
00:57:23,118 --> 00:57:23,986
Yeah.

1162
00:57:23,986 --> 00:57:28,940
JESS NOSS: Yeah, 3, 1, because
the minimum of g and rhs

1163
00:57:28,940 --> 00:57:30,020
is g, which is 1.

1164
00:57:30,020 --> 00:57:33,870
The heuristic is 1, and
the key modifier is 1.

1165
00:57:33,870 --> 00:57:34,370
OK.

1166
00:57:34,370 --> 00:57:35,530
How about node B?

1167
00:57:35,530 --> 00:57:36,650
What's the new rhs value?

1168
00:57:40,350 --> 00:57:43,077
So it has a neighbor
here with g as infinity,

1169
00:57:43,077 --> 00:57:45,118
a neighbor here of g is
1, except the edge length

1170
00:57:45,118 --> 00:57:46,454
is infinity.

1171
00:57:46,454 --> 00:57:48,550
And then this guy with g is 3.

1172
00:57:52,861 --> 00:57:55,740
AUDIENCE: So it's infinity, 4?

1173
00:57:55,740 --> 00:57:56,810
JESS NOSS: 4.

1174
00:57:56,810 --> 00:57:58,720
Yeah, because
currently, we still

1175
00:57:58,720 --> 00:58:02,100
think that you can
get to A and B units.

1176
00:58:02,100 --> 00:58:04,400
So this is actually
going to be 4.

1177
00:58:04,400 --> 00:58:06,910
So this is one of the
sillier things that D* does,

1178
00:58:06,910 --> 00:58:08,670
but luckily, it doesn't
do this very much,

1179
00:58:08,670 --> 00:58:12,750
so it doesn't actually
really cost too much time.

1180
00:58:12,750 --> 00:58:14,870
And then how about D?

1181
00:58:14,870 --> 00:58:16,904
What would be the new rhs value?

1182
00:58:25,860 --> 00:58:27,650
AUDIENCE: 5, 3?

1183
00:58:27,650 --> 00:58:29,940
rhs.

1184
00:58:29,940 --> 00:58:32,480
JESS NOSS: So it has
been 3 neighbors.

1185
00:58:32,480 --> 00:58:35,500
This edge length is infinity
is useless, so that's useless.

1186
00:58:35,500 --> 00:58:38,022
This one has g of 0
plus 10 would be 10.

1187
00:58:38,022 --> 00:58:39,230
Yes, so this is the best one.

1188
00:58:39,230 --> 00:58:42,490
G is 2, plus edge length of 1.

1189
00:58:42,490 --> 00:58:44,285
So that would give us 3.

1190
00:58:44,285 --> 00:58:45,510
So now what do we dequeue?

1191
00:58:50,870 --> 00:58:51,460
AUDIENCE: C?

1192
00:58:51,460 --> 00:58:54,084
JESS NOSS: C. So this is one of
those tie breaking cases, where

1193
00:58:54,084 --> 00:58:57,010
both of them have
the first value is 3,

1194
00:58:57,010 --> 00:58:59,100
but C has a better
second value in its key,

1195
00:58:59,100 --> 00:59:07,364
so we'll dequeue C. Is this
over or under consistent?

1196
00:59:07,364 --> 00:59:09,980
So the definitions of
over and under consistent

1197
00:59:09,980 --> 00:59:20,360
that we had earlier, we had
if g is greater than rhs,

1198
00:59:20,360 --> 00:59:24,950
then that's
overconsistent, and we

1199
00:59:24,950 --> 00:59:27,860
set g equal to rhs
when we're updating g.

1200
00:59:27,860 --> 00:59:30,630
So that was the case
that we've seen so far.

1201
00:59:30,630 --> 00:59:37,960
But then if g is less than
rhs, it's underconsistent,

1202
00:59:37,960 --> 00:59:40,180
and we set g to infinity.

1203
00:59:40,180 --> 00:59:43,790
So what is this?

1204
00:59:43,790 --> 00:59:45,260
AUDIENCE: Underconsistent.

1205
00:59:45,260 --> 00:59:46,540
JESS NOSS: Underconsistent.

1206
00:59:46,540 --> 00:59:48,127
So the reason
intuitively why we're

1207
00:59:48,127 --> 00:59:49,930
going to set g to
infinity is because we're

1208
00:59:49,930 --> 00:59:52,110
saying there's some sort
of contradiction here.

1209
00:59:52,110 --> 00:59:53,610
This one doesn't make any sense.

1210
00:59:53,610 --> 00:59:55,310
It's less than the
rhs value, so we're

1211
00:59:55,310 --> 00:59:58,760
going to just reset g and sort
of start over with this node.

1212
00:59:58,760 --> 01:00:00,812
So we set that to infinity.

1213
01:00:00,812 --> 01:00:02,270
And in this case,
we don't actually

1214
01:00:02,270 --> 01:00:03,765
need to add the node
back onto the queue,

1215
01:00:03,765 --> 01:00:04,931
because it's now consistent.

1216
01:00:04,931 --> 01:00:08,730
It's now locally consistent.

1217
01:00:08,730 --> 01:00:11,945
So next, we'll dequeue the one
with the smallest key, which

1218
01:00:11,945 --> 01:00:17,162
is B. B is also underconsistent,
so we're going to set its g

1219
01:00:17,162 --> 01:00:18,120
value back to infinity.

1220
01:00:18,120 --> 01:00:20,980
So this is sort of resetting
the weird thing we did earlier

1221
01:00:20,980 --> 01:00:23,830
that didn't make any sense.

1222
01:00:23,830 --> 01:00:27,830
We're going to update its
neighbors' rhs values.

1223
01:00:27,830 --> 01:00:30,420
So what would be the
new rhs value for A,

1224
01:00:30,420 --> 01:00:36,745
now that we know that we
don't actually have a path B?

1225
01:00:36,745 --> 01:00:37,695
AUDIENCE: Infinity?

1226
01:00:37,695 --> 01:00:38,528
JESS NOSS: Infinity.

1227
01:00:38,528 --> 01:00:40,550
Yeah.

1228
01:00:40,550 --> 01:00:42,770
And then, how about
rhs value to D?

1229
01:00:42,770 --> 01:00:44,885
Because currently,
D got its rhs value

1230
01:00:44,885 --> 01:00:53,602
by going from A to B to D.

1231
01:00:53,602 --> 01:00:54,600
AUDIENCE: 10.

1232
01:00:54,600 --> 01:00:55,260
JESS NOSS: 10.

1233
01:00:55,260 --> 01:00:57,950
'Cause now the only way--

1234
01:00:57,950 --> 01:00:58,710
rhs is 10.

1235
01:00:58,710 --> 01:01:03,580
The key would have
that one also.

1236
01:01:03,580 --> 01:01:05,454
Is that right?

1237
01:01:05,454 --> 01:01:06,060
Yeah.

1238
01:01:06,060 --> 01:01:08,370
Because currently,
the best path to D,

1239
01:01:08,370 --> 01:01:11,860
and actually, the best
path to D you can see

1240
01:01:11,860 --> 01:01:16,936
is from G to D, because you
can't go through C anymore.

1241
01:01:16,936 --> 01:01:19,310
So now, we'll dequeue node A,
because it has the smallest

1242
01:01:19,310 --> 01:01:21,130
key.

1243
01:01:21,130 --> 01:01:26,020
It was also underconsistent,
so we would set G to infinity.

1244
01:01:26,020 --> 01:01:29,410
So we'll propagate
that to its neighbors.

1245
01:01:29,410 --> 01:01:35,510
And if we continue doing
this, then we find--

1246
01:01:35,510 --> 01:01:37,980
eventually, we dequeue this
node, and in this case,

1247
01:01:37,980 --> 01:01:40,470
we had to dequeue
everything, so now the node's

1248
01:01:40,470 --> 01:01:42,420
are locally consistent.

1249
01:01:42,420 --> 01:01:44,920
So what's the best path that
we've found from B to the goal?

1250
01:01:50,860 --> 01:01:51,990
AUDIENCE: B, D, G?

1251
01:01:51,990 --> 01:01:53,448
JESS NOSS: B, D,
G. And that's also

1252
01:01:53,448 --> 01:01:55,308
the only path in this case.

1253
01:01:55,308 --> 01:01:58,481
And we had to update
these neighbors.

1254
01:01:58,481 --> 01:01:58,980
OK.

1255
01:01:58,980 --> 01:02:00,530
So here's our new shortest path.

1256
01:02:00,530 --> 01:02:03,690
Now, the robot will
move to node D. But now,

1257
01:02:03,690 --> 01:02:04,690
what's going to happen--

1258
01:02:04,690 --> 01:02:06,370
I think we updated
our heuristics.

1259
01:02:06,370 --> 01:02:08,880
But now what happens is it turns
out it's a moving obstacle,

1260
01:02:08,880 --> 01:02:10,880
and it starts
following the robot.

1261
01:02:10,880 --> 01:02:13,230
So now the obstacle
has moved to there.

1262
01:02:13,230 --> 01:02:15,750
So as humans, we can see
that, obviously, the shortest

1263
01:02:15,750 --> 01:02:17,580
path is through C,
but the algorithm

1264
01:02:17,580 --> 01:02:19,949
doesn't know this yet.

1265
01:02:19,949 --> 01:02:20,990
So what would we do next?

1266
01:02:26,990 --> 01:02:29,790
So we just updated
our edge weights.

1267
01:02:29,790 --> 01:02:31,890
So now all the edges
to B are infinity.

1268
01:02:31,890 --> 01:02:34,720
But some of the edges to
C are no longer infinity.

1269
01:02:34,720 --> 01:02:39,760
So we need to update the
rhs values accordingly.

1270
01:02:39,760 --> 01:02:41,970
So we'll update all
of the rhs values

1271
01:02:41,970 --> 01:02:44,580
that were affected
by this change.

1272
01:02:44,580 --> 01:02:46,800
In this case, G's
rhs value never

1273
01:02:46,800 --> 01:02:50,100
changes because the distance
from G to itself is always 0.

1274
01:02:50,100 --> 01:02:53,810
And this one doesn't
change because we still

1275
01:02:53,810 --> 01:02:55,070
use the G value here.

1276
01:02:58,910 --> 01:02:59,410
OK.

1277
01:02:59,410 --> 01:03:01,451
So now again, we can start
dequeuing and planning

1278
01:03:01,451 --> 01:03:03,644
a new path, so we'll
dequeue this node.

1279
01:03:03,644 --> 01:03:06,760
We'll propagate
to its neighbors.

1280
01:03:06,760 --> 01:03:08,870
Dequeue that one.

1281
01:03:08,870 --> 01:03:10,620
Update the rhs values.

1282
01:03:10,620 --> 01:03:13,908
But in this case, we don't--

1283
01:03:13,908 --> 01:03:16,685
let me go back step.

1284
01:03:16,685 --> 01:03:18,810
So we dequeue this node,
and we update its G value,

1285
01:03:18,810 --> 01:03:23,910
but we don't actually need
to continue dequeuing things.

1286
01:03:23,910 --> 01:03:26,960
We should-- oh, this rhs
value should be updated.

1287
01:03:26,960 --> 01:03:28,730
Oh, it is updated.

1288
01:03:28,730 --> 01:03:32,680
Why is this rhs infinity?

1289
01:03:32,680 --> 01:03:34,570
AUDIENCE: All edges
to it are infinity.

1290
01:03:34,570 --> 01:03:34,940
JESS NOSS: Yeah.

1291
01:03:34,940 --> 01:03:36,148
All edges to it are infinity.

1292
01:03:36,148 --> 01:03:37,405
You can't get there.

1293
01:03:37,405 --> 01:03:41,424
But we don't actually have
to bother expanding B,

1294
01:03:41,424 --> 01:03:43,340
because we've already
found the shortest path.

1295
01:03:43,340 --> 01:03:46,319
So this gives you sort of a hint
at when D* might be efficient.

1296
01:03:46,319 --> 01:03:48,360
Because even if there were
a whole bunch of nodes

1297
01:03:48,360 --> 01:03:50,234
over there, I wouldn't
need to consider them,

1298
01:03:50,234 --> 01:03:52,360
because we found
the shortest path.

1299
01:03:52,360 --> 01:03:57,040
So now, the robot
can move up to C.

1300
01:03:57,040 --> 01:03:59,500
And let's say the obstacle
chases the robot again.

1301
01:03:59,500 --> 01:04:04,221
Do we need to find a new path?

1302
01:04:04,221 --> 01:04:04,720
No.

1303
01:04:04,720 --> 01:04:06,886
So intuitively, we don't,
because the obstacle's not

1304
01:04:06,886 --> 01:04:09,330
in the way of our
current best path.

1305
01:04:09,330 --> 01:04:11,140
But we can also
see algorithmically

1306
01:04:11,140 --> 01:04:12,895
why that's the case.

1307
01:04:12,895 --> 01:04:15,020
So first of all, we'll
update all of our rhs values

1308
01:04:15,020 --> 01:04:16,810
accordingly, because
we've changed

1309
01:04:16,810 --> 01:04:18,120
all of the edge weights.

1310
01:04:20,788 --> 01:04:24,790
But the reason algorithmically
why we don't actually

1311
01:04:24,790 --> 01:04:27,190
need to consider any
of these change values

1312
01:04:27,190 --> 01:04:31,180
is because this node has
the smallest key, which

1313
01:04:31,180 --> 01:04:34,840
means that this is the node
that has the shortest path to G

1314
01:04:34,840 --> 01:04:36,420
anyway.

1315
01:04:36,420 --> 01:04:38,590
It is smaller key than
all the other nodes

1316
01:04:38,590 --> 01:04:39,810
that are on the queue.

1317
01:04:39,810 --> 01:04:41,470
So dequeuing things
wouldn't actually

1318
01:04:41,470 --> 01:04:44,796
help us find the shorter path.

1319
01:04:44,796 --> 01:04:50,008
So the robot can move to the
goal, and we've succeeded.

1320
01:04:50,008 --> 01:04:51,960
Any questions on that?

1321
01:04:56,695 --> 01:04:57,195
OK.

1322
01:04:57,195 --> 01:04:59,840
So now then we'll
talk about sometimes

1323
01:04:59,840 --> 01:05:02,500
when you would want to use
incremental path planning,

1324
01:05:02,500 --> 01:05:04,382
other than this five node graph.

1325
01:05:06,392 --> 01:05:07,350
ERLEND HARBITZ: Thanks.

1326
01:05:07,350 --> 01:05:11,395
So as Jess said, there
are sometimes good reasons

1327
01:05:11,395 --> 01:05:12,770
to use incremental
path planning,

1328
01:05:12,770 --> 01:05:14,353
but there are also
good reasons not to

1329
01:05:14,353 --> 01:05:15,762
in certain circumstances.

1330
01:05:15,762 --> 01:05:17,470
In certain circumstances,
we can actually

1331
01:05:17,470 --> 01:05:20,180
formulate graph problems,
where a problem is worse

1332
01:05:20,180 --> 01:05:22,640
by using incremental planning.

1333
01:05:22,640 --> 01:05:27,050
Remember that I said that D*
Lite puts any node on the queue

1334
01:05:27,050 --> 01:05:28,540
at most twice.

1335
01:05:28,540 --> 01:05:32,140
And so we can compare that to
A*, or something like that,

1336
01:05:32,140 --> 01:05:35,660
that does full replanning
whenever a search occurs.

1337
01:05:35,660 --> 01:05:39,110
That pus every node on the
search queue at most once.

1338
01:05:39,110 --> 01:05:43,730
So D* Lite is most beneficial
when putting the nodes

1339
01:05:43,730 --> 01:05:47,116
on the queue, potentially
more than once,

1340
01:05:47,116 --> 01:05:50,220
leads to less nodes
being examined overall.

1341
01:05:53,300 --> 01:05:57,710
So here is essentially
what I just said.

1342
01:05:57,710 --> 01:06:03,440
So A* might perform better for
certain problems if we only

1343
01:06:03,440 --> 01:06:08,060
have to consider a small
amount of nodes anyway.

1344
01:06:08,060 --> 01:06:11,030
So putting nodes
on the graph twice

1345
01:06:11,030 --> 01:06:13,970
would actually lead
to more expansions.

1346
01:06:13,970 --> 01:06:19,040
And so, to get at an intuition
of why that might occur

1347
01:06:19,040 --> 01:06:22,710
if changes are close
to the start node,

1348
01:06:22,710 --> 01:06:25,130
I want to show you
an example here.

1349
01:06:25,130 --> 01:06:27,410
So here, we're actually
showing an example that's

1350
01:06:27,410 --> 01:06:29,340
derived from [INAUDIBLE].

1351
01:06:29,340 --> 01:06:31,832
So we're searching forwards
here from the start node

1352
01:06:31,832 --> 01:06:32,540
to the goal node.

1353
01:06:32,540 --> 01:06:34,560
And that's just a
different reformulation,

1354
01:06:34,560 --> 01:06:37,730
so don't let that
confuse you too much.

1355
01:06:37,730 --> 01:06:41,330
When we walk forward through
A* with a good heuristic,

1356
01:06:41,330 --> 01:06:44,168
we might move directly from
the start node to goal node,

1357
01:06:44,168 --> 01:06:45,920
and find that path.

1358
01:06:45,920 --> 01:06:49,600
Now, if a change occurs
somewhere upstream

1359
01:06:49,600 --> 01:06:52,520
of the start node,
when we replanning,

1360
01:06:52,520 --> 01:06:57,320
we essentially have to
replan the sections that

1361
01:06:57,320 --> 01:07:00,890
are beyond the change
that's occurred.

1362
01:07:00,890 --> 01:07:05,570
So everything that's
here, to an extent, it's

1363
01:07:05,570 --> 01:07:08,540
relatively insensitive
to this change.

1364
01:07:08,540 --> 01:07:11,244
It could occur that
we would have to take

1365
01:07:11,244 --> 01:07:12,410
an entirely different route.

1366
01:07:12,410 --> 01:07:15,110
But for the most
part, it's likely

1367
01:07:15,110 --> 01:07:18,570
that changes are going to
couple to our existing path

1368
01:07:18,570 --> 01:07:19,836
and so forth.

1369
01:07:19,836 --> 01:07:22,700
And so we have an
efficient search

1370
01:07:22,700 --> 01:07:27,500
that will just go
around from the path

1371
01:07:27,500 --> 01:07:31,140
that we've already
planned to our goal node.

1372
01:07:31,140 --> 01:07:33,200
So in this sense,
we have an example

1373
01:07:33,200 --> 01:07:38,960
where incremental path planning
would be very efficient.

1374
01:07:38,960 --> 01:07:41,180
In this case, which
is a different one,

1375
01:07:41,180 --> 01:07:44,660
we introduce an obstacle
very near our start node.

1376
01:07:44,660 --> 01:07:49,320
This means that most of our
existing path is to charge.

1377
01:07:49,320 --> 01:07:55,880
So where we start out from our
start node to our goal node,

1378
01:07:55,880 --> 01:07:59,160
we have to do a lot of
recomputation from scratch.

1379
01:07:59,160 --> 01:08:02,007
And so an incremental
searching algorithm

1380
01:08:02,007 --> 01:08:04,340
is going to have to undo all
searches that we've already

1381
01:08:04,340 --> 01:08:07,940
done, and then search
through almost the entirety

1382
01:08:07,940 --> 01:08:09,890
of the graph again.

1383
01:08:09,890 --> 01:08:13,960
So intuitively, in this case,
just restarting with A* from

1384
01:08:13,960 --> 01:08:17,420
a blank slate, which we
would search directly down

1385
01:08:17,420 --> 01:08:20,770
to the goal, instead of doing
reparations that would send us

1386
01:08:20,770 --> 01:08:25,660
in one path, would be a
better idea in this case.

1387
01:08:25,660 --> 01:08:29,250
So since we're powering
from the start to the goal,

1388
01:08:29,250 --> 01:08:31,490
in this example
that we're showing,

1389
01:08:31,490 --> 01:08:34,910
it's worse to use incremental
path planning algorithms

1390
01:08:34,910 --> 01:08:38,720
when your changes happen
near your start node.

1391
01:08:38,720 --> 01:08:43,399
For something like D* Lite, we
search back from the goal node

1392
01:08:43,399 --> 01:08:47,130
to the start node, it's
better if changes occur nearer

1393
01:08:47,130 --> 01:08:50,720
to the start node, which for our
vehicles that we normally see

1394
01:08:50,720 --> 01:08:53,160
that have limited
sensor horizons,

1395
01:08:53,160 --> 01:08:55,740
is what's going to
occur in the most case.

1396
01:08:55,740 --> 01:08:58,455
So in those type of situations,
incremental path planning

1397
01:08:58,455 --> 01:09:00,842
is going to be very efficient.

1398
01:09:00,842 --> 01:09:03,050
AUDIENCE: So I understand
how this could be a problem

1399
01:09:03,050 --> 01:09:07,122
for LPA* and D* Lite over
finding a single path,

1400
01:09:07,122 --> 01:09:10,670
where you have like a
single source or destination

1401
01:09:10,670 --> 01:09:11,740
or something like that.

1402
01:09:11,740 --> 01:09:14,300
But what if it's incremental,
all-pairs shortest-paths,

1403
01:09:14,300 --> 01:09:17,479
where you've computed--

1404
01:09:17,479 --> 01:09:19,750
I'd assume you wouldn't
experience the same problems

1405
01:09:19,750 --> 01:09:22,490
if your algorithm was computing
choice paths between all pairs

1406
01:09:22,490 --> 01:09:23,759
of nodes indirect.

1407
01:09:23,759 --> 01:09:24,634
ERLEND HARBITZ: Yeah.

1408
01:09:24,634 --> 01:09:25,882
It already exists.

1409
01:09:29,189 --> 01:09:30,160
Well, that depends.

1410
01:09:33,720 --> 01:09:39,470
When you're redoing
recomputation,

1411
01:09:39,470 --> 01:09:41,760
between all the
nodes in the path,

1412
01:09:41,760 --> 01:09:45,726
it depends if our
obstacle has changed

1413
01:09:45,726 --> 01:09:47,850
[INAUDIBLE] of something
there that we to conceive.

1414
01:09:47,850 --> 01:09:49,790
And by that I mean
imagining this graph

1415
01:09:49,790 --> 01:09:53,540
we've computed, D, the
paths and path costs,

1416
01:09:53,540 --> 01:09:57,060
to get from every single node
to every single other node.

1417
01:09:57,060 --> 01:09:59,520
Now, if introducing
an obstacle here

1418
01:09:59,520 --> 01:10:02,940
only changed the path cost
to get from our stock node

1419
01:10:02,940 --> 01:10:06,580
to our goal node
from example, then

1420
01:10:06,580 --> 01:10:10,680
replanning that specific
pair of paths from start node

1421
01:10:10,680 --> 01:10:12,720
to the goal node
would use data that

1422
01:10:12,720 --> 01:10:15,000
already exists from the
other nodes to the goal node.

1423
01:10:15,000 --> 01:10:16,155
We already have that.

1424
01:10:16,155 --> 01:10:18,450
But if introducing an
obstacle would cause changes

1425
01:10:18,450 --> 01:10:20,460
throughout the
entire graph, then we

1426
01:10:20,460 --> 01:10:23,090
can potentially
create what would

1427
01:10:23,090 --> 01:10:26,170
have to be a very well-connected
graph, where again,

1428
01:10:26,170 --> 01:10:28,420
incremental path planning
would probably perform worse

1429
01:10:28,420 --> 01:10:28,962
in that case.

1430
01:10:28,962 --> 01:10:29,586
AUDIENCE: Yeah.

1431
01:10:29,586 --> 01:10:32,255
So you could still end up in
the same situation [INAUDIBLE]..

1432
01:10:35,580 --> 01:10:37,080
ERLEND HARBITZ: Now
these algorithms

1433
01:10:37,080 --> 01:10:40,050
are interesting, because they
allow for some extensions that

1434
01:10:40,050 --> 01:10:45,850
allow us to do some some very
different things quite easily.

1435
01:10:45,850 --> 01:10:49,080
One example is greedy
mapping, in terms

1436
01:10:49,080 --> 01:10:51,960
of how can we design
algorithm using

1437
01:10:51,960 --> 01:10:54,100
what we know about
incremental path planning,

1438
01:10:54,100 --> 01:10:57,810
to hit every single
node on our graph.

1439
01:10:57,810 --> 01:11:02,080
And what we do is then we
introduce a faux goal node,

1440
01:11:02,080 --> 01:11:06,610
essentially a node that doesn't
exist that we can never reach.

1441
01:11:06,610 --> 01:11:12,570
And we add to our
initial beliefs,

1442
01:11:12,570 --> 01:11:15,510
our initial understanding
of the graph

1443
01:11:15,510 --> 01:11:18,670
connections between all
nodes to this faux goal node.

1444
01:11:18,670 --> 01:11:22,230
Now, it's false in the sense
that whenever we move our start

1445
01:11:22,230 --> 01:11:25,530
node to one of the nodes
that we've examined before,

1446
01:11:25,530 --> 01:11:27,930
what happens in terms of
the changes of the edge

1447
01:11:27,930 --> 01:11:33,415
costs is removing that edge that
connected that node to the goal

1448
01:11:33,415 --> 01:11:34,200
node.

1449
01:11:34,200 --> 01:11:36,090
So what this means
in principle is

1450
01:11:36,090 --> 01:11:38,470
that our vehicle,
our poor vehicle,

1451
01:11:38,470 --> 01:11:40,820
can never reach the goal node.

1452
01:11:40,820 --> 01:11:42,330
Whenever it reaches
a node that it

1453
01:11:42,330 --> 01:11:44,700
thinks it can get to
the goal node from,

1454
01:11:44,700 --> 01:11:45,960
that path disappears.

1455
01:11:45,960 --> 01:11:49,680
But what that causes it to
do is when it's replanning,

1456
01:11:49,680 --> 01:11:53,460
it finds a path to a node that
we haven't visited before,

1457
01:11:53,460 --> 01:11:56,702
because it thinks there is a
path that leads from that node

1458
01:11:56,702 --> 01:11:57,410
to the goal node.

1459
01:11:57,410 --> 01:11:59,025
And so this means
that it's efficient,

1460
01:11:59,025 --> 01:12:03,160
but replanning a root from his
current understanding to a node

1461
01:12:03,160 --> 01:12:04,990
that it hasn't examined before.

1462
01:12:04,990 --> 01:12:09,930
And so it can apply something
like the D* Lite or LPA* with

1463
01:12:09,930 --> 01:12:14,430
this formalism in order to
search through the graph in its

1464
01:12:14,430 --> 01:12:19,110
entirety and make sure that in
a greedily efficient manner,

1465
01:12:19,110 --> 01:12:23,590
we hit every single
node in the graph.

1466
01:12:23,590 --> 01:12:26,700
AUDIENCE: Is this
[INAUDIBLE] TSP

1467
01:12:26,700 --> 01:12:29,300
problem, or the traveling
solutions problem?

1468
01:12:29,300 --> 01:12:32,955
ERLEND HARBITZ: I'm not very
familiar with the TSP problem.

1469
01:12:32,955 --> 01:12:34,320
AUDIENCE: You try to--

1470
01:12:34,320 --> 01:12:37,260
and you try to cover
all the Cs, and try

1471
01:12:37,260 --> 01:12:39,980
to come up with a route
that is the most efficient,

1472
01:12:39,980 --> 01:12:43,639
and try to visit every
known C value once.

1473
01:12:43,639 --> 01:12:44,430
ERLEND HARBITZ: OK.

1474
01:12:44,430 --> 01:12:45,388
Then in that case, yes.

1475
01:12:45,388 --> 01:12:48,610
It's not going to be
globally efficient.

1476
01:12:48,610 --> 01:12:50,910
As you said, it's going to
be an approximation for it.

1477
01:12:50,910 --> 01:12:52,970
In a sense, from each
node that you visited,

1478
01:12:52,970 --> 01:12:56,030
you're searching for
the next best choice.

1479
01:12:56,030 --> 01:12:58,540
So it's possible you
can corner yourself.

1480
01:12:58,540 --> 01:13:01,884
But it's a way that we use
what we already implemented

1481
01:13:01,884 --> 01:13:03,300
and the methods
that we've already

1482
01:13:03,300 --> 01:13:06,430
established to tackle a
different type of problem.

1483
01:13:06,430 --> 01:13:08,960
AUDIENCE: And all this
[INAUDIBLE] usually if

1484
01:13:08,960 --> 01:13:12,962
you have a strategy which is
visit the closest unvisited

1485
01:13:12,962 --> 01:13:15,810
node, instead of making
this whole incremental path

1486
01:13:15,810 --> 01:13:17,130
from this study.

1487
01:13:17,130 --> 01:13:21,000
If your policy is, always move
to the closest unvisited node,

1488
01:13:21,000 --> 01:13:23,753
how worse does that
get compared to this?

1489
01:13:28,850 --> 01:13:31,222
ERLEND HARBITZ: I think it
would behave quite similarly.

1490
01:13:31,222 --> 01:13:32,680
What we're just
trying to show here

1491
01:13:32,680 --> 01:13:36,560
is that we can implement
that problem as an extension

1492
01:13:36,560 --> 01:13:37,360
if this happened.

1493
01:13:37,360 --> 01:13:40,950
It may be an efficient
way to do it.

1494
01:13:40,950 --> 01:13:43,960
AUDIENCE: [INAUDIBLE]
probably like, zigzag.

1495
01:13:43,960 --> 01:13:46,680
ERLEND HARBITZ:
Also, if you're just

1496
01:13:46,680 --> 01:13:51,060
going to the nearest
connected node,

1497
01:13:51,060 --> 01:13:55,440
if you corner yourself
as we showed here,

1498
01:13:55,440 --> 01:13:57,200
then you could potentially
run into issues

1499
01:13:57,200 --> 01:14:00,180
if your algorithm doesn't then
do an extended search for it.

1500
01:14:00,180 --> 01:14:03,080
If it only considers nodes
in its immediate neighborhood

1501
01:14:03,080 --> 01:14:05,470
that hasn't visited,
once it corners itself

1502
01:14:05,470 --> 01:14:07,209
and doesn't know where to go.

1503
01:14:07,209 --> 01:14:09,000
AUDIENCE: Just keep
going through the graph

1504
01:14:09,000 --> 01:14:11,460
until you hit the
next inhibited node,

1505
01:14:11,460 --> 01:14:15,400
assuming that you know how many
invocations you have to do.

1506
01:14:15,400 --> 01:14:18,100
AUDIENCE: Sort of like the
random walk type approach?

1507
01:14:18,100 --> 01:14:20,683
AUDIENCE: Well, I mean, kind of
like the greedy version of it.

1508
01:14:20,683 --> 01:14:22,320
You kind of start
from a node, and then

1509
01:14:22,320 --> 01:14:23,820
you keep searching,
you'll see, what

1510
01:14:23,820 --> 01:14:27,690
is the next unvisited
node, and you pick that.

1511
01:14:27,690 --> 01:14:28,700
Kind of like--

1512
01:14:28,700 --> 01:14:32,140
[INAUDIBLE] some you see
what you have covered.

1513
01:14:32,140 --> 01:14:33,960
You see what's the
next best move.

1514
01:14:33,960 --> 01:14:35,486
You make that move.

1515
01:14:35,486 --> 01:14:36,826
And you're done.

1516
01:14:39,950 --> 01:14:50,230
[INAUDIBLE] So you probably
don't have this [INAUDIBLE]..

1517
01:14:57,587 --> 01:14:59,170
ERLEND HARBITZ: This
basically ensures

1518
01:14:59,170 --> 01:15:00,961
you're going to find
the shortest path back

1519
01:15:00,961 --> 01:15:02,603
to the next unvisited
node, I guess.

1520
01:15:08,990 --> 01:15:10,726
We also can use these
incremental path

1521
01:15:10,726 --> 01:15:14,540
planning algorithms
for anytime planners.

1522
01:15:14,540 --> 01:15:16,350
And the benefit of
an anytime planner

1523
01:15:16,350 --> 01:15:22,730
is that we quickly reach a
plan which is suboptimal.

1524
01:15:22,730 --> 01:15:26,660
And then from there, we
are repairing this plan

1525
01:15:26,660 --> 01:15:28,480
to approach optimality.

1526
01:15:28,480 --> 01:15:31,430
And this is useful
in the real world

1527
01:15:31,430 --> 01:15:34,100
where you want to get a
solution that comes out quickly,

1528
01:15:34,100 --> 01:15:37,050
and we want to start
along our solution,

1529
01:15:37,050 --> 01:15:38,900
and then repair it as we go.

1530
01:15:38,900 --> 01:15:41,390
And once we start it, we
need o save some more time

1531
01:15:41,390 --> 01:15:44,707
if your reparations might lead
to small changes and that type

1532
01:15:44,707 --> 01:15:45,206
of thing.

1533
01:15:45,206 --> 01:15:47,630
And this can be handled
quite efficiently

1534
01:15:47,630 --> 01:15:50,450
using an incremental
path planning.

1535
01:15:50,450 --> 01:15:56,500
So what happens is we apply
something, idea, called

1536
01:15:56,500 --> 01:15:57,940
inflated heuristics.

1537
01:15:57,940 --> 01:16:00,830
And so these inflated
heuristics are essentially

1538
01:16:00,830 --> 01:16:02,600
increasing our
heuristic values that

1539
01:16:02,600 --> 01:16:05,150
exist on our graph,
in our true graph,

1540
01:16:05,150 --> 01:16:07,130
by a cost of scale factor.

1541
01:16:07,130 --> 01:16:08,570
And this is
typically very large,

1542
01:16:08,570 --> 01:16:13,290
so differences in heuristics
become very large.

1543
01:16:13,290 --> 01:16:17,630
This means that once
we start along a path,

1544
01:16:17,630 --> 01:16:20,030
we continue along it.

1545
01:16:20,030 --> 01:16:23,690
And that causes us to very
quickly find the first path

1546
01:16:23,690 --> 01:16:25,550
that actually works.

1547
01:16:25,550 --> 01:16:28,870
Then, we can view reducing
those heuristics back

1548
01:16:28,870 --> 01:16:34,460
to their original values as
essentially a form of changing

1549
01:16:34,460 --> 01:16:37,640
our [INAUDIBLE] causes or--

1550
01:16:37,640 --> 01:16:39,920
so it's a form of replanning.

1551
01:16:39,920 --> 01:16:41,510
And then by applying
the principles

1552
01:16:41,510 --> 01:16:44,900
of an incremental planner, we
can efficiently use the data

1553
01:16:44,900 --> 01:16:49,090
that we already have in
order to perform the search

1554
01:16:49,090 --> 01:16:53,640
and repair our
solution as we go.

1555
01:16:53,640 --> 01:16:56,240
Finally, we want to
make explicit to you

1556
01:16:56,240 --> 01:17:00,470
guys how we can actually
apply these algorithms

1557
01:17:00,470 --> 01:17:02,930
to mobile robotics.

1558
01:17:02,930 --> 01:17:05,300
Because it may not
immediately clear how

1559
01:17:05,300 --> 01:17:09,680
we go from a full
space to a graph.

1560
01:17:09,680 --> 01:17:12,760
Joe on this kind of briefly
when he talked about we

1561
01:17:12,760 --> 01:17:13,884
discretize the world.

1562
01:17:13,884 --> 01:17:15,800
But we want to give you
guys some ideas of how

1563
01:17:15,800 --> 01:17:18,500
we might do this cleverly.

1564
01:17:18,500 --> 01:17:24,050
So in order to build a graph
system out of our world state,

1565
01:17:24,050 --> 01:17:26,390
we differentiate between
holonomic systems

1566
01:17:26,390 --> 01:17:29,630
and nonholonomic systems,
which their difference is just

1567
01:17:29,630 --> 01:17:32,440
in terms of whether your
constraints are differential

1568
01:17:32,440 --> 01:17:35,412
in nature or not.

1569
01:17:35,412 --> 01:17:37,370
Then we are going to
outline three methods here

1570
01:17:37,370 --> 01:17:39,700
very briefly, cell
decomposition,

1571
01:17:39,700 --> 01:17:42,500
using a visibility graph, and
sampling-based construction

1572
01:17:42,500 --> 01:17:44,280
methods.

1573
01:17:44,280 --> 01:17:46,970
So for cell
decomposition methods,

1574
01:17:46,970 --> 01:17:51,350
we might to do what you would
expect, take the free space

1575
01:17:51,350 --> 01:17:54,655
in this region and split it up
into various different cells.

1576
01:17:54,655 --> 01:17:56,530
And each of those cells
is going to represent

1577
01:17:56,530 --> 01:17:58,910
a node in our graph.

1578
01:17:58,910 --> 01:18:02,630
And how we can do this
is we split the region up

1579
01:18:02,630 --> 01:18:06,680
based on each of the
vertices of obstacles.

1580
01:18:06,680 --> 01:18:13,940
So we draw a line vertically
from each of our vertices

1581
01:18:13,940 --> 01:18:17,060
that goes in the direction
away from the obstacle.

1582
01:18:17,060 --> 01:18:20,560
So at the top here, it's going
up and out of the obstacle.

1583
01:18:20,560 --> 01:18:25,351
But in the lower left corner
there, both up and down,

1584
01:18:25,351 --> 01:18:26,600
don't intercept the obstacles.

1585
01:18:26,600 --> 01:18:28,017
We draw them in its entirely.

1586
01:18:28,017 --> 01:18:29,975
So we can continue this
throughout the entirety

1587
01:18:29,975 --> 01:18:32,420
of the state space,
and then we can

1588
01:18:32,420 --> 01:18:35,490
attribute to each
essential cell that we've

1589
01:18:35,490 --> 01:18:38,920
defined through these regions,
a single point that represents

1590
01:18:38,920 --> 01:18:44,090
the midpoint of that cell,
which is going to be our graph

1591
01:18:44,090 --> 01:18:47,360
node, because we attribute
a single set of coordinates

1592
01:18:47,360 --> 01:18:50,370
to each of our graph nodes.

1593
01:18:50,370 --> 01:18:52,730
And we also, to make
sure that our transitions

1594
01:18:52,730 --> 01:18:55,790
between these cells
are consistent,

1595
01:18:55,790 --> 01:18:59,660
additional nodes at the
midpoint of each of these lines

1596
01:18:59,660 --> 01:19:03,110
that we've drawn, where we make
the distinction that from here,

1597
01:19:03,110 --> 01:19:04,680
we've actually drawn two lines.

1598
01:19:04,680 --> 01:19:07,580
So we draw the midpoint
between all of those.

1599
01:19:07,580 --> 01:19:09,575
This allows us to
essentially create a graph

1600
01:19:09,575 --> 01:19:15,230
through the entire system,
and we connect each node

1601
01:19:15,230 --> 01:19:18,145
at the center of any
given cell to nodes

1602
01:19:18,145 --> 01:19:21,530
that are on the boundary
of any given cell.

1603
01:19:21,530 --> 01:19:25,000
This allows us to create a
full graph through the system,

1604
01:19:25,000 --> 01:19:26,810
that we can move
through, guaranteeing

1605
01:19:26,810 --> 01:19:30,760
that none of these paths,
if we follow them exactly

1606
01:19:30,760 --> 01:19:33,439
collide with the obstacles.

1607
01:19:33,439 --> 01:19:35,730
Now, this works in environments
where the obstacles are

1608
01:19:35,730 --> 01:19:39,460
2D polygons, and the path is
far from optimal, as we can see,

1609
01:19:39,460 --> 01:19:42,285
but we can plan an optimal
path over the graph

1610
01:19:42,285 --> 01:19:44,810
that we have defined.

1611
01:19:44,810 --> 01:19:48,300
And this only works
on holonomic systems.

1612
01:19:48,300 --> 01:19:50,675
We can also define what
is called a visibility

1613
01:19:50,675 --> 01:19:54,050
graph, where we essentially
draw a boundary between both

1614
01:19:54,050 --> 01:19:56,210
our vehicle down there
at that bottom left,

1615
01:19:56,210 --> 01:19:58,470
and boundaries
around our obstacles.

1616
01:19:58,470 --> 01:20:00,770
These boundaries are
sufficiently sized

1617
01:20:00,770 --> 01:20:06,500
so that if the center of the
vehicle that we're defining

1618
01:20:06,500 --> 01:20:10,130
is along the extended
edge of the obstacle,

1619
01:20:10,130 --> 01:20:15,090
that the vehicle will not
collide with that obstacle.

1620
01:20:15,090 --> 01:20:19,890
Then for each of the vertices
of the extended boundary

1621
01:20:19,890 --> 01:20:23,780
that we've defined,
we add in a new node,

1622
01:20:23,780 --> 01:20:27,260
and we draw all lines
between all nodes, which

1623
01:20:27,260 --> 01:20:29,830
can be connected
without intersecting

1624
01:20:29,830 --> 01:20:33,920
the extended obstacles,
which we've drawn here.

1625
01:20:33,920 --> 01:20:39,710
And this leads to a graph
that we can, again, plan over.

1626
01:20:39,710 --> 01:20:41,520
This, again, works
in environments where

1627
01:20:41,520 --> 01:20:42,810
the obstacles are 2D polygons.

1628
01:20:42,810 --> 01:20:44,930
But what's nice
about this system is

1629
01:20:44,930 --> 01:20:47,200
that we do get an
optimal path, provided

1630
01:20:47,200 --> 01:20:50,220
that we assume that we
assume that our extension

1631
01:20:50,220 --> 01:20:56,735
of the robot itself is valid,
or doesn't act too much--

1632
01:20:59,280 --> 01:21:03,560
isn't too excessive
for constraint.

1633
01:21:03,560 --> 01:21:07,970
Finally, going back to some
of the random sampling methods

1634
01:21:07,970 --> 01:21:10,830
that we mentioned at the very
beginning of this lecture,

1635
01:21:10,830 --> 01:21:14,690
we've introduced sampling-based
roadmap construction, wherein

1636
01:21:14,690 --> 01:21:18,440
we choose nodes randomly
or deterministically

1637
01:21:18,440 --> 01:21:19,685
throughout our environment.

1638
01:21:19,685 --> 01:21:21,830
The difference here,
though, is that we then

1639
01:21:21,830 --> 01:21:23,850
create a graph out
of this system,

1640
01:21:23,850 --> 01:21:26,610
and that we reason over
that graph using something

1641
01:21:26,610 --> 01:21:29,921
like an incremental
planning algorithm.

1642
01:21:29,921 --> 01:21:33,680
And so when we create that
graph, after we've sampled,

1643
01:21:33,680 --> 01:21:37,160
we can use the [INAUDIBLE]
neighbors of each node,

1644
01:21:37,160 --> 01:21:41,620
or we can connect each
node to every node

1645
01:21:41,620 --> 01:21:46,650
within a certain radius, which
is what we've drawn here.

1646
01:21:46,650 --> 01:21:50,880
Finally, in order to
actually move between nodes,

1647
01:21:50,880 --> 01:21:54,060
we need to have some
knowledge about the dynamics

1648
01:21:54,060 --> 01:21:56,460
of the vehicle, which is
what we're discussing here.

1649
01:21:56,460 --> 01:22:00,660
And one method that we can do
this for nonholonomic systems

1650
01:22:00,660 --> 01:22:05,750
is we can distinguish
between different paths that

1651
01:22:05,750 --> 01:22:07,670
use turning right, turning
left, and traveling

1652
01:22:07,670 --> 01:22:09,260
in straight lines.

1653
01:22:09,260 --> 01:22:11,720
It turns out that the shortest
path between two points

1654
01:22:11,720 --> 01:22:16,160
can be defined using
one of these systems.

1655
01:22:16,160 --> 01:22:19,510
And so we just have to search
over a finite number of options

1656
01:22:19,510 --> 01:22:23,390
in order to find the best path
that can connect any two nodes.

1657
01:22:26,720 --> 01:22:29,450
Hopefully, that has
given you some intuition

1658
01:22:29,450 --> 01:22:34,490
into how we can apply these
incremental path planning

1659
01:22:34,490 --> 01:22:35,910
[INAUDIBLE] systems.

1660
01:22:35,910 --> 01:22:39,810
Are there any questions?

1661
01:22:39,810 --> 01:22:43,460
[APPLAUSE]