# Graph Search-Based Motion Planner with Motion Primitives
This is a programming exercise for the lecture **Introduction to Artificial Intelligence**(WS19) delivered at the Department of Informatics, TUM. Please clone this repository or download it using the button at the upper-right corner. The repository has the following folder structure:
This is a programming exercise for the lecture **Introduction to Artificial Intelligence**and **Gems of Informatics II** delivered at the Department of Informatics, TUM. Please clone this repository or download it using the button at the upper-right corner. The repository has the following folder structure:
``` code-block:: text
commonroad-search/
├GSMP/
...
...
@@ -97,7 +97,7 @@ Navigate your terminal to `commonroad-search/` folder, and start Jupyter Noteboo
$ jupyter notebook
```
In the prompt up page, navigate to `notebooks/tutorials/` and follow the tutorials `tutorial_commonroad-io.ipynb` and `tutorial_commonroad-search.ipynb`. Remember to refer to `pdfs/0_Guide_for_Exercise.pdf` for additional explanation. The executed Jupyter notebooks for tutorials can also be found [here](https://commonroad.in.tum.de/tutorials/).
In the prompt up page, navigate to `notebooks/tutorials/` and follow the tutorials `tutorial_commonroad-search.ipynb`, `cr_uninformed_search_tutorial.ipynb`, and `cr_informed_search_tutorial.ipynb`. Remember to refer to `pdfs/0_Guide_for_Exercise.pdf` for additional explanation. The executed Jupyter notebooks for tutorials can also be found [here](https://commonroad.in.tum.de/tutorials/).
# Tutorial: Uninformed Search Algorithms and CommonRoad Search
This tutorial shows how we can use motion primitives, i.e., short trajectory pieces, in uninformed search algorithms to find a trajectory that connects an **initial state** and a **goal region**.
%% Cell type:markdown id: tags:
## How to use this tutorial
Before you start with this tutorial, make sure that
* you have read through the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/static/docs/commonroad-io/index.html). Its tutorial can be found [here](https://commonroad.in.tum.de/tutorials/). The API of CommonRoad-io can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html)
* you have installed all necessary modules for CommonRoad Search according to the installation manual.
* you have read through the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/static/docs/commonroad-io/index.html). Its tutorial can be found [here](https://gitlab.lrz.de/tum-cps/commonroad_io/-/tree/master/commonroad%2Ftutorials). The API of CommonRoad-io can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html).
* you have installed all necessary modules for CommonRoad Search according to the [installation manual](https://gitlab.lrz.de/tum-cps/commonroad-search/-/blob/master/README.md).
Let's start with importing the modules we need for setting up the automaton and the CommonRoad scenario.
In the following, we load the motion primitives from an XML-File and generate a Maneuver Automaton.
The maneuver automaton for this tutorial consists of 7 motion primitives and stores the connectivity to other motion primitives.
Some additional explanations on the motion primitives:
* 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)) and the vehicle parameter are chosen for a BMW320i (vehicle_type_id=2).
* We have motion primitives for driving with constant velocity and the steering angle is changed with constant steering angle velocity. We generated motion primitives for all combinations of the steering angles in the initial state and end state for 0 rad and 0.2 rad, i.e., 4 combinations. The three motion primitives for turning left are mirrored for turning right, resulting in total 7 motion primitives.
* motion primitives can only be connected if they have matching initial/final velocities and matching initial/final steering angles.
In the next step, we set-up the BFS with the generated maneuver automaton to obtain a trajectory from the initial state to the goal region. The inital state and the goal region are specified in the planning problem.
When executing the following code block, you will see a "visualize" button directly beneath the "iteration" slider if you are running this notebook for the first time. Otherwise you can always find the button on the bottom.
Click the "visualize" button and let the search algorithm run through, once it's completed, you can use the slider to see all the iterations step by step.
Now we show the same example for the DFS. We use a simple implementation of the DFS which is similar to the BFS implementation but uses a LastInFirstOut(LIFO)-queue. Since the rest of the implementation is the same as the BFS, we directly run the DFS.
In this scenario, we were not able to find a solution using DFS, since DFS would append motion primitives for an infinitely long time (infinite state space). This shows that DFS is not complete, i.e., DFS is not guaranteed to find a solution if one exist.
To overcome this problem we introduce a depth limit, resulting in Depth-Limited Search (DLS). This search algorithm is introduced in the next section.
%% Cell type:markdown id: tags:
## Depth-Limited Search (DLS)
%% Cell type:markdown id: tags:
Before we run the algorithm, you can have a look at the impementation. We use the recursive implementation as introduced in the lecture.
Now let's run the algorithm and see what changes with the introduced limit. Here we set the limit to 7, as we know from BFS there exists a solution consisting of 7 motion primtives.
As you can see, depth-limited search finds a solution.
%% Cell type:markdown id: tags:
## Uniform-Cost Search
Up to now, we looked at all algorithms, which do not consider costs during search. In the following, we look at the uniform-cost search. Uniform-Cost search is optimal for any step costs, as it expands the node with the lowest path cost g(n). In this example our cost is the time to reach the goal. Thus, our cost g(n) is the time it took, to reach our current final state.
The Uniform-Cost Search is based on the Best-First Search, which we will also use for Greedy-Best-First Search and A\* Search. These algorithms only differ in their evaluation function. In Uniform-Cost Search, the evaluation function is f(n) = g(n).
Before we run the search, you can have a look at the implementation of the algorithm and the evaluation function. Again, we removed all the visualization parts, so it is easier to understand the code.
```python
defsearch_alg(self):
'''
Implementation of Best-First Search (tree search) using a Priority queue
Congratulations! You finished the tutorial on uninformed search and commonroad search! Next, you can check out the tutorial on informed search and commonroad search.
## Reading, Modifying, and Writing Common Scenarios
This tutorial shows how CommonRoad XML-files can be read, modified, visualized, and stored. To start with, a CommonRoad XML-file consists of a **Scenario** and a **PlanningProblemSet**:
* A **Scenario** represents the environment including a **LaneletNetwork** and a set of **DynamicObstacle** and **StaticObstacle**.
* A **LaneletNetwork** is built from lane segments (**Lanelet**), that can be connected arbitrarily.
* A **PlanningProblemSet** contains one **PlanningProblem** for every ego vehicle in the **Scenario**, consisting of an **initial position** and a **GoalRegion** that has to be reached.
%% Cell type:markdown id: tags:
## 0. Preparation
* Before you proceed any further, make sure you have skimmed through [CommonRoad API](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html#modules) to gain an overall view of the funtionalities provided by CommonRoad modules. You may need to refer to it for implementation details throughout this tutorial.
* Additional documentations on **CommonRoad XML format, Cost Functions, Vehicle Models, etc.** can be found at [CommonRoad](https://commonroad.in.tum.de/) under section **Documentation**.
%% Cell type:markdown id: tags:
## 1. Read XML file
As documented in [CommonRoadFileReader](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.file_reader), the **CommonRoadFileReader** reads in a CommonRoad XML file, and its **open()** method returns a **Scenario** and a **PlanningProblemSet** object:
%% Cell type:code id: tags:
``` python
importos
importmatplotlib.pyplotasplt
fromIPythonimportdisplay
# import functions to read xml file and visualize commonroad objects
It is possible to modify existing CommonRoad scenarios to costumize them to one's need. First, we would like to add a static obstacle to the scenario with the following specification:
- obstacle type: parked vehicle
- obstacle shape: rectangle, with a width of 2.0m and a length of 4.5m
- initial state:
- position: (30, 3.5) m
- orientation: 0.02 rad
* obstacle id: since every object in the scenario must have a unique ID, we can use the member function **generate_object_id** of **Scenario** class to generate a unique ID for the object.
As documented in [StaticObstacle](https://commonroad.in.tum.de/static/docs/commonroad-io/api/scenario.html#commonroad.scenario.obstacle.StaticObstacle), we need to provide `obstacle_id, obstacle_type, obstacle_shape, initial_state` to construct a static obstacle.
As can be seen, we have added a new static obstacle to the scenario. We can further add a dynamic obstacle with the following specifications:
- obstacle type: car
- obstacle shape: rectangle with a width of 1.8m and a length of 4.3m
- initial state:
- position: (50, 0.0) m
- orientation: 0.00 rad
- velocity: 22 m/s along x axis
- we assume that the dynamic obstacle drives with constant velocity.
As documented in [DynamicObstacle](https://commonroad.in.tum.de/static/docs/commonroad-io/api/scenario.html#commonroad.scenario.obstacle.DynamicObstacle), we need to pass in a **Prediction** object which in this case is a **TrajectoryPrediction** object. Its generation goes as follows:
1. compute all subsequent states for the dynamic obstacle
2. create a Trajectory from these states
3. create a TrajectoryPrediction from this trajectory and obstacle shape
After we have modified the scenario, we would like to save the changes and write the **Scenario** and **PlanningProblemSet** to a CommonRoad XML file. [CommonRoadFileWriter](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.file_writer) helps us with this purpse.
Note that we did not modify the **PlanningProblemSet** in this tutorial.