change placement of motion primitive generator/

GSMP/tools/motion_primitive_generator/configuration/motion_primitives_config.yaml

+# 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

GSMP/tools/motion_primitive_generator/script/.ipynb_checkpoints/batch_processing-checkpoint.ipynb

+{
+ "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
+}

GSMP/tools/motion_primitive_generator/script/.ipynb_checkpoints/motion_primitives_generator-checkpoint.ipynb

+{
+ "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
+}

GSMP/tools/motion_primitive_generator/script/implementation.py

+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)
+
+