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

update tutorial, add plot grid

parent 10b67a2c
...@@ -9,6 +9,7 @@ __status__ = "Beta" ...@@ -9,6 +9,7 @@ __status__ = "Beta"
import enum import enum
from typing import List, Dict, Tuple, Type from typing import List, Dict, Tuple, Type
import numpy as np
import ipywidgets as widgets import ipywidgets as widgets
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from IPython import display from IPython import display
...@@ -191,7 +192,9 @@ def display_steps(scenario_data, config, algorithm, **args): ...@@ -191,7 +192,9 @@ def display_steps(scenario_data, config, algorithm, **args):
def plot_primitives(list_primitives, figsize=(12, 3)): def plot_primitives(list_primitives, figsize=(12, 3)):
plt.figure(figsize=figsize) fig = plt.figure(figsize=figsize)
ax = fig.gca()
for primitive in list_primitives: for primitive in list_primitives:
list_x = [state.position[0] for state in primitive.trajectory.state_list] list_x = [state.position[0] for state in primitive.trajectory.state_list]
list_y = [state.position[1] for state in primitive.trajectory.state_list] list_y = [state.position[1] for state in primitive.trajectory.state_list]
...@@ -201,7 +204,11 @@ def plot_primitives(list_primitives, figsize=(12, 3)): ...@@ -201,7 +204,11 @@ def plot_primitives(list_primitives, figsize=(12, 3)):
plt.plot(list_x, list_y) plt.plot(list_x, list_y)
ax.set_xticks(np.arange(-5, 20, 0.5))
ax.set_yticks(np.arange(-5, 5., 0.5))
plt.axis('equal') plt.axis('equal')
plt.grid(alpha=0.5)
plt.show() plt.show()
...@@ -251,7 +258,14 @@ def visualize_solution(scenario: Scenario, planning_problem_set: PlanningProblem ...@@ -251,7 +258,14 @@ def visualize_solution(scenario: Scenario, planning_problem_set: PlanningProblem
draw_object(planning_problem_set) draw_object(planning_problem_set)
draw_object(dynamic_obstacle, draw_object(dynamic_obstacle,
draw_params={'time_begin': i, draw_params={'time_begin': i,
'dynamic_obstacle': {'shape': {'facecolor': 'green'}}}) 'dynamic_obstacle': {'shape': {'facecolor': 'green'},
'trajectory': {'draw_trajectory': True,
'facecolor': '#ff00ff',
'draw_continuous': True,
'z_order': 60,
'line_width': 5}
}
})
plt.gca().set_aspect('equal') plt.gca().set_aspect('equal')
plt.show() plt.show()
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
# Tutorial: Motion Primitive Generator # Tutorial: Motion Primitive Generator
This tutorial demonstrates how are the motion primitives used in solving motion planning problems generated. This tutorial demonstrates how are the motion primitives used in solving motion planning problems generated.
%% 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.
The configuration parameters related to our motion primitive generator are stored in **generator_config.yaml**. The configuration file contains the following parameters: The configuration parameters related to our motion primitive generator are stored in **generator_config.yaml**. The configuration file contains the following parameters:
* **output setting**: * **output setting**:
* output_directory: output directory of the generated motion primitives. The path can be either **relative** to this notebook or **absolute**. * output_directory: output directory of the generated motion primitives. The path can be either **relative** to this notebook or **absolute**.
* **vehicle setting**: * **vehicle setting**:
* vehicle_type_id: id of vehicle type. 1: FORD_ESCORT, 2: BMW_320i, 3: VW_VANAGON * vehicle_type_id: id of vehicle type. 1: FORD_ESCORT, 2: BMW_320i, 3: VW_VANAGON
* **primitive setting**: * **primitive setting**:
* duration: time length of trajectory [s]. * duration: time length of trajectory [s].
* dt_simulation: time step for forwad state simulation [s]. Note that CommonRoad scenarios have a discrete time step dt of 0.1 seconds * dt_simulation: time step for forwad state simulation [s]. Note that CommonRoad scenarios have a discrete time step dt of 0.1 seconds
* velocity_sample_min: minimum sampling velocity [m/s]. * velocity_sample_min: minimum sampling velocity [m/s].
* velocity_sample_max: maximum sampling velocity [m/s]. * velocity_sample_max: maximum sampling velocity [m/s].
* num_sample_velocity: number of velocity samples. * num_sample_velocity: number of velocity samples.
* steering_angle_sample_min: minimum sampling angle [rad]. Note that here we only consider steering to one side, as we will mirror the primitives afterwards. * steering_angle_sample_min: minimum sampling angle [rad]. Note that here we only consider steering to one side, as we will mirror the primitives afterwards.
* steering_angle_sample_max: maximum sampling angle [rad]. If set to 0, it will be assigned the maximum permissible value given by the selected vehicle parameter. * steering_angle_sample_max: maximum sampling angle [rad]. If set to 0, it will be assigned the maximum permissible value given by the selected vehicle parameter.
* num_sample_steering_angle: number of steering angle samples * num_sample_steering_angle: number of steering angle samples
* **sample trajectory setting**: * **sample trajectory setting**:
* num_segment_trajectory: number of segments in sample trajectories * num_segment_trajectory: number of segments in sample trajectories
* num_simulations: number of sample trajectories to be generated * num_simulations: number of sample trajectories to be generated
**Note**: Generating too sparse primitives (low branching factor) may restrict the search space such that no feasible solution can be found. On the other hand, generating too dense primitives (high branching factor) may dramatically inscrease the time of search. Thus striking a balance between diversity and efficiency is important here. **Note**: Generating too sparse primitives (low branching factor) may restrict the search space such that no feasible solution can be found. On the other hand, generating too dense primitives (high branching factor) may dramatically inscrease the time of search. Thus striking a balance between diversity and efficiency is important here.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## 1. Loading configuration file and parameters ## 1. Loading configuration file and parameters
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
%load_ext autoreload %load_ext autoreload
%autoreload 2 %autoreload 2
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np
from MotionPrimitiveGenerator import MotionPrimitiveGenerator as MPG from MotionPrimitiveGenerator import MotionPrimitiveGenerator as MPG
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# specify path to configuration file # specify path to configuration file
path_file_config = "./generator_config.yaml" path_file_config = "./generator_config.yaml"
# load configuration file # load configuration file
MPG.load_configuration(path_file_config) MPG.load_configuration(path_file_config)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## 2. Generating motion primitives ## 2. Generating motion primitives
### 2.1 Generating motion primitives which steers to one side only ### 2.1 Generating motion primitives which steers to one side only
The attributes of the states in a motion primitive are: The attributes of the states in a motion primitive are:
- x position - x position
- y position - y position
- steering angle - steering angle
- velocity - velocity
- orientation - orientation
- time step - time step
Here we only generate motion primitives with positive steering angles, as the other half can be easily obtained by mirroring them. Here we only generate motion primitives with positive steering angles, as the other half can be easily obtained by mirroring them.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
list_motion_primitives = MPG.generate_motion_primitives() list_motion_primitives = MPG.generate_motion_primitives()
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We plot the generated motion primitives: We plot the generated motion primitives:
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
plt.figure(figsize=(12, 3)) fig = plt.figure(figsize=(12, 3))
ax = fig.gca()
for traj in list_motion_primitives: for traj in list_motion_primitives:
list_x = [state.position[0] for state in traj.state_list] list_x = [state.position[0] for state in traj.state_list]
list_y = [state.position[1] for state in traj.state_list] list_y = [state.position[1] for state in traj.state_list]
plt.plot(list_x, list_y) plt.plot(list_x, list_y)
ax.set_xticks(np.arange(-5, 20, 0.5))
ax.set_yticks(np.arange(-5, 5., 0.5))
plt.axis('equal') plt.axis('equal')
plt.grid(alpha=0.5)
plt.xlim((-1,11)) plt.xlim((-1,11))
plt.ylim((-1,2)) plt.ylim((-1,2))
plt.show() plt.show()
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### 2.2 Mirroring motion primitives ### 2.2 Mirroring motion primitives
As we only computed primitives that have a positive steering angle, we now mirror them to get the other half of the feasible primitives. As we only computed primitives that have a positive steering angle, we now mirror them to get the other half of the feasible primitives.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
list_motion_primitives_mirrored = MPG.create_mirrored_primitives(list_motion_primitives) list_motion_primitives_mirrored = MPG.create_mirrored_primitives(list_motion_primitives)
print("Total number of primitives (mirrored included): ", len(list_motion_primitives_mirrored)) print("Total number of primitives (mirrored included): ", len(list_motion_primitives_mirrored))
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We now plot the final generated motion primitives. We now plot the final generated motion primitives.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
plt.figure(figsize=(12, 5)) fig = plt.figure(figsize=(12, 5))
ax = fig.gca()
for traj in list_motion_primitives_mirrored: for traj in list_motion_primitives_mirrored:
list_x = [state.position[0] for state in traj.state_list] list_x = [state.position[0] for state in traj.state_list]
list_y = [state.position[1] for state in traj.state_list] list_y = [state.position[1] for state in traj.state_list]
plt.plot(list_x, list_y) plt.plot(list_x, list_y)
ax.set_xticks(np.arange(-5, 20, 0.5))
ax.set_yticks(np.arange(-5, 5., 0.5))
plt.axis('equal') plt.axis('equal')
plt.grid(alpha=0.5)
plt.xlim((-1,11)) plt.xlim((-1,11))
plt.ylim((-2,2)) plt.ylim((-2,2))
plt.show() plt.show()
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### 2.3 Checking average branching factor of the generated motion primitives ### 2.3 Checking average branching factor of the generated motion primitives
We can inspect the average branching factor of the generated primitives to have a rough idea how many successors a given motion primitive can have. We can inspect the average branching factor of the generated primitives to have a rough idea how many successors a given motion primitive can have.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
branching_factor_average = MPG.compute_branching_factor(list_motion_primitives_mirrored) branching_factor_average = MPG.compute_branching_factor(list_motion_primitives_mirrored)
print("Average branching factor of primitives: ", branching_factor_average) print("Average branching factor of primitives: ", branching_factor_average)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### 3. Generating sample trajectories ### 3. Generating sample trajectories
We create some sample trajectories with the motion primitives that we have just generated. We create some sample trajectories with the motion primitives that we have just generated.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
MPG.generate_sample_trajectories(list_motion_primitives_mirrored) MPG.generate_sample_trajectories(list_motion_primitives_mirrored)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## 4. Saving motion primitives to XML files ## 4. Saving motion primitives to XML files
We save the generated motion primitves to XML files, which are output tothe directory specified in the configuration file. We save the generated motion primitves to XML files, which are output tothe directory specified in the configuration file.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
MPG.save_motion_primitives(list_motion_primitives_mirrored) MPG.save_motion_primitives(list_motion_primitives_mirrored)
``` ```
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment