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

minor update

parent 25832298
...@@ -2,12 +2,12 @@ ...@@ -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 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: primitives to solve [CommonRoad](https://commonroad.in.tum.de/) scenarios. The following search algorithms have been implemented as examples:
1. Breadth First Search - Breadth First Search
2. Depth First Search - Depth First Search
3. Depth-limited Search - Depth-limited Search
4. Uniform Cost Search (aka Dijkstra's algorithm) - Uniform Cost Search (aka Dijkstra's algorithm)
5. Greedy Best First Search - Greedy Best First Search
6. A* 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: 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: ...@@ -56,14 +56,14 @@ Then, install the dependencies with:
$ pip install -r requirements.txt $ 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). 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 ```sh
$ git clone https://gitlab.lrz.de/tum-cps/commonroad-drivability-checker.git $ git clone https://gitlab.lrz.de/tum-cps/commonroad-drivability-checker.git
$ cd commonroad-drivability-checker $ 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). `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: ...@@ -65,7 +65,7 @@ scenario_loader:
# DEFAULT: loads all scenarios from the specified scenario folder # DEFAULT: loads all scenarios from the specified scenario folder
# RANDOM: loads randomly selected scenarios from the specified scenario folder # RANDOM: loads randomly selected scenarios from the specified scenario folder
# SPECIFIC: loads specified scenarios, see below # 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 # number of scenarios to be randomly selected. this only works for inputmode set to RANDOM
random_count: 3 random_count: 3
......
import warnings import warnings
from typing import List
from copy import deepcopy from copy import deepcopy
from typing import List
import numpy as np import numpy as np
from commonroad.common.solution import VehicleModel, VehicleType
from commonroad.scenario.trajectory import Trajectory, State from commonroad.scenario.trajectory import Trajectory, State
from commonroad.common.solution_writer import VehicleModel, VehicleType
class MotionPrimitive: class MotionPrimitive:
......
...@@ -14,8 +14,9 @@ from SMP.route_planner.route_planner.utils_route import chaikins_corner_cutting, ...@@ -14,8 +14,9 @@ from SMP.route_planner.route_planner.utils_route import chaikins_corner_cutting,
try: try:
import pycrccosy import pycrccosy
except ModuleNotFoundError as exp: except ModuleNotFoundError as exp:
warnings.warn(f"""You won't be able to use the Curvilinear Coordinate System for the Navigator, pass
the calculations won't be precise. {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}""")
class RouteType(Enum): class RouteType(Enum):
......
...@@ -25,8 +25,9 @@ from SMP.route_planner.route_planner.route import RouteType, RouteCandidateHolde ...@@ -25,8 +25,9 @@ from SMP.route_planner.route_planner.route import RouteType, RouteCandidateHolde
try: try:
import pycrccosy import pycrccosy
except ModuleNotFoundError as exp: except ModuleNotFoundError as exp:
warnings.warn(f"""You won't be able to use the Curvilinear Coordinate System for the Navigator, pass
the calculations won't be precise. {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}""")
class RoutePlanner: class RoutePlanner:
......
...@@ -24,7 +24,7 @@ ...@@ -24,7 +24,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
......
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
# Tutorial: Uninformed Search Algorithms # 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. 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: %% Cell type:markdown id: tags:
## 0. Preparation ## 0. Preparation
Before you proceed with this tutorial, make sure that Before you proceed with this tutorial, make sure that
* you have gone through the tutorial for **CommonRoad Input-Output**. * you have gone through the tutorial for **CommonRoad Input-Output**.
* you have installed all necessary modules for **CommonRoad Search** according to the installation manual. * 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. Let's start with importing relevant modules and classes for setting up the automaton and the CommonRoad (CR) scenario.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
%matplotlib inline %matplotlib inline
%load_ext autoreload %load_ext autoreload
%autoreload 2 %autoreload 2
import os import os
import sys import sys
path_notebook = os.getcwd() path_notebook = os.getcwd()
# add the SMP folder to python path # add the SMP folder to python path
sys.path.append(os.path.join(path_notebook, "../../")) sys.path.append(os.path.join(path_notebook, "../../"))
# add the 1_search_algorithms folder to python path # add the 1_search_algorithms folder to python path
sys.path.append(os.path.join(path_notebook, "../")) sys.path.append(os.path.join(path_notebook, "../"))
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from commonroad.common.file_reader import CommonRoadFileReader from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.draw_dispatch_cr import draw_object from commonroad.visualization.draw_dispatch_cr import draw_object
from SMP.motion_planner.motion_planner import MotionPlanner from SMP.motion_planner.motion_planner import MotionPlanner
from SMP.maneuver_automaton.maneuver_automaton import ManeuverAutomaton from SMP.maneuver_automaton.maneuver_automaton import ManeuverAutomaton
from SMP.motion_planner.utility import plot_primitives, display_steps from SMP.motion_planner.utility import plot_primitives, display_steps
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## 1. Loading CR Scenario and Planning Problem ## 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: 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 * **Dot**: initial state projected onto the position domain
* **Red rectangle**: static obstacle * **Red rectangle**: static obstacle
* **Yellow rectangle**: goal region projected onto the position domain * **Yellow rectangle**: goal region projected onto the position domain
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# load scenario # load scenario
path_scenario = os.path.join(path_notebook, "../../scenarios/tutorial/") path_scenario = os.path.join(path_notebook, "../../scenarios/tutorial/")
id_scenario = 'ZAM_Tutorial_Urban-3_2' id_scenario = 'ZAM_Tutorial_Urban-3_2'
# read in scenario and planning problem set # read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open() scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set # retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0] planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
# plot the scenario and the planning problem set # plot the scenario and the planning problem set
plt.figure(figsize=(15, 5)) plt.figure(figsize=(15, 5))
draw_object(scenario) draw_object(scenario)
draw_object(planning_problem_set) draw_object(planning_problem_set)
plt.gca().set_aspect('equal') plt.gca().set_aspect('equal')
plt.margins(0, 0) plt.margins(0, 0)
plt.show() plt.show()
``` ```
%%%% Output: display_data
![](data:image/png;base64,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)
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## 2. Generating a Maneuver Automaton ## 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. 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 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 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 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. * 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. * 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: %% Cell type:code id: tags:
``` python ``` python
# load the xml with stores the motion primitives # 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' 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 # generate automaton
automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives) automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)
# plot motion primitives # plot motion primitives
plot_primitives(automaton.list_primitives) plot_primitives(automaton.list_primitives)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## 3. Search algorithms ## 3. Search algorithms
Next, we demonstrate the search results for the following algorithms: Next, we demonstrate the search results for the following algorithms:
1. Breadth-First Search (BFS) 1. Breadth-First Search (BFS)
2. Depth-First Search (DFS) 2. Depth-First Search (DFS)
3. Depth-Limited Search (DLS) 3. Depth-Limited Search (DLS)
4. Uniform-Cost Search (UCS) 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/** 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: 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 solid:** frontier
* **Yellow dashed:** collision * **Yellow dashed:** collision
* **Red solid:** currently exploring * **Red solid:** currently exploring
* **Gray solid:** explored * **Gray solid:** explored
* **Green solid:** final solution * **Green solid:** final solution
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### 3.1 Breadth-First Search (BFS) ### 3.1 Breadth-First Search (BFS)
The BFS algorithm uses a FIFO (First-In First-Out) queue for pushing the nodes. The BFS algorithm uses a FIFO (First-In First-Out) queue for pushing the nodes.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# construct motion planner # construct motion planner
planner_BFS = MotionPlanner.BreadthFirstSearch(scenario=scenario, planner_BFS = MotionPlanner.BreadthFirstSearch(scenario=scenario,
planning_problem=planning_problem, planning_problem=planning_problem,
automaton=automaton) automaton=automaton)
# prepare input for visualization # prepare input for visualization
scenario_data = (scenario, planner_BFS.state_initial, planner_BFS.shape_ego, planning_problem) scenario_data = (scenario, planner_BFS.state_initial, planner_BFS.shape_ego, planning_problem)
# display search steps # display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_BFS.execute_search, display_steps(scenario_data=scenario_data, algorithm=planner_BFS.execute_search,
config=planner_BFS.config_plot) config=planner_BFS.config_plot)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### 3.2 Depth-First Search (DFS) ### 3.2 Depth-First Search (DFS)
The DFS algorithm uses a LIFO (Last-In First-Out) queue for pushing the nodes. The DFS algorithm uses a LIFO (Last-In First-Out) queue for pushing the nodes.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# construct motion planner # construct motion planner
planner_DFS = MotionPlanner.DepthFirstSearch(scenario=scenario, planner_DFS = MotionPlanner.DepthFirstSearch(scenario=scenario,
planning_problem=planning_problem, planning_problem=planning_problem,
automaton=automaton) automaton=automaton)
# prepare input for visualization # prepare input for visualization
scenario_data = (scenario, planner_DFS.state_initial, planner_DFS.shape_ego, planning_problem) scenario_data = (scenario, planner_DFS.state_initial, planner_DFS.shape_ego, planning_problem)
# display search steps # display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_DFS.execute_search, display_steps(scenario_data=scenario_data, algorithm=planner_DFS.execute_search,
config=planner_DFS.config_plot) config=planner_DFS.config_plot)
``` ```
%% Cell type:markdown id: tags: %% 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. 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: %% Cell type:code id: tags:
``` python ``` python
import numpy as np import numpy as np
# read in scenario and planning problem set # read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open() scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set # retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0] planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
for state in planning_problem.goal.state_list: for state in planning_problem.goal.state_list:
state.position = state.position.translate_rotate(np.array([0, 4]), 0) state.position = state.position.translate_rotate(np.array([0, 4]), 0)
# Plot the scenario and the planning problem set # Plot the scenario and the planning problem set
plt.figure(figsize=(15, 5)) plt.figure(figsize=(15, 5))
draw_object(scenario) draw_object(scenario)
draw_object(planning_problem_set) draw_object(planning_problem_set)
plt.gca().set_aspect('equal') plt.gca().set_aspect('equal')
plt.margins(0, 0) plt.margins(0, 0)
plt.show() plt.show()
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# construct motion planner # construct motion planner
planner_DFS = MotionPlanner.DepthFirstSearch(scenario=scenario, planner_DFS = MotionPlanner.DepthFirstSearch(scenario=scenario,
planning_problem=planning_problem, planning_problem=planning_problem,
automaton=automaton) automaton=automaton)
# prepare input for visualization # prepare input for visualization
scenario_data = (scenario, planner_DFS.state_initial, planner_DFS.shape_ego, planning_problem) scenario_data = (scenario, planner_DFS.state_initial, planner_DFS.shape_ego, planning_problem)
# display search steps # display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_DFS.execute_search, display_steps(scenario_data=scenario_data, algorithm=planner_DFS.execute_search,
config=planner_DFS.config_plot) config=planner_DFS.config_plot)
``` ```
%% Cell type:markdown id: tags: %% 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. 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). To overcome this problem, we introduce a depth limit, resulting in Depth-Limited Search (DLS).
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### 3.3 Depth-Limited Search (DLS) ### 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. 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: %% Cell type:code id: tags:
``` python ``` python
limit_depth = 7 limit_depth = 7
# construct motion planner # construct motion planner
planner_DLS = MotionPlanner.DepthLimitedSearch(scenario=scenario, planner_DLS = MotionPlanner.DepthLimitedSearch(scenario=scenario,
planning_problem=planning_problem, planning_problem=planning_problem,
automaton=automaton) automaton=automaton)
# prepare input for visualization # prepare input for visualization
scenario_data = (scenario, planner_DLS.state_initial, planner_DLS.shape_ego, planning_problem) scenario_data = (scenario, planner_DLS.state_initial, planner_DLS.shape_ego, planning_problem)
# display search steps # display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_DLS.execute_search, display_steps(scenario_data=scenario_data, algorithm=planner_DLS.execute_search,
config=planner_DLS.config_plot, limit_depth=limit_depth) config=planner_DLS.config_plot, limit_depth=limit_depth)
``` ```
%% Cell type:markdown id: tags: %% 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. 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: %% Cell type:markdown id: tags:
### 3.4 Uniform-Cost Search (UCS) ### 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. 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). 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: %% Cell type:code id: tags:
``` python ``` python
# read in scenario and planning problem set # read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open() scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()
# retrieve the first planning problem in the problem set # retrieve the first planning problem in the problem set
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0] planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]
# construct motion planner # construct motion planner
planner_UCS = MotionPlanner.UniformCostSearch(scenario=scenario, planner_UCS = MotionPlanner.UniformCostSearch(scenario=scenario,
planning_problem=planning_problem, planning_problem=planning_problem,
automaton=automaton) automaton=automaton)
# prepare input for visualization # prepare input for visualization
scenario_data = (scenario, planner_UCS.state_initial, planner_UCS.shape_ego, planning_problem) scenario_data = (scenario, planner_UCS.state_initial, planner_UCS.shape_ego, planning_problem)
# display search steps # display search steps
display_steps(scenario_data=scenario_data, algorithm=planner_UCS.execute_search, display_steps(scenario_data=scenario_data, algorithm=planner_UCS.execute_search,
config=planner_UCS.config_plot) config=planner_UCS.config_plot)
``` ```