update tutorial, add plot grid

SMP/motion_planner/utility.py

@@ -9,6 +9,7 @@ __status__ = "Beta"
 import enum
 from typing import List, Dict, Tuple, Type
 
+import numpy as np
 import ipywidgets as widgets
 import matplotlib.pyplot as plt
 from IPython import display
@@ -191,7 +192,9 @@ def display_steps(scenario_data, config, algorithm, **args):
 
 
 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:
         list_x = [state.position[0] 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)):
 
         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.grid(alpha=0.5)
     plt.show()
 
 
@@ -251,7 +258,14 @@ def visualize_solution(scenario: Scenario, planning_problem_set: PlanningProblem
         draw_object(planning_problem_set)
         draw_object(dynamic_obstacle,
                     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.show()

tutorials/3_motion_primitive_generator/tutorial_motion_primitive_generator.ipynb

 %% Cell type:markdown id: tags:
 
 # Tutorial: Motion Primitive Generator
 
 This tutorial demonstrates how are the motion primitives used in solving motion planning problems generated.
 
 %% 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.
 
 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_directory: output directory of the generated motion primitives. The path can be either **relative** to this notebook or **absolute**.
 * **vehicle setting**:
     * vehicle_type_id: id of vehicle type. 1: FORD_ESCORT, 2: BMW_320i, 3: VW_VANAGON
 * **primitive setting**:
     * 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
     * velocity_sample_min: minimum sampling velocity [m/s].
     * velocity_sample_max: maximum sampling velocity [m/s].
     * 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_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
 * **sample trajectory setting**:
     * num_segment_trajectory: number of segments in sample trajectories
     * 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.
 
 %% Cell type:markdown id: tags:
 
 ## 1. Loading configuration file and parameters
 
 %% Cell type:code id: tags:
 
 ``` python
 %load_ext autoreload
 %autoreload 2
 
 import matplotlib.pyplot as plt
+import numpy as np
 from MotionPrimitiveGenerator import MotionPrimitiveGenerator as MPG
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 # specify path to configuration file
 path_file_config = "./generator_config.yaml"
 
 # load configuration file
 MPG.load_configuration(path_file_config)
 ```
 
 %% Cell type:markdown id: tags:
 
 ## 2. Generating motion primitives
 ### 2.1 Generating motion primitives which steers to one side only
 The attributes of the states in a motion primitive are:
 - x position
 - y position
 - steering angle
 - velocity
 - orientation
 - time step
 
 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:
 
 ``` python
 list_motion_primitives = MPG.generate_motion_primitives()
 ```
 
 %% Cell type:markdown id: tags:
 
 We plot the generated motion primitives:
 
 %% Cell type:code id: tags:
 
 ``` python
-plt.figure(figsize=(12, 3))
+fig = plt.figure(figsize=(12, 3))
+ax = fig.gca()
 
 for traj in list_motion_primitives:
     list_x = [state.position[0] for state in traj.state_list]
     list_y = [state.position[1] for state in traj.state_list]
     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.grid(alpha=0.5)
 plt.xlim((-1,11))
 plt.ylim((-1,2))
 plt.show()
 ```
 
 %% Cell type:markdown id: tags:
 
 ### 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.
 
 %% Cell type:code id: tags:
 
 ``` python
 list_motion_primitives_mirrored = MPG.create_mirrored_primitives(list_motion_primitives)
 print("Total number of primitives (mirrored included): ", len(list_motion_primitives_mirrored))
 ```
 
 %% Cell type:markdown id: tags:
 
 We now plot the final generated motion primitives.
 
 %% Cell type:code id: tags:
 
 ``` python
-plt.figure(figsize=(12, 5))
+fig = plt.figure(figsize=(12, 5))
+ax = fig.gca()
 
 for traj in list_motion_primitives_mirrored:
     list_x = [state.position[0] for state in traj.state_list]
     list_y = [state.position[1] for state in traj.state_list]
     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.grid(alpha=0.5)
 plt.xlim((-1,11))
 plt.ylim((-2,2))
 plt.show()
 ```
 
 %% Cell type:markdown id: tags:
 
 ### 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.
 
 %% Cell type:code id: tags:
 
 ``` python
 branching_factor_average = MPG.compute_branching_factor(list_motion_primitives_mirrored)
 print("Average branching factor of primitives: ", branching_factor_average)
 ```
 
 %% Cell type:markdown id: tags:
 
 ### 3. Generating sample trajectories
 We create some sample trajectories with the motion primitives that we have just generated.
 
 %% Cell type:code id: tags:
 
 ``` python
 MPG.generate_sample_trajectories(list_motion_primitives_mirrored)
 ```
 
 %% Cell type:markdown id: tags:
 
 ## 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.
 
 %% Cell type:code id: tags:
 
 ``` python
 MPG.save_motion_primitives(list_motion_primitives_mirrored)
 ```