Commit 28ff4ce0 authored by Sebastian Maierhofer's avatar Sebastian Maierhofer
Browse files

update links in tutorials

parent 1ee84487
# 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/).
## Implement your own search algorithm
......
%% Cell type:markdown id: tags:
# 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.
%% Cell type:code id: tags:
``` python
%matplotlib inline
import matplotlib.pyplot as plt
import sys
sys.path.append("../../GSMP/motion_automata")
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.draw_dispatch_cr import draw_object
from automata.HelperFunctions import load_scenario, generate_automata
from automata.helper_tree_search import *
import automata.tree_search as tree_search
```
%% Cell type:markdown id: tags:
## Open and Load the CR Scenario
In the next step, we load the scenario and planning problem, which we use in the uninformed search algorithm.
If you do not understand the following steps, please go back to the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/tutorials/).
%% Cell type:code id: tags:
``` python
# Load scenario ZAM_Urban-3_2
scenario_path = '../../scenarios/tutorial/'
scenario_id = 'ZAM_Tutorial_Urban-3_2'
scenario, planning_problem_set = CommonRoadFileReader(scenario_path+scenario_id+'.xml').open()
# Plot scenario and 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:markdown id: tags:
## Generating a Maneuver Automaton
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.
%% Cell type:code id: tags:
``` python
# Load motion primitives (mp)
mp_path = 'motion_primitives_search_tutorial/'
mp_file = 'V_9.0_9.0_Vstep_0_SA_-0.2_0.2_SAstep_0.4_T_0.5_Model_BMW320i.xml'
vehicle_type_id = 2
automaton = generate_automata(vehicle_type_id, mp_file= mp_path+mp_file, search_tutorial=True)
# plot motion primitives
plt.figure(figsize=(8,8))
for mp in automaton.Primitives:
plot_motion_primitive(mp)
plt.show()
```
%% Cell type:markdown id: tags:
## Breadth-First Search (BFS)
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.
%% Cell type:code id: tags:
``` python
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
# construct motion planner and set up the initial state for planning problem
bfs_planner = tree_search.BreadthFirstSearch(scenario=scenario, planningProblem=planning_problem,
automaton=automaton)
```
%% Cell type:markdown id: tags:
Before we run the BFS, you can check the code below to get an idea how BFS works for motion primitives.
Please note, we removed the parts for visualization in the code below so it easier to understand the code.
```python
def search_alg(self):
'''
Implementation of BFS (tree search) using a FIFO queue
'''
# First node
initial_node = Node(path=[[self.initial_state]], primitives=[], tree_depth=0)
# check if we already reached the goal state
if self.reached_goal(initial_node.path[-1]):
return self.remove_states_behind_goal(initial_node.path), initial_node.primitives
# add current node to the frontier
self.frontier.insert(initial_node)
while not self.frontier.empty():
# Pop the shallowest node
current_node: Node = self.frontier.pop()
# Check all possible successor primitives(i.e., actions) for current node
for succ_primitive in current_node.get_successors():
# translate/rotate motion primitive to current position
current_primitive_list = copy.copy(current_node.primitives)
path_translated = self.translate_primitive_to_current_state(succ_primitive,
current_node.path[-1])
# check for collision, if is not collision free it is skipped
if not self.check_collision_free(path_translated):
continue
current_primitive_list.append(succ_primitive)
# Goal test
if self.reached_goal(path_translated):
path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]
solution_path = self.remove_states_behind_goal(path_new)
# return solution
return self.remove_states_behind_goal(path_new), current_primitive_list
# Inserting the child to the frontier:
path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]
child = Node(path=path_new, primitives=current_primitive_list,
tree_depth=current_node.tree_depth + 1)
self.frontier.insert(child)
if path_translated[-1].time_step > self.desired_time.end:
return None, None
return None, None
```
%% Cell type:markdown id: tags:
Now we want to run the algorithm:
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.
%% Cell type:code id: tags:
``` python
scenario_data = (scenario, bfs_planner.initial_state, bfs_planner.egoShape, planning_problem)
display_steps(scenario_data=scenario_data, algorithm=bfs_planner.search_alg,
config=bfs_planner.config)
```
%% Cell type:markdown id: tags:
## Depth-First Search (DFS)
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.
%% Cell type:code id: tags:
``` python
# constructing the motion planner
dfs_planner = tree_search.DepthFirstSearch(scenario=scenario, planningProblem=planning_problem,
automaton=automaton)
# run the planner
display_steps(scenario_data=scenario_data, algorithm=dfs_planner.search_alg,
config=dfs_planner.config)
```
%% Cell type:markdown id: tags:
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.
```python
def search_alg(self, limit=7):
'''
Recursive implementation of DLS
'''
# First node
initial_node = Node(path=[[self.initial_state]], primitives=[], tree_depth=0)
result = self.recursive_dls(initial_node, limit)
if result is None:
return None, None
return path, list_primitives
def recursive_dls(self, current_node: Node, limit: int):
# Goal test
if self.reached_goal(current_node.path[-1]):
solution_path = self.remove_states_behind_goal(current_node.path)
# return solution
return solution_path, current_node.primitives
elif limit == 0:
return 'cutoff'
else:
cutoff_occurred = False
for succ_primitive in reversed(current_node.get_successors()):
# translate/rotate motion primitive to current position
current_primitive_list = copy.copy(current_node.primitives)
path_translated = self.translate_primitive_to_current_state(succ_primitive, current_node.path[-1])
# check for collision, if is not collision free it is skipped
if not self.check_collision_free(path_translated):
continue
# Continue search with child node
current_primitive_list.append(succ_primitive)
path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]
child = Node(path=path_new, primitives=current_primitive_list,
tree_depth=current_node.tree_depth + 1)
result = self.recursive_dls(current_node=child, limit=limit-1)
if result == 'cutoff':
cutoff_occurred = True
elif result is not None:
return result
return 'cutoff' if cutoff_occurred else None
```
%% Cell type:markdown id: tags:
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.
%% Cell type:code id: tags:
``` python
dls_planner = tree_search.DepthLimitedSearch(scenario=scenario, planningProblem=planning_problem,
automaton=automaton)
limit = 7
# run the planner
display_steps(scenario_data=scenario_data, algorithm=dls_planner.search_alg,
config=dls_planner.config, limit=7)
```
%% Cell type:markdown id: tags:
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
def search_alg(self):
'''
Implementation of Best-First Search (tree search) using a Priority queue
'''
# First node
initial_node = PrioNode(path=[[self.initial_state]], primitives=[], tree_depth=0, current_cost=0)
# add current node (i.e., current path and primitives) to the frontier
f = self.evaluation_function(initial_node)
self.frontier.insert(item=initial_node, priority=f)
while not self.frontier.empty():
# Pop the shallowest node
current_node: PrioNode = self.frontier.pop()
# Goal test
if self.reached_goal(current_node.path[-1]):
solution_path = self.remove_states_behind_goal(current_node.path)
# return solution
return solution_path, current_node.primitives
# Check all possible successor primitives(i.e., actions) for current node
for succ_primitive in current_node.get_successors():
# translate/rotate motion primitive to current position
current_primitive_list = copy.copy(current_node.primitives)
path_translated = self.translate_primitive_to_current_state(succ_primitive,
current_node.path[-1])
# check for collision, if is not collision free it is skipped
if not self.check_collision_free(path_translated):
continue
current_primitive_list.append(succ_primitive)
path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]
child_node = PrioNode(path=path_new, primitives=current_primitive_list,
tree_depth=current_node.tree_depth + 1,
current_cost=current_node.current_cost)
f = self.evaluation_function(current_node=child_node)
print(f)
# Inserting the child into the frontier:
self.frontier.insert(item=child_node, priority=f)
return None, None
def evaluation_function(self, current_node: PrioNode) -> float:
"""
Evaluation function of UCS is f(n) = g(n)
"""
if self.reached_goal(current_node.path[-1]):
current_node.path = self.remove_states_behind_goal(current_node.path)
# calculate g(n)
current_node.current_cost += len(current_node.path[-1]) * self.scenario.dt
return current_node.current_cost
```
%% Cell type:code id: tags:
``` python
ucs_planner = tree_search.UniformCostSearch(scenario=scenario, planningProblem=planning_problem,
automaton=automaton)
# run the planner
display_steps(scenario_data=scenario_data, algorithm=ucs_planner.search_alg,
config=ucs_planner.config)
```
%% Cell type:markdown id: tags:
Congratulations! You finished the tutorial on uninformed search and commonroad search! Next, you can check out the tutorial on informed search and commonroad search.
%% Cell type:code id: tags:
``` python
```
......
%% Cell type:markdown id: tags:
# Tutorial: CommonRoad-io
## 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
import os
import matplotlib.pyplot as plt
from IPython import display
# import functions to read xml file and visualize commonroad objects
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.draw_dispatch_cr import draw_object
# generate path of the file to be opened
file_path = "../../scenarios/tutorial/ZAM_Tutorial-1_1_T-1.xml"
# read in the scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()
# plot the scenario for 40 time step, here each time step corresponds to 0.1 second
for i in range(0, 40):
# uncomment to clear previous graph
# display.clear_output(wait=True)
plt.figure(figsize=(20, 10))
# plot the scenario at different time step
draw_object(scenario, draw_params={'time_begin': i})
# plot the planning problem set
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.show()
```
%% Cell type:markdown id: tags:
## 2. Modify XML file
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.
%% Cell type:code id: tags:
``` python
import numpy as np
# import necesary classes from different modules
from commonroad.geometry.shape import Rectangle
from commonroad.scenario.obstacle import StaticObstacle, ObstacleType
from commonroad.scenario.trajectory import State
# read in the scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()
# generate the static obstacle according to the specification, refer to API for details of input parameters
static_obstacle_id = scenario.generate_object_id()
static_obstacle_type = ObstacleType.PARKED_VEHICLE
static_obstacle_shape = Rectangle(width = 2.0, length = 4.5)
static_obstacle_initial_state = State(position = np.array([30.0, 3.5]), orientation = 0.02, time_step = 0)
# feed in the required components to construct a static obstacle
static_obstacle = StaticObstacle(static_obstacle_id, static_obstacle_type, static_obstacle_shape, static_obstacle_initial_state)
# add the static obstacle to the scenario
scenario.add_objects(static_obstacle)
# plot the scenario for each time step
for i in range(0, 40):
# uncomment to clear previous graph
# display.clear_output(wait=True)
plt.figure(figsize=(25, 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:
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
%% Cell type:code id: tags:
``` python
# import necesary classes from different modules
from commonroad.scenario.obstacle import DynamicObstacle
from commonroad.scenario.trajectory import Trajectory
from commonroad.prediction.prediction import TrajectoryPrediction
dynamic_obstacle_id = scenario.generate_object_id()
dynamic_obstacle_type = ObstacleType.CAR
# initial state has a time step of 0
dynamic_obstacle_initial_state = State(position = np.array([50.0, 0.0]),
velocity = 22,
orientation = 0.02,
time_step = 0)
# generate the states for the obstacle for time steps 1 to 40 by assuming constant velocity
state_list = []
for i in range(1, 40):
# compute new position
new_position = np.array([dynamic_obstacle_initial_state.position[0] + scenario.dt * i * 22, 0])
# create new state
new_state = State(position = new_position, velocity = 22,orientation = 0.02, time_step = i)
# add new state to state_list
state_list.append(new_state)
# create the trajectory of the obstacle, starting at time step 1
dynamic_obstacle_trajectory = Trajectory(1, state_list)
# create the prediction using the trajectory and the shape of the obstacle
dynamic_obstacle_shape = Rectangle(width = 1.8, length = 4.3)
dynamic_obstacle_prediction = TrajectoryPrediction(dynamic_obstacle_trajectory, dynamic_obstacle_shape)
# generate the dynamic obstacle according to the specification
dynamic_obstacle_id = scenario.generate_object_id()
dynamic_obstacle_type = ObstacleType.CAR
dynamic_obstacle = DynamicObstacle(dynamic_obstacle_id,
dynamic_obstacle_type,
dynamic_obstacle_shape,
dynamic_obstacle_initial_state,
dynamic_obstacle_prediction)
# add dynamic obstacle to the scenario
scenario.add_objects(dynamic_obstacle)
# plot the scenario for each time step
for i in range(0, 40):
# uncomment to clear previous graph
# display.clear_output(wait=True)
plt.figure(figsize=(25, 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:
## 3. Write XML file
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.
%% Cell type:code id: tags:
``` python
# import necesary classes from different modules
from commonroad.common.file_writer import CommonRoadFileWriter
from commonroad.common.file_writer import OverwriteExistingFile
author = 'Max Mustermann'
affiliation = 'Technical University of Munich, Germany'
source = ''
tags = 'highway multi_lane no_oncoming_traffic parallel_lanes comfort'
# write new scenario
fw = CommonRoadFileWriter(scenario, planning_problem_set, author, affiliation, source, tags)
filename = "../../solutions/ZAM_Tutorial-1_2_T-1.xml"
fw.write_to_file(filename, OverwriteExistingFile.ALWAYS)
```
%% Cell type:markdown id: tags:
We can open our stored file again to check if everything is correct:
%% Cell type:code id: tags:
``` python
file_path = "../../solutions/ZAM_Tutorial-1_2_T-1.xml"
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()
# plot the scenario for each time step