Commit fd1b829e authored by Edmond Irani Liu's avatar Edmond Irani Liu 🏂
Browse files

minor update

parent 25832298
......@@ -2,12 +2,12 @@
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).
```sh
$ git clone https://gitlab.lrz.de/tum-cps/commonroad-drivability-checker.git
$ cd commonroad-drivability-checker
$ bash build.sh -e /path/to/your/anaconda3/envs/commonroad-py37 -v 3.X --cgal --serializer -i -j 4
$ sudo bash build.sh -e /path/to/your/anaconda3/envs/commonroad-py37 -v 3.X --cgal --serializer -i -j 4
```
`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).
......
......@@ -65,7 +65,7 @@ scenario_loader:
# DEFAULT: loads all scenarios from the specified scenario folder
# RANDOM: loads randomly selected scenarios from the specified scenario folder
# SPECIFIC: loads specified scenarios, see below
inputmode: DEFAULT
inputmode: RANDOM
# number of scenarios to be randomly selected. this only works for inputmode set to RANDOM
random_count: 3
......
import warnings
from typing import List
from copy import deepcopy
from typing import List
import numpy as np
from commonroad.common.solution import VehicleModel, VehicleType
from commonroad.scenario.trajectory import Trajectory, State
from commonroad.common.solution_writer import VehicleModel, VehicleType
class MotionPrimitive:
......
......@@ -14,8 +14,9 @@ from SMP.route_planner.route_planner.utils_route import chaikins_corner_cutting,
try:
import pycrccosy
except ModuleNotFoundError as exp:
warnings.warn(f"""You won't be able to use the Curvilinear Coordinate System for the Navigator,
the calculations won't be precise. {exp}""")
pass
# warnings.warn(f"""You won't be able to use the Curvilinear Coordinate System for the Navigator,
# the calculations won't be precise. {exp}""")
class RouteType(Enum):
......
......@@ -25,8 +25,9 @@ from SMP.route_planner.route_planner.route import RouteType, RouteCandidateHolde
try:
import pycrccosy
except ModuleNotFoundError as exp:
warnings.warn(f"""You won't be able to use the Curvilinear Coordinate System for the Navigator,
the calculations won't be precise. {exp}""")
pass
# warnings.warn(f"""You won't be able to use the Curvilinear Coordinate System for the Navigator,
# the calculations won't be precise. {exp}""")
class RoutePlanner:
......
......@@ -24,7 +24,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
......
%% Cell type:markdown id: tags:
# Tutorial: Uninformed Search Algorithms
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.
%% Cell type:code id: tags:
``` python
%matplotlib inline
%load_ext autoreload
%autoreload 2
import os
import sys
path_notebook = os.getcwd()
# add the SMP folder to python path
sys.path.append(os.path.join(path_notebook, "../../"))
# add the 1_search_algorithms folder to python path
sys.path.append(os.path.join(path_notebook, "../"))
import matplotlib.pyplot as plt
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.draw_dispatch_cr import draw_object
from SMP.motion_planner.motion_planner import MotionPlanner
from SMP.maneuver_automaton.maneuver_automaton import ManeuverAutomaton
from SMP.motion_planner.utility import plot_primitives, display_steps
```
%% Cell type:markdown id: tags:
## 1. Loading CR Scenario and Planning Problem
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
%% Cell type:code id: tags:
``` python
# load scenario
path_scenario = os.path.join(path_notebook, "../../scenarios/tutorial/")
id_scenario = 'ZAM_Tutorial_Urban-3_2'
# read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
# plot the scenario and the planning problem set
plt.figure(figsize=(15, 5))
draw_object(scenario)
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.margins(0, 0)
plt.show()
```
%%%% Output: display_data
![](data:image/png;base64,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)
%% Cell type:markdown id: tags:
## 2. Generating a Maneuver Automaton
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.
%% Cell type:code id: tags:
``` python
# load the xml with stores the motion primitives
name_file_motion_primitives = 'V_9.0_9.0_Vstep_0_SA_-0.2_0.2_SAstep_0.2_T_0.5_Model_BMW_320i.xml'
# generate automaton
automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)
# plot motion primitives
plot_primitives(automaton.list_primitives)
```
%% Cell type:markdown id: tags:
## 3. Search algorithms
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.
%% Cell type:code id: tags:
``` python
# construct motion planner
planner_BFS = MotionPlanner.BreadthFirstSearch(scenario=scenario,
planning_problem=planning_problem,
automaton=automaton)
# prepare input for visualization
scenario_data = (scenario, planner_BFS.state_initial, planner_BFS.shape_ego, planning_problem)
# display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_BFS.execute_search,
config=planner_BFS.config_plot)
```
%% Cell type:markdown id: tags:
### 3.2 Depth-First Search (DFS)
The DFS algorithm uses a LIFO (Last-In First-Out) queue for pushing the nodes.
%% Cell type:code id: tags:
``` python
# construct motion planner
planner_DFS = MotionPlanner.DepthFirstSearch(scenario=scenario,
planning_problem=planning_problem,
automaton=automaton)
# prepare input for visualization
scenario_data = (scenario, planner_DFS.state_initial, planner_DFS.shape_ego, planning_problem)
# display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_DFS.execute_search,
config=planner_DFS.config_plot)
```
%% Cell type:markdown id: tags:
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.
%% Cell type:code id: tags:
``` python
import numpy as np
# read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
for state in planning_problem.goal.state_list:
state.position = state.position.translate_rotate(np.array([0, 4]), 0)
# Plot the scenario and the planning problem set
plt.figure(figsize=(15, 5))
draw_object(scenario)
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.margins(0, 0)
plt.show()
```
%% Cell type:code id: tags:
``` python
# construct motion planner
planner_DFS = MotionPlanner.DepthFirstSearch(scenario=scenario,
planning_problem=planning_problem,
automaton=automaton)
# prepare input for visualization
scenario_data = (scenario, planner_DFS.state_initial, planner_DFS.shape_ego, planning_problem)
# display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_DFS.execute_search,
config=planner_DFS.config_plot)
```
%% Cell type:markdown id: tags:
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.
%% Cell type:code id: tags:
``` python
limit_depth = 7
# construct motion planner
planner_DLS = MotionPlanner.DepthLimitedSearch(scenario=scenario,
planning_problem=planning_problem,
automaton=automaton)
# prepare input for visualization
scenario_data = (scenario, planner_DLS.state_initial, planner_DLS.shape_ego, planning_problem)
# display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_DLS.execute_search,
config=planner_DLS.config_plot, limit_depth=limit_depth)
```
%% Cell type:markdown id: tags:
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).
%% Cell type:code id: tags:
``` python
# read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
# construct motion planner
planner_UCS = MotionPlanner.UniformCostSearch(scenario=scenario,
planning_problem=planning_problem,
automaton=automaton)
# prepare input for visualization
scenario_data = (scenario, planner_UCS.state_initial, planner_UCS.shape_ego, planning_problem)
# display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_UCS.execute_search,
config=planner_UCS.config_plot)
```
%% Cell type:markdown id: tags:
## Congratulations!
You have finished the tutorial on uninformed search algorithms! Next, you can proceed with the tutorial on informed search algorithms.
......
%% Cell type:markdown id: tags:
# Tutorial: Solving CommonRoad Planning Problems
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
%matplotlib inline
# 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
%load_ext autoreload
%autoreload 2
import os
import sys
path_notebook = os.getcwd()
# add the root folder to python path
sys.path.append(os.path.join(path_notebook, "../../"))
import matplotlib.pyplot as plt
from IPython import display
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.draw_dispatch_cr import draw_object
from SMP.motion_planner.motion_planner import MotionPlanner, MotionPlannerType
from SMP.maneuver_automaton.maneuver_automaton import ManeuverAutomaton
from SMP.motion_planner.utility import plot_primitives
```
%% Cell type:markdown id: tags:
Then, we specify the motion planner that we want to use:
%% Cell type:code id: tags:
``` python
# uncomment the following lines to use different motion planners
# type_motion_planner = MotionPlannerType.UCS
# type_motion_planner = MotionPlannerType.GBFS
type_motion_planner = MotionPlannerType.ASTAR
# type_motion_planner = MotionPlannerType.STUDENT_EXAMPLE
# 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
%% Cell type:code id: tags:
``` python
# load scenario
path_scenario = os.path.join(path_notebook, "../../scenarios/exercise/")
id_scenario = 'USA_Lanker-1_2_T-1'
# read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
```
%% Cell type:code id: tags:
``` python
# visualize scenario
for i in range(0, 50):
display.clear_output(wait=True)
plt.figure(figsize=(10, 10))
draw_object(scenario, draw_params={'time_begin': i})
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.show()
```
%% Cell type:markdown id: tags:
## 2. Generating a Maneuver Automaton
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.
%% Cell type:code id: tags:
``` python
# load the xml with stores the motion primitives
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'
# generate automaton
automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)
# plot motion primitives
plot_primitives(automaton.list_primitives)
```
%% Cell type:code id: tags:
``` python
# load the xml with stores the motion primitives
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'
# generate automaton
automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)
# plot motion primitives
plot_primitives(automaton.list_primitives)
```
%% Cell type:markdown id: tags:
If necessary, you can also create your own set of motion primitives (refer to **tutorial_motion_primitive_generator.ipynb**)
%% Cell type:markdown id: tags:
## 3. Searching for solutions
We now carry out the search for solutions with the automaton of 167 primitives.
### 3.1 Initializing motion planner
In this step, we would like to create a motion planner with the loaded scenario, planning problem, and the automaton.
%% Cell type:code id: tags:
``` python
# construct motion planner
motion_planner = MotionPlanner.create(scenario=scenario,
planning_problem=planning_problem,
automaton=automaton,
motion_planner_type=type_motion_planner)
```
%% Cell type:markdown id: tags:
### 3.2 Executing the search
Search for the solution can be executed by calling the `execute_search` function of the motion planner.
%% Cell type:code id: tags:
``` python
# solve for solution
list_paths_primitives, _, _ = motion_planner.execute_search()
```
%% Cell type:markdown id: tags:
### 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.
%% Cell type:code id: tags:
``` python
from commonroad.scenario.trajectory import State, Trajectory
from SMP.motion_planner.utility import create_trajectory_from_list_states
trajectory_solution = create_trajectory_from_list_states(list_paths_primitives)
```
%% Cell type:markdown id: tags:
### 3.4 Visualizing planned trajectory
Given that we have constructed a feasible trajectory, we now visualize our planned trajectory.
Given that we have constructed a feasible trajectory, we now visualize our solution (ego vehicle shown with green rectangle):
%% Cell type:code id: tags:
``` python
from SMP.motion_planner.utility import visualize_solution
visualize_solution(scenario, planning_problem_set, trajectory_solution)
```
%% Cell type:markdown id: tags:
## 4. Checking Validity of Planning Results
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.
%% Cell type:code id: tags:
``` python
from commonroad.common.solution import Solution, PlanningProblemSolution
from commonroad.common.solution_writer import VehicleModel, VehicleType, CostFunction
from commonroad.common.solution import Solution, PlanningProblemSolution, \
VehicleModel, VehicleType, CostFunction
# create PlanningProblemSolution object
kwarg = {'planning_problem_id': planning_problem.planning_problem_id,
'vehicle_model':VehicleModel.KS, # used vehicle model, change if needed
'vehicle_type':VehicleType.BMW_320i, # used vehicle type, change if needed
'cost_function':CostFunction.SA1, # cost funtion, DO NOT use JB1
'trajectory':trajectory_solution}
planning_problem_solution = PlanningProblemSolution(**kwarg)
# create Solution object
kwarg = {'scenario_id':scenario.scenario_id,
'planning_problem_solutions':[planning_problem_solution]}
solution = Solution(**kwarg)
```
%% Cell type:markdown id: tags:
### 4.2 Checking validity
Now, we can easily verify the validity of our solution using function `valid_solution`.
%% Cell type:code id: tags:
``` python
from commonroad_dc.feasibility.solution_checker import valid_solution
valid_solution(scenario, planning_problem_set, solution)
```
%% Cell type:markdown id: tags:
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**.