Notice to GitKraken users: A vulnerability has been found in the SSH key generation of GitKraken versions 7.6.0 to 8.0.0 (https://www.gitkraken.com/blog/weak-ssh-key-fix). If you use GitKraken and have generated a SSH key using one of these versions, please remove it both from your local workstation and from your LRZ GitLab profile.

21.10.2021, 9:00 - 11:00: Due to updates GitLab may be unavailable for some minutes between 09:00 and 11:00.

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

change placement of motion primitive generator/

parent 28ff4ce0
# paths can be either relative or absolute, units shown in brackets []
outputs:
# output directory for generated motion primitives
output_directory: ../motion_primitives/
vehicles:
# id of vehicle. 1: FORD_ESCORT, 2: BMW_320i, 3: VW_VANAGON
veh_type_id: 2
primitives:
# time length of trajectory [s]
T: 0.5
# time step for forward state simulation [s]
# note that commonroad scenarios have a discrete time step dt of 0.1 seconds; for higher accuracy
# of forward simulation, here dt is set to 0.05. the simulated states will be downsampled afterwards
dt_simulation: 0.05
# sampling range [m/s]
velocity_sample_min: 0.0
velocity_sample_max: 20.0
# for a step of 1.0 m/s, num = (v_max - v_min) / 1.0 + 1
num_sample_velocity: 21
# steer to one side only, we can mirror the primitives afterwards [rad]
steering_angle_sample_min: 0.0
# if steering_angle_sample_max set to 0, it will be assigned the maximum value
# given by the selected vehicle parameter
steering_angle_sample_max: 0.0
num_sample_steering_angle: 9
sample_trajectories:
# number of segments of trajectory
num_segment_trajectory: 10
# number of sample trajectories to be generated
num_simulations: 2
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Tutorial: Batch Processing"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This tutorial shows how [Batch Processing](https://gitlab.lrz.de/tum-cps/commonroad_io/tree/batch_processing) is used to solve for solutions to different scenarios. It is composed of \n",
"* a configuration file (.yaml format),\n",
"* some helper functions,\n",
"* and the script below to call the planer function.\n",
"\n",
"Before proceeding, you should make sure the configuration file is set correctly. Here are the explanation for each of the parameters in the configuration file:\n",
"* **input_path**: input directory of CommonRoad scenarios that you indend to solve.\n",
"* **output_path**: output directory of the solution files.\n",
"* **overwrite**: flag to determine whether the existing solution files should be overwritten.\n",
"* **timeout**: timeout time for your motion planner, unit in seconds\n",
"* **motion_planner_path**: directory where the module containing the function to execute your motion planner is located\n",
"* **motion_planner_module_name**: name of the module that contains the function to execute your motion planner\n",
"* **motion_planner_function_name**: name of the function that executes your motion planner\n",
"* **default**: parameters specified under this section will be applied to all scenarios. If you wish to specify a different paramter for specific scenarios, simply copy the section and replace 'default' with the id of your scenario.\n",
"* **vehicle_model**: model of the vehicle, valid values: **PM, KS, ST and MB**. Please refer to [Vehicle Models](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf) for more information.\n",
"* **vehicle_type**: type of the vehicle, valid values: **FORD_ESCORT, BMW_320i and VW_VANAGON**.\n",
"* **cost_function**: identifier of cost function. Please refer to [Cost Functions](https://gitlab.lrz.de/tum-cps/commonroad-cost-functions/blob/master/costFunctions_commonRoad.pdf) for more information.\n",
"* **planner_id**: id of the planner that is used to solve for solutions. in this example, 1 = Greedy Best First Search, 2 = A*, else = Your own planner.\n",
"* **planning_problem_idx**: planner problem index. for cooperative scenarios: 0, 1, 2, ..., otherwise: 0\n",
"* **max_tree_depth**: maximum permissible depth of the search tree\n",
"\n",
"Note: the paths can be either **relative** to this script or **absolute**.\n",
"\n",
"Simply run the following script to start batch processing. The results here is just exemplary, you will see different output as your configuration varies."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Total number of files to be processed: 10\n",
"Timeout setting: 60 seconds\n",
"\n",
"Processing file No. 1 / 10. Scenario ID: RUS_Bicycle-3_3_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"=========> Motion planner timeout.\n",
"<FAILURE> Solution for planning problem with ID=9 is not provided. Skipped.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 2 / 10. Scenario ID: RUS_Bicycle-2_2_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 4.07 seconds\n",
"Planning finished.\n",
"<SUCCESS> Solution written.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 3 / 10. Scenario ID: RUS_Bicycle-1_2_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 3.46 seconds\n",
"Planning finished.\n",
"<SUCCESS> Solution written.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 4 / 10. Scenario ID: RUS_Bicycle-3_1_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 25.81 seconds\n",
"Planning finished.\n",
"<SUCCESS> Solution written.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 5 / 10. Scenario ID: RUS_Bicycle-2_1_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 3.4 seconds\n",
"Planning finished.\n",
"<SUCCESS> Solution written.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 6 / 10. Scenario ID: RUS_Bicycle-5_1_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 5.95 seconds\n",
"Planning finished.\n",
"<SUCCESS> Solution written.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 7 / 10. Scenario ID: RUS_Bicycle-4_2_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"=========> Motion planner timeout.\n",
"<FAILURE> Solution for planning problem with ID=15 is not provided. Skipped.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 8 / 10. Scenario ID: RUS_Bicycle-3_2_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 0.65 seconds\n",
"Planning finished.\n",
"<FAILURE> Solution for planning problem with ID=9 is not provided. Skipped.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 9 / 10. Scenario ID: RUS_Bicycle-1_1_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"Solving this scenario took 3.1 seconds\n",
"Planning finished.\n",
"<SUCCESS> Solution written.\n",
"\n",
"=========================================================\n",
"\n",
"Processing file No. 10 / 10. Scenario ID: RUS_Bicycle-4_1_T-1\n",
"Reading motion primitives...\n",
"Automata created.\n",
"Number of loaded primitives: 3066\n",
"Planning..\n",
"=========> Motion planner timeout.\n",
"<FAILURE> Solution for planning problem with ID=15 is not provided. Skipped.\n",
"\n",
"=========================================================\n",
"\n"
]
}
],
"source": [
"# import helper functions\n",
"import helper_functions\n",
"\n",
"# specify path to configuration file\n",
"path_file_config = \"../configuration/batch_processing_config.yaml\"\n",
"\n",
"# load configuration file\n",
"configuration = helper_functions.load_configuration(path_file_config)\n",
"\n",
"# get target function\n",
"function_target = helper_functions.get_target_function(configuration)\n",
"\n",
"# get a list of scenario files\n",
"list_files_input = helper_functions.get_input_files(configuration)\n",
"\n",
"# get length of the list and time before timeout\n",
"num_files = len(list_files_input)\n",
"time_timeout = configuration['timeout']\n",
"\n",
"print(\"Total number of files to be processed: {}\".format(num_files))\n",
"print(\"Timeout setting: {} seconds\\n\".format(time_timeout))\n",
"count_processed = 0\n",
"\n",
"# iterate through scenarios\n",
"for file_scenario in list_files_input:\n",
" count_processed += 1\n",
" print(\"Processing file No. {} / {}. Scenario ID: {}\".format(count_processed, num_files, file_scenario[:-4]))\n",
" # parse the scenario file\n",
" result_parse = helper_functions.parse_scenario_file(configuration, file_scenario)\n",
" # execute target function\n",
" solution_trajectories = helper_functions.execute_target_function(function_target, result_parse, time_timeout)\n",
" # save solution file\n",
" helper_functions.save_solution(configuration, solution_trajectories, result_parse)\n",
" print(\"\\n=========================================================\\n\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python (cr37)",
"language": "python",
"name": "cr37"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.7.3"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Tutorial: Motion Primitives Generator\n",
"\n",
"This tutorial demonstrates how are the motion primitives generated. You can find all related files in the [CommonRoad Search](https://gitlab.lrz.de/tum-cps/commonroad-search) repository, under folder GSMP/tools/motion_primitive_generator/.\n",
"\n",
"The main components are:\n",
"* **motion_primitives_config.yaml** in which the configuration is set;\n",
"* **implementation.py** in which the main functions are implemented; and\n",
"* the script below to execute the functions.\n",
"\n",
"Before proceeding, you should make sure the configuration file is set correctly, which consists of the following parameters:\n",
"* **output setting**:\n",
" * output_directory: output directory for generated motion primitives.\n",
"* **vehicle setting**:\n",
" * veh_type_id: id of vehicle type. 1: FORD_ESCORT, 2: BMW_320i, 3: VW_VANAGON\n",
"* **primitive setting**:\n",
" * T: time length of trajectory [s].\n",
" * dt_simulation: time step for forwad state simulation [s].\n",
" * velocity_sample_min: minimum sampling velocity [m/s].\n",
" * velocity_sample_max: maximum sampling velocity [m/s].\n",
" * num_sample_velocity: number of velocity samples.\n",
" * 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.\n",
" * 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.\n",
" * num_sample_steering_angle: number of steering angle samples\n",
"* **sample trajectory setting**: \n",
" * num_segment_trajectory: number of segments in sample trajectories\n",
" * num_simulations: number of sample trajectories to be generated\n",
"\n",
"Note: the paths can be either **relative** to this script, or **absolute**."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Load configuration file and parameters"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%load_ext autoreload\n",
"%autoreload 2\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"# import helper functions\n",
"from implementation import Parameter, load_configuration, generate_motion_primitives, create_mirrored_primitives, \\\n",
"save_motion_primitives, compute_number_successors_average, compute_number_successors_average, \\\n",
"generate_sample_trajectories\n",
"\n",
"# specify path to configuration file\n",
"path_file_config = \"../configuration/motion_primitives_config.yaml\"\n",
"\n",
"# load configuration file\n",
"configuration = load_configuration(path_file_config)\n",
"\n",
"# parse configuration and generate parameter object\n",
"parameters = Parameter(configuration)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Generate motion primitives\n",
"The attributes of each of the states in a motion primitive are: x position, y position, steering angle, velocity in x direction, orientation and time step"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"list_traj_accepted = generate_motion_primitives(parameters)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Plot generated motion primitives"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"for traj in list_traj_accepted:\n",
" list_x = [state.position[0] for state in traj.state_list]\n",
" list_y = [state.position[1] for state in traj.state_list]\n",
" plt.plot(list_x, list_y)\n",
" \n",
"plt.axis('equal')\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Create mirrored primitives\n",
"As we only computed primitives that have a positive (negative) steering angle, we mirror them to get the other half of primitives."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"list_traj_accepted_mirrored = create_mirrored_primitives(list_traj_accepted, parameters)\n",
"print(\"Total number of primitives (mirrored included): \", len(list_traj_accepted_mirrored))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Check average number of successors\n",
"We can inspect how many successors does every primitive have on average."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"num_average_successors = compute_number_successors_average(list_traj_accepted_mirrored)\n",
"print(\"average number of successors: \", num_average_successors)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Plot final motion primitives\n",
"We can plot the final generated motion primitives."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# fig = plt.figure()\n",
"for traj in list_traj_accepted_mirrored:\n",
" list_x = [state.position[0] for state in traj.state_list]\n",
" list_y = [state.position[1] for state in traj.state_list]\n",
" plt.plot(list_x, list_y)\n",
" \n",
"plt.axis('equal')\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Generate sample trajectories\n",
"We can generate sample trajectories with the motion primitives that we have just generated"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"generate_sample_trajectories(list_traj_accepted_mirrored, parameters)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Save the motion primitives into xml file\n",
"The xml containing generated motion primitives are output to the directory specified in the configuration"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"save_motion_primitives(list_traj_accepted_mirrored, parameters)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.7.6"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
import os
import sys
sys.path.append("../vehicle_model/")
import yaml
import xml.etree.ElementTree as et
from xml.dom import minidom
import copy
import random
import itertools
import numpy as np
import matplotlib.pyplot as plt
from math import atan2, sin, cos
# vehicle model
from vehicleDynamics_KS import vehicleDynamics_KS
from parameters_vehicle1 import parameters_vehicle1
from parameters_vehicle2 import parameters_vehicle2
from parameters_vehicle3 import parameters_vehicle3
# solution checker
from solution_checker import TrajectoryValidator, KSTrajectory, KinematicSingleTrackModel
# commonroad objects
from commonroad.scenario.trajectory import State, Trajectory
# solution writer
from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction
class Parameter:
'''
A class to hold all parameters
'''
def __init__(self, configuration):
# outputs
self.directory_output = configuration['outputs']['output_directory']
# vehicle related
self.veh_type_id = configuration['vehicles']['veh_type_id']
if self.veh_type_id == 1:
self.veh_type = VehicleType.FORD_ESCORT
self.veh_param = parameters_vehicle1()
self.veh_name = "FORD_ESCORT"
elif self.veh_type_id == 2:
self.veh_type = VehicleType.BMW_320i
self.veh_param = parameters_vehicle2()
self.veh_name = "BMW320i"
elif self.veh_type_id == 3:
self.veh_type = VehicleType.VW_VANAGON
self.veh_param = parameters_vehicle3()
self.veh_name = "VW_VANAGON"
# time
self.T = configuration['primitives']['T']
self.dt_simulation = configuration['primitives']['dt_simulation']
# times 100 for two significant digits accuracy, turns into centi-seconds
self.time_stamps = ((np.arange(0, self.T, self.dt_simulation) + self.dt_simulation) * 100).astype(int)
# velocity
self.velocity_sample_min = configuration['primitives']['velocity_sample_min']
self.velocity_sample_max = configuration['primitives']['velocity_sample_max']
self.num_sample_velocity = configuration['primitives']['num_sample_velocity']
# steering angle
self.steering_angle_sample_min = configuration['primitives']['steering_angle_sample_min']
self.steering_angle_sample_max = configuration['primitives']['steering_angle_sample_max']
self.num_sample_steering_angle = configuration['primitives']['num_sample_steering_angle']
if int(self.steering_angle_sample_max) == 0:
self.steering_angle_sample_max = self.veh_param.steering.max
# sample trajectories
self.num_segment_trajectory = configuration['sample_trajectories']['num_segment_trajectory']
self.num_simulations = configuration['sample_trajectories']['num_simulations']
def load_configuration(path_file_config):
'''
load input configuration file
'''
with open(path_file_config, 'r') as stream:
try:
configuration = yaml.load(stream)
return configuration
except yaml.YAMLError as exc:
print(exc)