This is a programming exercise for the lecture **Foundations of Artificial Intelligence** (WS20) delivered at the Department of Informatics, TUM. The task in this exercise is to implement a heuristic function and/or a search algorithm with motion
primitives to solve [CommonRoad](https://commonroad.in.tum.de/) scenarios. The following search algorithms have been implemented as examples:
1. Breadth First Search
2. Depth First Search
3. Depth-limited Search
4. Uniform Cost Search (aka Dijkstra's algorithm)
5. Greedy Best First Search
6. A* Search
- Breadth First Search
- Depth First Search
- Depth-limited Search
- Uniform Cost Search (aka Dijkstra's algorithm)
- Greedy Best First Search
- A* Search
The code is written in Python 3.7 and has been tested on Ubuntu 18.04. As the first step, clone this repository with:
...
...
@@ -56,14 +56,14 @@ Then, install the dependencies with:
$ pip install-r requirements.txt
```
This will install related dependencies specified in the requirements.txt.
This will install related dependencies specified in the `requirements.txt`.
Next, we move on to the installation of [CommonRoad Drivability Checker](https://commonroad.in.tum.de/drivability_checker). This package provides functionalities such as collision checks, kinematic feasibility checks, road boundary checks, etc. Full installation commands are given below, other installation options can be found [here](https://commonroad.in.tum.de/docs/commonroad-drivability-checker/sphinx/installation.html).
`Note`: you need to substitute `/path/to/your/anaconda3/envs/commonroad-py37` with the path to your Anaconda environment, and `X` with your python version (e. g. setting X to 7 for 3.7).
This tutorial shows how we can combine motion primitives of vehicles, i.e., short trajectory segments, with uninformed search algorithms to find feasible trajectories that connect the **initial state** and the **goal region** of a given planning problem.
%% Cell type:markdown id: tags:
## 0. Preparation
Before you proceed with this tutorial, make sure that
* you have gone through the tutorial for **CommonRoad Input-Output**.
* you have installed all necessary modules for **CommonRoad Search** according to the installation manual.
Let's start with importing relevant modules and classes for setting up the automaton and the CommonRoad (CR) scenario.
In the next step, we load a CommonRoad scenario and its planning problem, which should be solved with the search algorithms. The meaning of the symbols in a scenario are explained as follows:
***Dot**: initial state projected onto the position domain
***Red rectangle**: static obstacle
***Yellow rectangle**: goal region projected onto the position domain
In the following, we load the pre-generated motion primitives from an XML-File, and generate a **Maneuver Automaton** out of them.
The maneuver automaton used for this tutorial consists of 7 motion primitives; the connectivity within the motion primitives are also computed and stored. 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)) with parameters taken from vehicle model *BMW320i* (id_type_vehicle=2).
* The motion primitives have a constant driving velocity of 9 m/s, varying steering angles with constant steering angle velocity, and a duration of 0.5 seconds. We assume constant input during this period.
* The motion primitives are generated for all combinations of the steering angles of values 0 rad and 0.2 rad at the initial state and the final state, thereby producing 2 (initial states) x 2 (final states) = 4 primitives. Except for the primive moving straight (with 0 rad steering angle at the initial and the final states), the remaining 3 left-turning primitives are mirrored with regard to the x-axis, resulting in a total number of 7 motion primitives.
* 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.
Next, we demonstrate the search results for the following algorithms:
1. Breadth-First Search (BFS)
2. Depth-First Search (DFS)
3. Depth-Limited Search (DLS)
4. Uniform-Cost Search (UCS)
For each of the algorithms, we create a corresponding motion planner implemented in the **MotionPlanner** class, with the scenario, the planning problem, and the generated automaton as the input. The source codes are located at **SMP/motion_planner/search_algorithms/**
After executing the code block for every algorithm, you will see a **"visualize"** button directly beneath the **"iteration"** slider. Click the **"visualize"** button and let the search algorithm run through; once it's completed, you can use the slider to examine all intermediate search steps. Meaning of colors and lines are explained as follows:
***Yellow solid:** frontier
***Yellow dashed:** collision
***Red solid:** currently exploring
***Gray solid:** explored
***Green solid:** final solution
%% Cell type:markdown id: tags:
### 3.1 Breadth-First Search (BFS)
The BFS algorithm uses a FIFO (First-In First-Out) queue for pushing the nodes.
In this scenario, we were lucky enough to find a solution using DFS. However, the DFS is not complete, i.e., DFS is not guaranteed to find a solution if one exist. To justify our claim, we slightly manipulate the scenario, such that the goal region is shifted for 4 m in the y-axis, and re-run the search.
As you can see, in this case DFS was not able to find a solution, since DFS would go infinitely deep when appending motion primitives (leading to infinite state space). **Question**: is this also the case with BFS? You can test it out.
To overcome this problem, we introduce a depth limit, resulting in Depth-Limited Search (DLS).
%% Cell type:markdown id: tags:
### 3.3 Depth-Limited Search (DLS)
Now let's run the algorithm and see what changes with the introduced depth limit. Here we set the limit to 7, as we know from the result of BFS that there exists a solution consisting of 7 motion primtives.
As you can see, DLS now finds a solution. **Question**: what happends if you have a lower or higher depth limit? Try it out.
%% Cell type:markdown id: tags:
### 3.4 Uniform-Cost Search (UCS)
The algorithms we have considered so far do not consider any cost during the search process. In the following, we look at the Uniform-Cost Search. UCS is optimal for any step costs, as it expands the node with the lowest path cost g(n). In this example, the cost is set to the time to reach the goal. Thus, our cost g(n) is the time it took to reach our current final state.
UCS is based on the Best-First Search algorithm, which we will also use for Greedy-Best-First Search and A\* Search in the next tutorial. These algorithms only differ in their evaluation function: in UCS, the evaluation function is f(n) = g(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 id: tags:
## 0. Preparation
Before you proceed with this tutorial, make sure that
* you have gone through the tutorials on **CommonRoad Input-Output** and **(Un)Informed Search Algorithms**.
* you have installed all necessary modules for CommonRoad Search according to the installation manual.
Let's start with importing relevant modules and classes for setting up the automaton and the CommonRoad (CR) scenario.
%% Cell type:code id: tags:
``` python
%matplotlibinline
# enabling autoreload will reload modules automatically before executing the code, so that
# you can edit the code for your motion planners and execute them right away without reloading
# your own motion planner can be called by uncommenting next line
# type_motion_planner = MotionPlannerType.STUDENT
```
%% Cell type:markdown id: tags:
## 1. Loading CR Scenario and Planning Problem Set
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:
***Dot**: initial state projected onto the position domain
***Red rectangle**: static obstacle
***Blue rectangle**: dynamic obstacle
***Yellow rectangle**: goal region projected onto the position domain
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
* 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).
* 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.
* 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.
* 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.
We create two automata with 524 and 167 feasible motion primitives, respectively.
### 3.3 Creating Trajectory object from the planning result
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.
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 id: tags:
### 4.1 Creating Solution object
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.
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 id: tags:
## 5. Saving Planning Results
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**.
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.
In the next step, we load a CommonRoad scenario and its planning problem(s), for which the routes should be planned. The route planner handles **one planning problem** at a time, thus we need to manually specify which planning problem it should take care. In our case, we select the first planning problem in the planning problem set. The meaning of the symbols in a scenario are explained as follows:
***Dot**: initial state of the planning problem projected onto the position domain
***Blue rectangle**: dynamic obstacle
***Yellow rectangle**: goal region projected onto the position domain
## 2. Creating a route planner and planning for routes
### 2.1 Instantiation
A route planner can be easily constructed by passing the **scenario** and the **planning problem** to `RoutePlanner` object. As for the backend, there are currently three supported options:
1. NETWORKX: uses built-in functions from the networkx package, tends to change lane later
2. NETWORKX_REVERSED: uses built-in functions from the networkx package, tends to change lane earlier
3. PRIORITY_QUEUE: uses A-star search to find routes, lane change maneuver depends on the heuristic cost
### 2.2 Planning all possible routes
The route planner plans a route for all possible combinations of start / goal lanelets. E.g. if our initial state is located in two lanes (due to overlapping of lanelets), and the same for our goal state, the route planner will try to plan routes for the four possible combinations.
### 2.3 Retrieving a route
Planned routes can be retrieved by using simple indices, or based on some heuristic functions to determine the best route of all. A route consists of a list of lanelet ids that leads from the initial state to the goal state.
### 2.4 Retrieving reference path
A reference path is automatically generated for each planned routes. The center lines of lanelets of a route is used to construct the reference path. The resulting polyline is then smoothened with Chaikin's corner cutting algorithm.
# print out coordinates of the vertices of the reference path
print("\nCoordiantes [x, y]:")
print("\nCoordinates [x, y]:")
print(route.reference_path)
```
%% Cell type:markdown id: tags:
## 3. Visualizing planning results
%% Cell type:markdown id: tags:
The planned routes can be easily visualized with the `draw_route` function. The arguements `draw_route_lanelets` and `draw_reference_path` indicates whether the lanelets of the route and the reference path should be drawn, respectively. The lanelets of the route is colored in cyan.
