Edmond Irani Liu committed Nov 13, 2020 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 committed Nov 13, 2020 244 `````` "Given that we have constructed a feasible trajectory, we now visualize our solution (ego vehicle shown with green rectangle):" `````` Edmond Irani Liu committed Nov 13, 2020 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 committed Nov 13, 2020 279 280 `````` "from commonroad.common.solution import Solution, PlanningProblemSolution, \\\n", " VehicleModel, VehicleType, CostFunction\n", `````` Edmond Irani Liu committed Nov 13, 2020 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 }``````