tutorial_commonroad_search.ipynb 14.4 KB
Newer Older
Edmond Irani Liu's avatar
Edmond Irani Liu committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial: Solving CommonRoad Planning Problems\n",
    "This tutorial demonstrates how we can use the (un)informed search algorithms, combined with motion primitives, to solve real planning problems introduced in CommonRoad [scenarios](https://commonroad.in.tum.de/scenarios). "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "Before you proceed with this tutorial, make sure that\n",
    "\n",
    "* you have gone through the tutorials on **CommonRoad Input-Output** and **(Un)Informed Search Algorithms**.\n",
    "* you have installed all necessary modules for CommonRoad Search according to the installation manual.\n",
    "\n",
    "Let's start with importing relevant modules and classes for setting up the automaton and the CommonRoad (CR) scenario."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "# enabling autoreload will reload modules automatically before executing the code, so that\n",
    "# you can edit the code for your motion planners and execute them right away without reloading\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "import os\n",
    "import sys\n",
    "path_notebook = os.getcwd()\n",
    "\n",
    "# add the root folder to python path\n",
    "sys.path.append(os.path.join(path_notebook, \"../../\"))\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "from IPython import display\n",
    "\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad.visualization.draw_dispatch_cr import draw_object\n",
    "\n",
    "from SMP.motion_planner.motion_planner import MotionPlanner, MotionPlannerType\n",
    "from SMP.maneuver_automaton.maneuver_automaton import ManeuverAutomaton\n",
    "from SMP.motion_planner.utility import plot_primitives"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we specify the motion planner that we want to use:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# uncomment the following lines to use different motion planners\n",
    "\n",
    "# type_motion_planner = MotionPlannerType.UCS\n",
    "# type_motion_planner = MotionPlannerType.GBFS\n",
    "type_motion_planner = MotionPlannerType.ASTAR\n",
    "# type_motion_planner = MotionPlannerType.STUDENT_EXAMPLE\n",
    "\n",
    "# your own motion planner can be called by uncommenting next line\n",
    "# type_motion_planner = MotionPlannerType.STUDENT"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Loading CR Scenario and Planning Problem Set\n",
    "In the next step, we load CommonRoad scenarios and their planning problems. Scenario ids beginning with prefix **\"C-\"** stand for cooeprative driving scenarios and possess multiple planning problems. The meaning of the symbols in a scenario are explained as follows:\n",
    "* **Dot**: initial state projected onto the position domain\n",
    "* **Red rectangle**: static obstacle\n",
    "* **Blue rectangle**: dynamic obstacle\n",
    "* **Yellow rectangle**: goal region projected onto the position domain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# load scenario\n",
    "path_scenario = os.path.join(path_notebook, \"../../scenarios/exercise/\")\n",
    "id_scenario = 'USA_Lanker-1_2_T-1'\n",
    "\n",
    "# read in scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()\n",
    "# retrieve the first planning problem in the problem set\n",
    "planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# visualize scenario\n",
    "for i in range(0, 50):\n",
    "    display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(10, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    \n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Generating a Maneuver Automaton\n",
    "\n",
    "In the following, we load the pre-generated motion primitives from an XML-File, and generate a **Maneuver Automaton** out of them. The primitives used here \n",
    "* The motion primitives are generated for the *Kinematic Single Track Model* (see [Vehicle Model Documentation](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf)) with parameters taken from vehicle model *BMW320i* (id_type_vehicle=2). \n",
    "* The motion primitives have a duration of 0.5 seconds, time step size of 0.1 seconds, and consists of 6 states. We assume constant input during this period. They have different velocities (ranging from 0 m/s to 20 m/s with step size of 4 m/s) and steering angles (ranging from -1.066 rad to 1.066 rad with a step size rounded to 0.18 rad) in their initial/final states, and resulting in a total of 167 distinct primitives.\n",
    "* Two motion primitives are considered connectable if the velocity and the steering angle of the final state of the preceding primitive are equal to those of the initial state of the following primitive.\n",
    "* By setting different ranges and steps for velocity and steering angle, one can obtain different sets of motion primitives: more primitives brings a higher coverage of state space when performing the search, but yields a higher branching factor and time of search; search with less primitives are usually faster, but chances are that no feasible trajectory leading to the goal region can be constructed with the set of primitives.\n",
    "\n",
    "We create two automata with 524 and 167 feasible motion primitives, respectively."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load the xml with stores the motion primitives\n",
    "name_file_motion_primitives = 'V_0.0_20.0_Vstep_2.0_SA_-1.066_1.066_SAstep_0.18_T_0.5_Model_BMW_320i.xml'\n",
    "# generate automaton\n",
    "automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)\n",
    "# plot motion primitives\n",
    "plot_primitives(automaton.list_primitives)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load the xml with stores the motion primitives\n",
    "name_file_motion_primitives = 'V_0.0_20.0_Vstep_4.0_SA_-1.066_1.066_SAstep_0.18_T_0.5_Model_BMW_320i.xml'\n",
    "# generate automaton\n",
    "automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)\n",
    "# plot motion primitives\n",
    "plot_primitives(automaton.list_primitives)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If necessary, you can also create your own set of motion primitives (refer to **tutorial_motion_primitive_generator.ipynb**)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Searching for solutions\n",
    "We now carry out the search for solutions with the automaton of 167 primitives.\n",
    "\n",
    "### 3.1 Initializing motion planner\n",
    "\n",
    "In this step, we would like to create a motion planner with the loaded scenario, planning problem, and the automaton."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# construct motion planner\n",
    "motion_planner = MotionPlanner.create(scenario=scenario, \n",
    "                                      planning_problem=planning_problem,\n",
    "                                      automaton=automaton, \n",
    "                                      motion_planner_type=type_motion_planner)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2 Executing the search\n",
    "Search for the solution can be executed by calling the `execute_search` function of the motion planner."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# solve for solution\n",
    "list_paths_primitives, _, _ = motion_planner.execute_search()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.3 Creating Trajectory object from the planning result\n",
    "The solutions returned by the motion planner is a list of List[State], which stores the states of the motion primitives constructing the solution trajectory. Note that the final state of a predecessor primitive is the same as the initial state of a successor primitive, thus we have to remove the duplicates in states."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad.scenario.trajectory import State, Trajectory\n",
    "from SMP.motion_planner.utility import create_trajectory_from_list_states\n",
    "\n",
    "trajectory_solution = create_trajectory_from_list_states(list_paths_primitives)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.4 Visualizing planned trajectory\n",
Edmond Irani Liu's avatar
Edmond Irani Liu committed
244
    "Given that we have constructed a feasible trajectory, we now visualize our solution (ego vehicle shown with green rectangle):"
Edmond Irani Liu's avatar
Edmond Irani Liu committed
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from SMP.motion_planner.utility import visualize_solution\n",
    "visualize_solution(scenario, planning_problem_set, trajectory_solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Checking Validity of Planning Results\n",
    "We provide a [Drivability Checker](https://commonroad.in.tum.de/drivability_checker) to verify the validity of the planned trajectories in terms of collision avoidance, kinematic feasibility, reaching goal state, etc. Here we demonstrate how to verify your solutions before saving and uploading them to the CommonRoad server. More information can be found at [Solution Checker](https://commonroad.in.tum.de/docs/commonroad-drivability-checker/sphinx/api_feasibility.html?highlight=valid_solution#module-commonroad_dc.feasibility.solution_checker)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.1 Creating Solution object\n",
    "Documentation regarding the **Solution** object can be found [here](https://commonroad-io.readthedocs.io/en/latest/api/common/#solution-class). You may refer to [Vehicle Models and Cost Functions](https://commonroad.in.tum.de/models_cost_functions) for more information. In this tutorial the Kinematic Single-Track Model is used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Edmond Irani Liu's avatar
Edmond Irani Liu committed
279
280
    "from commonroad.common.solution import Solution, PlanningProblemSolution, \\\n",
    "                                       VehicleModel, VehicleType, CostFunction\n",
Edmond Irani Liu's avatar
Edmond Irani Liu committed
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
    "\n",
    "# create PlanningProblemSolution object\n",
    "kwarg = {'planning_problem_id': planning_problem.planning_problem_id,\n",
    "         'vehicle_model':VehicleModel.KS,                            # used vehicle model, change if needed\n",
    "         'vehicle_type':VehicleType.BMW_320i,                        # used vehicle type, change if needed\n",
    "         'cost_function':CostFunction.SA1,                           # cost funtion, DO NOT use JB1\n",
    "         'trajectory':trajectory_solution}\n",
    "\n",
    "planning_problem_solution = PlanningProblemSolution(**kwarg)\n",
    "\n",
    "# create Solution object\n",
    "kwarg = {'scenario_id':scenario.scenario_id,\n",
    "         'planning_problem_solutions':[planning_problem_solution]}\n",
    "\n",
    "solution = Solution(**kwarg)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2 Checking validity\n",
    "Now, we can easily verify the validity of our solution using function `valid_solution`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad_dc.feasibility.solution_checker import valid_solution\n",
    "\n",
    "valid_solution(scenario, planning_problem_set, solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If the first element of the above validation returns `True`, you can then safely save and submit your solutions to the CommonRoad server."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Saving Planning Results\n",
    "The final step is saving your solution trajectory to the planning problem into a CommonRoad XML file, which can later be uploaded to the [CommonRoad website](https://commonroad.in.tum.de/submissions/new) for the solutions to be ranked. [Here](https://commonroad-io.readthedocs.io/en/latest/api/common/#commonroadsolutionwriter-class) is a documentation regarding the **Solution Writer**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad.common.solution import CommonRoadSolutionWriter\n",
    "\n",
    "dir_output = \"../../outputs/solutions/\"\n",
    "\n",
    "# write solution to a CommonRoad XML file\n",
    "csw = CommonRoadSolutionWriter(solution)\n",
    "csw.write_to_file(output_path=dir_output, overwrite=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations!\n",
    "\n",
    "You have finished the tutorial on CommonRoad Search! You can already proceed with coming up with your own implementation of the motion planner. You can also proceed with the tutorial on motion primitive generator."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}