1
00:00:01,790 --> 00:00:04,580
Before moving on to defining the go to goal function.

2
00:00:04,820 --> 00:00:10,790
One thing I must reinforce that is all the information that we gathered, the current force of the board

3
00:00:10,970 --> 00:00:12,410
and the part to go.

4
00:00:12,590 --> 00:00:18,590
Everything was done considering the image of the mes computer in the mapping stage as the frame of reference.

5
00:00:18,920 --> 00:00:23,870
So in the go to go function, the board will just iteratively try to reach those many goals.

6
00:00:24,110 --> 00:00:26,000
Define on that frame of reference.

7
00:00:26,210 --> 00:00:28,490
And eventually reaching them is exit.

8
00:00:30,650 --> 00:00:36,770
To add the go to go functionality to our board motion class, we first need to add a few instance variable

9
00:00:36,770 --> 00:00:40,400
inside the init function so we head on over to the init function.

10
00:00:40,670 --> 00:00:44,420
Scroll down and then add in the first instance variable.

11
00:00:44,420 --> 00:00:49,700
That is basically a state variable that tells us about whether the ME's exit has been reached or not.

12
00:00:50,000 --> 00:00:54,830
And because this is in this state and state, the goal not reach back what we set to do.

13
00:00:55,710 --> 00:01:00,690
And then we add in the next instance, we label that basically containers that stores the current when

14
00:01:00,690 --> 00:01:02,520
you go x and y axis.

15
00:01:03,390 --> 00:01:08,850
And then we add in the final instance variable, that is basically an iterator that tells us about the

16
00:01:08,850 --> 00:01:10,290
current minimal iteration.

17
00:01:10,800 --> 00:01:16,680
And because this is the initializing step, the pot iterator is set to zero because iteration starts

18
00:01:16,680 --> 00:01:20,250
from zero and then we head on over to the navigate path function.

19
00:01:20,340 --> 00:01:26,040
We previously computed the bot angle in image from the bot and the relation that we previously computed.

20
00:01:26,670 --> 00:01:32,160
So after this, because we have the complete force computed, we can now call in the go to go function

21
00:01:32,370 --> 00:01:34,310
to make the robot exit.

22
00:01:34,720 --> 00:01:37,890
But before calling the go to go function we first need to define it.

23
00:01:38,340 --> 00:01:44,370
So we head back before the navigate bot function and define the go to go function and that would be

24
00:01:44,370 --> 00:01:52,470
named as go to go and its arguments are exactly the same as navigate function that is both location.

25
00:01:54,660 --> 00:01:58,590
But we lost the object and velocity probably should.

26
00:02:00,380 --> 00:02:02,390
And then we define the go to go function.

27
00:02:02,780 --> 00:02:08,000
Now inside it, the very first thing that we need to do is basically compute the angle to go and the

28
00:02:08,000 --> 00:02:08,870
distance to go.

29
00:02:09,230 --> 00:02:14,750
And that can be done by simply using the angle and distance function that we defined earlier and passing

30
00:02:14,750 --> 00:02:18,620
in the current smart location and that many go x and y axis.

31
00:02:19,220 --> 00:02:26,270
So we assessed angular distance function for the point where we pass in the current port location and

32
00:02:26,270 --> 00:02:31,010
for the point B we pass passing the tuple that access the goalpost x.

33
00:02:32,050 --> 00:02:33,220
And the goal was why?

34
00:02:35,200 --> 00:02:40,810
So once we have provided this information, what we receive as an output are two outputs.

35
00:02:41,500 --> 00:02:45,340
The first is the angle to boot, and the second is the distance to go.

36
00:02:46,150 --> 00:02:51,370
So once we have the angle and the distance to go, we can now compute the actual angle the car needs

37
00:02:51,370 --> 00:02:56,290
to take to orient itself towards that meaning goal and to compute the angle to turn.

38
00:02:57,830 --> 00:02:58,730
What we need to do.

39
00:02:59,060 --> 00:03:00,080
We need to simply.

40
00:03:02,100 --> 00:03:02,970
We need to simply.

41
00:03:04,520 --> 00:03:10,610
Subtract and go to go from the car orientation.

42
00:03:11,040 --> 00:03:14,720
So got orientation is basically the bot angle.

43
00:03:15,290 --> 00:03:22,820
So once we subtract the boat angle from the angle to go, we get the angle to turn or the angle with

44
00:03:22,820 --> 00:03:26,570
the card needs to turn so that it can orient itself towards the winning goal.

45
00:03:27,440 --> 00:03:32,690
So once we have the information about the distance to go, angle to turn, we can now compute the actual

46
00:03:32,690 --> 00:03:38,150
angle of the angle and the speed of the car that it needs to take to make the exit.

47
00:03:38,900 --> 00:03:45,470
So that can be done by simply using a function of inter that is a function of an numbers library.

48
00:03:45,860 --> 00:03:51,740
So we compute speed for that is basically using the inter function of number library.

49
00:03:52,010 --> 00:03:57,290
We pass in the variable that is basically the distance to go and we basically use the distance to go

50
00:03:57,680 --> 00:04:01,550
and the speed would be just proportional to distance to go.

51
00:04:01,820 --> 00:04:05,240
So the greater the distance to the goal, the greater the speed of the car.

52
00:04:05,870 --> 00:04:14,060
So basically how this works is that our distance to go either has a range of 0 to 100 and we interpolate

53
00:04:14,450 --> 00:04:21,660
this list down from 0 to 100 for distance to go to get the speed that will be basically be from 0.2

54
00:04:21,660 --> 00:04:23,240
to 1.5.

55
00:04:23,540 --> 00:04:28,340
So for the hundred distance, the maximum speed would be given that is 1.5.

56
00:04:28,580 --> 00:04:32,880
And for the zero distance, the minimum speed would be given that is 0.2.

57
00:04:33,680 --> 00:04:39,320
And then be adding a compute angle or steering angle which to God needs to orient itself towards the

58
00:04:39,650 --> 00:04:40,070
exit.

59
00:04:40,370 --> 00:04:44,420
And that is again using the inter function we provide in.

60
00:04:45,950 --> 00:04:46,910
The angle to turn.

61
00:04:49,260 --> 00:04:50,970
And go through June as a variable.

62
00:04:51,180 --> 00:04:56,070
And then it was in its range that is basically -360.

63
00:04:58,190 --> 00:04:59,360
To Boston 360.

64
00:04:59,570 --> 00:05:02,300
And then we pass in the new range that we want to interpolate to.

65
00:05:03,550 --> 00:05:08,950
That is basically negative for two, positive four or basically the range of the steering angle.

66
00:05:09,280 --> 00:05:14,170
Once we have computed the speed and the angle of the car, which it needs to take to reach the miss

67
00:05:14,170 --> 00:05:19,780
or the meaning goal, then what we need to do, we need to simply print out this information to the

68
00:05:19,780 --> 00:05:22,300
user and also print out a distance to go.

69
00:05:23,080 --> 00:05:26,650
Now, once we have this information, the next thing that we need to do, we need to start.

70
00:05:27,870 --> 00:05:32,810
Start checking if the car is far away, then what we need to do, we need to turn towards the goal.

71
00:05:33,570 --> 00:05:39,930
So if the distance of to go is greater than two, then we set angular component of the velocity object

72
00:05:40,380 --> 00:05:42,330
to set equal to the angle.

73
00:05:42,720 --> 00:05:45,960
So basically the distance to go is greater than some value.

74
00:05:46,230 --> 00:05:53,100
Then we start or we turn the car towards the exit or towards the mini go and then we address the second

75
00:05:53,100 --> 00:05:53,460
case.

76
00:05:53,670 --> 00:05:59,430
That is because our car is a differential drive, but it cannot turn at high speeds and it cannot run

77
00:05:59,610 --> 00:06:00,840
more than a certain limit.

78
00:06:01,290 --> 00:06:02,670
So we need to address that limit.

79
00:06:03,060 --> 00:06:08,460
And what we need to do, we simply check if the angle of the car angle the car needs to take.

80
00:06:08,460 --> 00:06:13,890
When a steering angle is greater than a certain value, then we either stop the car or if it is less

81
00:06:13,890 --> 00:06:17,010
than a certain value, then we move at a very low speed.

82
00:06:17,220 --> 00:06:23,550
Otherwise, if it is within a reasonable value, then we run with a speed computer earlier.

83
00:06:24,760 --> 00:06:30,910
So once you've completed this stuff, we then proceed on to simply pass on the velocity component or

84
00:06:30,910 --> 00:06:34,240
velocity object that we have just updated using the velocity publisher.

85
00:06:34,510 --> 00:06:40,000
And if we have not reached the final goal, so once you're done with this is simply checked.

86
00:06:40,150 --> 00:06:45,040
If we have reached the winning goal and that is done by simply checking if the distance to goal is less

87
00:06:45,040 --> 00:06:46,300
than eight pixels.

88
00:06:46,870 --> 00:06:52,270
So if the distance to goal is within a certain threshold, then we have reached the winning goal and

89
00:06:52,270 --> 00:06:53,680
we need to simply stop the card.

90
00:06:53,680 --> 00:06:58,720
We're starting the linear and angular component of the car to zero and publishing this velocity using

91
00:06:58,720 --> 00:06:59,680
the velocity publisher.

92
00:07:00,610 --> 00:07:06,820
And then we simply check if we have reached the final goal, and that is done by simply checking the

93
00:07:06,820 --> 00:07:07,620
sift or path.

94
00:07:07,630 --> 00:07:10,690
Iteration is equal to the length of port minus one.

95
00:07:11,200 --> 00:07:17,020
So if we have reached a five minute goal, then we simply set the goal, not reach back to false, indicating

96
00:07:17,140 --> 00:07:23,470
that we have reached the final means exit and we simply play the party music and then we address the

97
00:07:23,470 --> 00:07:24,090
other case.

98
00:07:24,520 --> 00:07:29,470
For the other case, if we have not reached the final goal, then we need to iterate over the next winning

99
00:07:29,470 --> 00:07:36,430
goal in the list and that is done by simply writing an L statement where we iterate over the next part,

100
00:07:36,430 --> 00:07:43,090
iterate to get the next goal, post X and the goal post y and simply play the music of that.

101
00:07:43,090 --> 00:07:44,500
We have reached the next winning goal.

102
00:07:45,310 --> 00:07:49,960
Once we are done with this, our port to go or go to goal has been completed.

103
00:07:50,800 --> 00:07:52,960
So we head back for the navigate port function.

104
00:07:53,170 --> 00:07:53,920
But before this.

105
00:07:55,200 --> 00:07:56,760
We need to do a few imports.

106
00:07:57,090 --> 00:08:01,230
The first import is basically the internal function that is part of the library.

107
00:08:01,410 --> 00:08:07,250
And the second function and second import that we need to do is basically the importing of the by name

108
00:08:07,260 --> 00:08:07,740
library.

109
00:08:08,100 --> 00:08:10,320
And this is necessary to play the music that we want.

110
00:08:10,470 --> 00:08:13,290
Once we have reached the winning goal or the final exit.

111
00:08:13,860 --> 00:08:15,150
So we head over to the top.

112
00:08:16,320 --> 00:08:21,870
Add in the new imports that are the into and the buying game library.

113
00:08:22,140 --> 00:08:27,930
And once we are done with this we head back to the navigate bot function where we simply check or address

114
00:08:27,930 --> 00:08:31,650
the case for if the path iterator is at the very beginning.

115
00:08:32,010 --> 00:08:35,400
At this point, the Google equals x will set to zero and zero.

116
00:08:35,490 --> 00:08:40,820
We don't want zero and zero, but rather we would want to tell it that it needs to reach the first reading

117
00:08:41,070 --> 00:08:43,800
in the path list and that is done by simply searching.

118
00:08:43,950 --> 00:08:48,750
If the path is equal to zero, then we simply set the port first variable.

119
00:08:48,750 --> 00:08:55,140
A path iteration to the goal was X and the goal was Y and then we head on over down where we have computed

120
00:08:55,140 --> 00:09:02,550
the bot angle inside the image case, we simply now call in the go to goal function, which is simply

121
00:09:02,550 --> 00:09:04,080
now calling the go to go function.

122
00:09:05,160 --> 00:09:11,250
So you provide in the vault location, report, provide in the port computer, and then you providing

123
00:09:11,250 --> 00:09:14,490
the velocity object and then you provided the velocity publisher.

124
00:09:14,970 --> 00:09:21,270
So this basically tells the car to reach the next me to go iteratively until it reaches a final exit.

125
00:09:21,990 --> 00:09:26,580
So this means that we have completed our go to go functionality.

126
00:09:26,790 --> 00:09:31,750
We have provided it to the motion planning class and now we can simply integrated to our Ms..

127
00:09:31,770 --> 00:09:36,570
Salvador Pi and see if it does the job of reaching the exit or not.

128
00:09:38,050 --> 00:09:42,010
To perform the analysis of the go to go function for the motion planner class.

129
00:09:42,250 --> 00:09:48,610
We are adding a new function that basically displays a control mechanism in action and that is done

130
00:09:48,610 --> 00:09:50,560
before the angle and distance function.

131
00:09:51,250 --> 00:09:55,030
So right over here we have the display control mechanism in action.

132
00:09:55,390 --> 00:09:59,210
This takes in the input argument of what location the port to go.

133
00:09:59,650 --> 00:10:02,950
It was taught to spot the localize and the frame to display.

134
00:10:03,550 --> 00:10:06,400
Then we head back by saving this to Miss Solver.

135
00:10:06,730 --> 00:10:07,450
Scroll down.

136
00:10:07,790 --> 00:10:12,400
We previously called the bot motion planner function of Navigate Path to navigate to go.

137
00:10:12,910 --> 00:10:18,430
We then call in the function that is basically display the control mechanism in action.

138
00:10:18,880 --> 00:10:24,490
We passing the board location and part previously computed and then image shows the spot that would

139
00:10:24,490 --> 00:10:27,280
basically be the bot path planner instance variable.

140
00:10:27,760 --> 00:10:33,160
We have not defined this instance variable, so let's define it inside the board for class.

141
00:10:33,520 --> 00:10:39,560
So we head back to the board planner, create the new instance variable that is in the shortest path.

142
00:10:41,380 --> 00:10:47,710
Assign this to an empty list and then we head back to the drop bot on Miss Function where we have the

143
00:10:47,710 --> 00:10:48,610
Miss video.

144
00:10:49,060 --> 00:10:54,940
And this Miss BGR would become the value of the newly assigned image's shortest path.

145
00:10:56,200 --> 00:10:59,740
So we assign the value of Miss VTR to this instance variable.

146
00:11:00,040 --> 00:11:04,510
You save this, we head back to the Ms. solver, maybe boss in this image.

147
00:11:04,510 --> 00:11:05,110
Shortest path.

148
00:11:05,110 --> 00:11:08,680
And the third argument and then we boss in the bot localize the object.

149
00:11:08,860 --> 00:11:10,180
And finally the frame to the.

150
00:11:10,990 --> 00:11:17,260
Then we have we simply, I am sure, the same display which shows us the life made solving in action.

151
00:11:18,290 --> 00:11:21,140
Now we can simply head on over to our terminal.

152
00:11:22,220 --> 00:11:23,810
I have already started a simulation.

153
00:11:24,320 --> 00:11:25,190
Build the project.

154
00:11:27,330 --> 00:11:28,410
Run the missile over.

155
00:11:30,400 --> 00:11:33,310
By writing rules to run.

156
00:11:35,560 --> 00:11:37,450
Mays bought it.

157
00:11:37,450 --> 00:11:37,840
So.

158
00:11:39,410 --> 00:11:45,830
To start the localizing state and then we get this output of the mail solving bought life.

159
00:11:47,180 --> 00:11:55,040
So oops, we have landed on an error and the error is that we have a value error that is FP and XB are

160
00:11:55,040 --> 00:11:55,940
not of the same length.

161
00:11:56,510 --> 00:12:01,370
So FP and SBY are basically the initial range that we wanted to interpolate from.

162
00:12:02,030 --> 00:12:09,080
And this is occurring on the line 14 to basically underline not fitting to our line one to do one and

163
00:12:09,080 --> 00:12:15,320
go to go function where we have incorrectly passing the information of negative four to positive four.

164
00:12:15,920 --> 00:12:20,600
And here if you can see this should be a comma representing a range of a list.

165
00:12:21,140 --> 00:12:24,410
So we head back to the go to go function inside the motions class.

166
00:12:24,620 --> 00:12:26,120
Inside the go to go function.

167
00:12:27,710 --> 00:12:32,600
Really we have basically defined the range and we here we have the range, the new range that we wanted

168
00:12:32,750 --> 00:12:34,280
the angle to be interpreted to.

169
00:12:34,550 --> 00:12:39,680
So this should be -4 to 4, but rather it is written as -4.4.

170
00:12:40,070 --> 00:12:41,750
So this is why it is causing an error.

171
00:12:42,290 --> 00:12:48,020
Once we fix this error, we head back, close the simulation, run the project again.

172
00:12:49,680 --> 00:12:50,790
Launch the simulation.

173
00:12:52,090 --> 00:12:54,880
Once a simulation starts, I would run the Maze Solver.

174
00:12:56,900 --> 00:13:00,770
Then we get the first output of the localizing stage dressing space for.

175
00:13:02,550 --> 00:13:03,710
The output of them is now.

176
00:13:04,120 --> 00:13:05,250
The car is moving.

177
00:13:05,730 --> 00:13:11,220
It's trying to compute the correct robust pause or the complete robot pause, and then it's heading

178
00:13:11,940 --> 00:13:13,140
somewhere very long.

179
00:13:13,590 --> 00:13:14,670
So it has collided.

180
00:13:15,120 --> 00:13:16,680
And if you look in the simulation.

181
00:13:18,870 --> 00:13:23,040
Our car has just collided to the to the to the wall.

182
00:13:23,490 --> 00:13:28,560
There seems to be some problem with the algorithm and is this problem is more complex.

183
00:13:29,130 --> 00:13:30,150
So if you look.

184
00:13:32,120 --> 00:13:34,310
The minute is shown very wrong.

185
00:13:34,520 --> 00:13:38,300
So there is something wrong with the mini gold being printed, so we need to fix that.

186
00:13:38,780 --> 00:13:41,900
So the bottle's computer inside the bottling algorithm.

187
00:13:42,110 --> 00:13:43,820
So let's see if we can find the error.

188
00:13:44,210 --> 00:13:45,920
So we head back to the board then.

189
00:13:45,920 --> 00:13:46,450
Good bye.

190
00:13:46,520 --> 00:13:48,380
Maybe have the board part, plan a class.

191
00:13:48,890 --> 00:13:55,940
And here we were, computing or saving the fourth to go inside this instance variable and it is initialized

192
00:13:55,940 --> 00:14:01,550
inside the fine part in display function where we are initializing the part to display value for the

193
00:14:01,550 --> 00:14:02,300
part to go.

194
00:14:02,900 --> 00:14:08,420
But the problem is we required the coordinates to point, so we required the point to be saved inside

195
00:14:08,420 --> 00:14:10,580
the particle instead of the coordinates.

196
00:14:11,030 --> 00:14:18,830
So that can be done by simply going deleting this going after the parts point to display has been computed

197
00:14:18,830 --> 00:14:24,380
using the coordinates, the point function and simply assigning the port to go the value of what point

198
00:14:24,380 --> 00:14:24,950
to display.

199
00:14:25,340 --> 00:14:32,540
So hopefully this fixes the error and we can now get the correct location the robot needs to move to

200
00:14:32,540 --> 00:14:33,500
reach the main exit.

201
00:14:34,190 --> 00:14:40,490
So we re rebuild our main solver, run the simulation.

202
00:14:41,710 --> 00:14:44,650
And once the simulation starts up, it will run the myth solver.

203
00:14:46,540 --> 00:14:49,990
You get the first output of the localise and stage dressing space for.

204
00:14:51,010 --> 00:14:52,650
Figured out of the man's life.

205
00:14:53,110 --> 00:14:56,770
This time the car is moving and it's moving towards.

206
00:14:58,240 --> 00:15:04,910
Computing is complete robot pose and then it goes back and tries to raise the very first meeting goal

207
00:15:05,170 --> 00:15:08,470
that was of our path computed from the path then anchored by.

208
00:15:10,790 --> 00:15:15,380
So once it has reached the first minute goal, it makes a sound that it has reached the winning goal,

209
00:15:15,740 --> 00:15:16,880
just like Pac-Man.

210
00:15:17,060 --> 00:15:20,360
And then it tries to approach the next minute goal, which is shown right here.

211
00:15:21,050 --> 00:15:25,820
So there should be another window displays this all action live.

212
00:15:26,000 --> 00:15:30,890
And as you can see, this red ball basically represents the location of a robot.

213
00:15:31,250 --> 00:15:36,170
The green is basically the winning goal that we have already reached, and this is basically the winning

214
00:15:36,170 --> 00:15:38,300
goal that the robot is about to reach.

215
00:15:38,780 --> 00:15:44,330
So if you see right over here on the left window, the red represents the car moving towards the orange

216
00:15:44,510 --> 00:15:48,080
mini goal and the green is the winning goal that it has already reached.

217
00:15:48,650 --> 00:15:54,860
So the farther it was from the winning goal, the faster it move and the more angle the car needs to

218
00:15:54,860 --> 00:15:58,640
doing so, the slower the car's speed is so that they can turn easily.

219
00:15:59,700 --> 00:16:04,870
So it tries to raise one meaning goal at a time it attractively until it reaches the music.

220
00:16:05,610 --> 00:16:09,750
At this moment you are seeing that it is reaching the winning goal that is in front of it.

221
00:16:11,080 --> 00:16:17,050
So at this moment it reaches its minimum goals and then it needs to make a turn so its force makes Leyton

222
00:16:17,470 --> 00:16:21,460
orient itself towards the winning goal and then it reaches that winning goal.

223
00:16:22,720 --> 00:16:28,480
And once each segment go, it tries to re-orient towards the next minute goal that is on its right side.

224
00:16:28,690 --> 00:16:29,830
So you can see it's not moving.

225
00:16:30,010 --> 00:16:33,610
And once it reorients itself, then it moves toward that when you go.

226
00:16:34,830 --> 00:16:40,290
And keep this whole process going until it reaches all the mini goals, until the final and when you

227
00:16:40,290 --> 00:16:40,560
go.

228
00:16:40,820 --> 00:16:45,560
So on the top you could see the length of the path is basically 78 to 78.

229
00:16:45,570 --> 00:16:52,370
Many goals are what our go to go function has to try to reach until it reaches the final winning goal.

230
00:16:52,680 --> 00:16:56,670
And the current iteration is basically 48 mini goals.

231
00:16:57,240 --> 00:16:59,760
So it is right now moving in this direction.

232
00:17:00,000 --> 00:17:04,500
And if you look on the left hand side, this is basically what is considered as a frame of reference,

233
00:17:04,680 --> 00:17:11,340
this left image, because the entry is on the top of the image and the exit is on the bottom.

234
00:17:11,910 --> 00:17:15,960
So car is about to reach the maze exit as it approaches this winning goal.

235
00:17:15,960 --> 00:17:18,300
That is basically the winning goal is number 60.

236
00:17:18,930 --> 00:17:25,110
It reorients itself towards this winning goal by rotating itself, making the angle to turn, and then

237
00:17:25,110 --> 00:17:26,700
moving towards the final winning goal.

238
00:17:27,920 --> 00:17:30,620
So right now is approaching is why I'm going to go.

239
00:17:30,620 --> 00:17:32,180
And this is a large screen when you go.

240
00:17:34,050 --> 00:17:36,510
And we'll love it as rich is finally going to go.

241
00:17:36,840 --> 00:17:43,590
So in this way we have conformed that our go to function indeed helps car solve the maze by reaching

242
00:17:43,800 --> 00:17:44,640
the maze exit.

243
00:17:45,300 --> 00:17:51,330
So you can now proceed on to testing it out on a more complex scenario that is Maze two and see if it

244
00:17:51,330 --> 00:17:56,340
does the same job or the same correct job of reaching the muse exit on that particular case.

245
00:17:57,120 --> 00:17:58,940
Until then, happy coding.

246
00:17:59,280 --> 00:17:59,910
And by.