├── PGame ├── scripts │ ├── motion_primitive_planner │ │ ├── __init__.py │ │ ├── parameters.py │ │ ├── math │ │ │ ├── polynomial.py │ │ │ └── cubic_spline.py │ │ ├── utils.py │ │ └── mpp.py │ └── mpp_node.py ├── launch │ └── mpp.launch ├── setup.py ├── package.xml └── CMakeLists.txt ├── README.md ├── LICENSE └── common ├── collision_check.py ├── collision_check_jax.py └── test.ipynb /PGame/scripts/motion_primitive_planner/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PGame/launch/mpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /PGame/setup.py: -------------------------------------------------------------------------------- 1 | """ 2 | Setup for simulator package 3 | """ 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | d = generate_distutils_setup( 8 | packages=['motion_primitive_planner'], 9 | package_dir={'': 'scripts'} 10 | ) 11 | 12 | setup(**d) 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GT-BMPC 2 | This repository contains the code for the paper titled **"Automated Lane Merging via Game Theory and Branch Model Predictive Control"**, submitted to IEEE T-CST. 3 | 4 | ## Overview 5 | 6 | This repository provides the implementation of the proposed method along with the baseline approaches. The code has been tested on Ubuntu 20.04 with ROS Noetic. 7 | 8 | **Code Availability** 9 | 10 | Please note that only a portion of the code has been uploaded to the repository currently. Specifically, you can find the reproduced baseline potential game in the following folder: [GT-BMPC/PGame](https://github.com/SailorBrandon/GT-BMPC/tree/main/PGame). 11 | 12 | We are actively working on preparing and releasing the remaining code. We appreciate your patience and will update the repository as soon as the additional code is available. 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Shaohang Han 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /common/collision_check.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def is_separating_axis(n, P1, P2): 4 | min1, max1 = float('+inf'), float('-inf') 5 | min2, max2 = float('+inf'), float('-inf') 6 | for v in P1: 7 | proj = np.dot(v, n) 8 | min1 = min(min1, proj) 9 | max1 = max(max1, proj) 10 | for v in P2: 11 | proj = np.dot(v, n) 12 | min2 = min(min2, proj) 13 | max2 = max(max2, proj) 14 | 15 | if max1 >= min2 and max2 >= min1: 16 | return False 17 | 18 | return True 19 | 20 | def find_edges_norms(P): 21 | edges = [] 22 | norms = [] 23 | num_edge = len(P) 24 | for i in range(num_edge): 25 | edge = P[(i + 1)%num_edge] - P[i] 26 | norm = np.array([-edge[1], edge[0]]) 27 | edges.append(edge) 28 | norms.append(norm) 29 | return edges, norms 30 | 31 | def collide(P1, P2): 32 | """ 33 | Check if two polygons overlap. 34 | We follow https://hackmd.io/@US4ofdv7Sq2GRdxti381_A/ryFmIZrsl?type=view 35 | Args: 36 | p1 (List): List of the vertices of a polygon 37 | p2 (List): List of the vertices of a polygon 38 | """ 39 | 40 | P1 = [np.array(v, 'float64') for v in P1] 41 | P2 = [np.array(v, 'float64') for v in P2] 42 | 43 | _, norms1 = find_edges_norms(P1) 44 | _, norms2 = find_edges_norms(P2) 45 | norms = norms1 + norms2 46 | for n in norms: 47 | if is_separating_axis(n, P1, P2): 48 | return False 49 | 50 | return True -------------------------------------------------------------------------------- /common/collision_check_jax.py: -------------------------------------------------------------------------------- 1 | import os 2 | os.environ['JAX_PLATFORMS'] = 'cpu' 3 | import jax 4 | import jax.numpy as jnp 5 | 6 | @jax.jit 7 | def is_separating_axis(n, P1, P2): 8 | # Project points of P1 onto the axis n 9 | projections1 = jnp.dot(P1, n) 10 | min1 = jnp.min(projections1) 11 | max1 = jnp.max(projections1) 12 | 13 | # Project points of P2 onto the axis n 14 | projections2 = jnp.dot(P2, n) 15 | min2 = jnp.min(projections2) 16 | max2 = jnp.max(projections2) 17 | 18 | # Check if there is an overlap on this axis 19 | return jnp.logical_not((max1 >= min2) & (max2 >= min1)) 20 | 21 | @jax.jit 22 | def find_edges_norms(P): 23 | # Calculate edges and their perpendicular norms 24 | edges = jnp.roll(P, -1, axis=0) - P 25 | norms = jnp.stack([-edges[:, 1], edges[:, 0]], axis=1) 26 | return edges, norms 27 | 28 | @jax.jit 29 | def collide_jax(P1, P2): 30 | """ 31 | Check if two polygons overlap using the Separating Axis Theorem. 32 | Args: 33 | P1, P2: List of vertices of the polygons. 34 | """ 35 | P1 = jnp.array(P1, dtype=jnp.float32) 36 | P2 = jnp.array(P2, dtype=jnp.float32) 37 | 38 | # Find the edges and normals for both polygons 39 | _, norms1 = find_edges_norms(P1) 40 | _, norms2 = find_edges_norms(P2) 41 | 42 | # Combine the norms 43 | norms = jnp.concatenate([norms1, norms2], axis=0) 44 | 45 | # Check if any separating axis exists 46 | separating_axis = jax.vmap(lambda n: is_separating_axis(n, P1, P2))(norms) 47 | 48 | return jnp.logical_not(jnp.any(separating_axis)) 49 | -------------------------------------------------------------------------------- /PGame/scripts/motion_primitive_planner/parameters.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | desired_velocities = {"EV": 10, "SV0": 10, "SV1": 10, "SV2": 10} 4 | target_lanes = {"EV": 3.5, "SV0": 3.5, "SV1": 3.5, "SV2": 3.5} 5 | lane_width = 3.5 6 | 7 | T = 5 8 | dt = 0.1 9 | MIN_T = 5.0 10 | MAX_T = 5.0 11 | DT = 0.5 12 | 13 | # Typical dimensions for a Tesla Model 3 14 | # vehicle_length = 4.694 # in meters 15 | # vehicle_width = 1.849 # in meters 16 | 17 | # Parameters for the trajectory cost 18 | all_cost_weights = { 19 | "proximity": 5, 20 | 0: { 21 | "jerk": 1, 22 | "efficiency": 1, 23 | "target_lane": 10, 24 | }, 25 | 1: { 26 | "jerk": 1, 27 | "efficiency": 10, 28 | "target_lane": 1, 29 | }, 30 | 2: { 31 | "jerk": 1, 32 | "efficiency": 10, 33 | "target_lane": 1, 34 | }, 35 | 36 | } 37 | 38 | # Parameters for the motion primitive planner 39 | MAX_LAT_DIST = 0.0 40 | MIN_LAT_DIST = -4.0 41 | D_LAT_DIST = 1.0 42 | 43 | MAX_SPEED = 6.0 44 | MIN_SPEED = 0.5 45 | D_V = 0.5 46 | 47 | collision_check_inflation = 1 48 | 49 | if __name__ == "__main__": 50 | import numpy as np 51 | num_action = 0 52 | for v_terminal in np.arange(MIN_SPEED, MAX_SPEED + 0.5*D_V, D_V): 53 | for y_terminal in np.arange(MIN_LAT_DIST, MAX_LAT_DIST + D_LAT_DIST, D_LAT_DIST): 54 | print(f"Action {num_action}: v_terminal = {v_terminal}, y_terminal = {y_terminal}") 55 | num_action += 1 56 | print("\n") 57 | num_action = 0 58 | for v_terminal in np.arange(MIN_SPEED, MAX_SPEED + 0.5*D_V, D_V): 59 | print(f"Action {num_action}: v_terminal = {v_terminal}") 60 | num_action += 1 -------------------------------------------------------------------------------- /common/test.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "from collision_check import collide\n", 10 | "from collision_check_jax import collide_jax\n", 11 | "import jax.numpy as jnp" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "P1 = [(0, 0), (1, 0), (1, 1), (0, 1)]\n", 21 | "P2 = [(0.5, 0.5), (1.5, 0.5), (1.5, 1.5), (0.5, 1.5)]\n" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": null, 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "%%timeit -n 10000 -r 5 \n", 31 | "flag = collide(P1, P2)\n" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "P1_jax = jnp.array(P1)\n", 41 | "P2_jax = jnp.array(P2)\n", 42 | "# P1_jax = P1\n", 43 | "# P2_jax = P2\n", 44 | "print(P1_jax)\n", 45 | "print(P2_jax)" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "%%timeit -n 10000 -r 5 \n", 55 | "flag = collide_jax(P1_jax, P2_jax).block_until_ready()" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": null, 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "collide_jax(P1_jax, P2_jax)" 65 | ] 66 | } 67 | ], 68 | "metadata": { 69 | "kernelspec": { 70 | "display_name": "jax", 71 | "language": "python", 72 | "name": "python3" 73 | }, 74 | "language_info": { 75 | "codemirror_mode": { 76 | "name": "ipython", 77 | "version": 3 78 | }, 79 | "file_extension": ".py", 80 | "mimetype": "text/x-python", 81 | "name": "python", 82 | "nbconvert_exporter": "python", 83 | "pygments_lexer": "ipython3", 84 | "version": "3.9.19" 85 | } 86 | }, 87 | "nbformat": 4, 88 | "nbformat_minor": 2 89 | } 90 | -------------------------------------------------------------------------------- /PGame/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | motion_primitive_planner 4 | 0.0.0 5 | The motion_primitive_planner package 6 | 7 | 8 | 9 | 10 | shaohang 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | vehicle_msgs 55 | rospy 56 | std_msgs 57 | vehicle_msgs 58 | rospy 59 | std_msgs 60 | vehicle_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /PGame/scripts/motion_primitive_planner/math/polynomial.py: -------------------------------------------------------------------------------- 1 | """ 2 | This script is adapted from the work of Atsushi Sakai and his PythonRobotics project (https://github.com/AtsushiSakai/PythonRobotics). 3 | """ 4 | 5 | import numpy as np 6 | 7 | # 4-th order polynomial 8 | class QuarticPolynomial: 9 | def __init__(self, xs, vxs, axs, vxe, axe, time): 10 | if time <= 0: 11 | raise ValueError("The time duration must be greater than zero to avoid singular matrix issues.") 12 | 13 | # Initialize coefficients from initial conditions 14 | self.a0 = xs 15 | self.a1 = vxs 16 | self.a2 = axs / 2.0 17 | 18 | # Set up matrix A and vector b to solve for a3 and a4 19 | A = np.array([ 20 | [3 * time ** 2, 4 * time ** 3], 21 | [6 * time, 12 * time ** 2] 22 | ]) 23 | b = np.array([ 24 | vxe - self.a1 - 2 * self.a2 * time, 25 | axe - 2 * self.a2 26 | ]) 27 | 28 | # Solve the matrix equation for a3 and a4 29 | try: 30 | x = np.linalg.solve(A, b) 31 | except np.linalg.LinAlgError as e: 32 | raise ValueError("An error occurred solving for the coefficients: " + str(e)) 33 | 34 | self.a3, self.a4 = x 35 | 36 | def calc_point(self, t): 37 | """ Calculate the position at time t """ 38 | return self.a0 + self.a1 * t + self.a2 * t ** 2 + self.a3 * t ** 3 + self.a4 * t ** 4 39 | 40 | def calc_first_derivative(self, t): 41 | """ Calculate the velocity at time t """ 42 | return self.a1 + 2 * self.a2 * t + 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 43 | 44 | def calc_second_derivative(self, t): 45 | """ Calculate the acceleration at time t """ 46 | return 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 47 | 48 | def calc_third_derivative(self, t): 49 | """ Calculate the jerk (rate of change of acceleration) at time t """ 50 | return 6 * self.a3 + 24 * self.a4 * t 51 | 52 | 53 | # 5-th order polynomial 54 | class QuinticPolynomial: 55 | 56 | def __init__(self, xs, vxs, axs, xe, vxe, axe, time): 57 | if time <= 0: 58 | raise ValueError("The time duration must be greater than zero.") 59 | 60 | # Initialize coefficients based on initial conditions 61 | self.a0 = xs 62 | self.a1 = vxs 63 | self.a2 = axs / 2.0 64 | 65 | # Setup the matrix A and vector b to solve for a3, a4, a5 66 | A = np.array([ 67 | [time ** 3, time ** 4, time ** 5], 68 | [3 * time ** 2, 4 * time ** 3, 5 * time ** 4], 69 | [6 * time, 12 * time ** 2, 20 * time ** 3] 70 | ]) 71 | b = np.array([ 72 | xe - self.a0 - self.a1 * time - self.a2 * time ** 2, 73 | vxe - self.a1 - 2 * self.a2 * time, 74 | axe - 2 * self.a2 75 | ]) 76 | 77 | # Solve the linear equation system 78 | try: 79 | x = np.linalg.solve(A, b) 80 | except np.linalg.LinAlgError as e: 81 | raise ValueError("An error occurred solving the polynomial coefficients: " + str(e)) 82 | 83 | self.a3, self.a4, self.a5 = x 84 | 85 | def calc_point(self, t): 86 | """ Calculate position at time t """ 87 | return (self.a0 + self.a1 * t + self.a2 * t ** 2 + 88 | self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5) 89 | 90 | def calc_first_derivative(self, t): 91 | """ Calculate velocity at time t """ 92 | return (self.a1 + 2 * self.a2 * t + 3 * self.a3 * t ** 2 + 93 | 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4) 94 | 95 | def calc_second_derivative(self, t): 96 | """ Calculate acceleration at time t """ 97 | return (2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3) 98 | 99 | def calc_third_derivative(self, t): 100 | """ Calculate jerk at time t """ 101 | return (6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2) -------------------------------------------------------------------------------- /PGame/scripts/motion_primitive_planner/utils.py: -------------------------------------------------------------------------------- 1 | import math 2 | import motion_primitive_planner.parameters as parameters 3 | 4 | 5 | dt = 0.1 6 | 7 | class Trajectory: 8 | def __init__(self) -> None: 9 | self.t = [] 10 | self.s = [] 11 | self.s_d = [] 12 | self.s_dd = [] 13 | self.d = [] 14 | self.d_d = [] 15 | self.d_dd = [] 16 | 17 | self.y = [] 18 | self.y_d = [] 19 | self.y_dd = [] 20 | self.x = [] 21 | self.x_d = [] 22 | self.x_dd = [] 23 | 24 | self.heading = [] 25 | self.vel = [] 26 | self.acc_long = [] 27 | self.acc_lat = [] 28 | self.steer = [] 29 | self.curvature = [] 30 | self.jerk = [] 31 | self.angular_acc = [] 32 | 33 | self.self_oriented_cost = 0 34 | self.mutual_cost = 0 35 | self.cost_jerk = 0 36 | self.cost_target_lane = 0 37 | self.cost_efficiency = 0 38 | self.y_terminal = None 39 | self.v_terminal = None 40 | 41 | def get_state_from_flat_output(self, vehicle_wheel_base): 42 | self.vel = [math.hypot(x_d, y_d) for x_d, y_d in zip(self.x_d, self.y_d)] 43 | self.acc_long = [(x_d*x_dd + y_d*y_dd) / math.hypot(x_d, y_d) for x_d, y_d, x_dd, y_dd in zip(self.x_d, self.y_d, self.x_dd, self.y_dd)] 44 | self.acc_lat = [(x_d*y_dd - y_d*x_dd) / math.hypot(x_d, y_d) for x_d, y_d, x_dd, y_dd in zip(self.x_d, self.y_d, self.x_dd, self.y_dd)] 45 | self.heading = [math.atan2(y_d, x_d) for x_d, y_d in zip(self.x_d, self.y_d)] 46 | self.steer = [math.atan((x_d*y_dd - y_d*x_dd) * vehicle_wheel_base / (math.hypot(x_d, y_d))**3) for x_d, y_d, x_dd, y_dd in zip(self.x_d, self.y_d, self.x_dd, self.y_dd)] 47 | self.curvature = [(x_d*y_dd - y_d*x_dd) / (math.hypot(x_d, y_d))**3 for x_d, y_d, x_dd, y_dd in zip(self.x_d, self.y_d, self.x_dd, self.y_dd)] 48 | 49 | import numpy as np 50 | from math import cos, sin 51 | 52 | def find_vehicle_vertices(rear_wheel_center, heading, length, width, vehicle_wheel_base): 53 | center = rear_wheel_center + vehicle_wheel_base / 2 * np.array([cos(heading), sin(heading)]) 54 | inflation = parameters.collision_check_inflation 55 | half_veh_len = length / 2 * inflation 56 | half_veh_wid = width / 2 * inflation 57 | 58 | R = np.array([[cos(heading), -sin(heading)], 59 | [sin(heading), cos(heading)]]) 60 | return [center + R @ np.array([-half_veh_len, -half_veh_wid]), 61 | center + R @ np.array([+half_veh_len, -half_veh_wid]), 62 | center + R @ np.array([+half_veh_len, +half_veh_wid]), 63 | center + R @ np.array([-half_veh_len, +half_veh_wid])] 64 | 65 | def is_separating_axis(n, P1, P2): 66 | min1, max1 = float('+inf'), float('-inf') 67 | min2, max2 = float('+inf'), float('-inf') 68 | for v in P1: 69 | proj = np.dot(v, n) 70 | min1 = min(min1, proj) 71 | max1 = max(max1, proj) 72 | for v in P2: 73 | proj = np.dot(v, n) 74 | min2 = min(min2, proj) 75 | max2 = max(max2, proj) 76 | 77 | if max1 >= min2 and max2 >= min1: 78 | return False 79 | 80 | return True 81 | 82 | def find_edges_norms(P): 83 | edges = [] 84 | norms = [] 85 | num_edge = len(P) 86 | for i in range(num_edge): 87 | edge = P[(i + 1)%num_edge] - P[i] 88 | norm = np.array([-edge[1], edge[0]]) 89 | edges.append(edge) 90 | norms.append(norm) 91 | return edges, norms 92 | 93 | def collide(P1, P2): 94 | """ 95 | Check if two polygons overlap. 96 | We follow https://hackmd.io/@US4ofdv7Sq2GRdxti381_A/ryFmIZrsl?type=view 97 | Args: 98 | p1 (List): List of the vertices of a polygon 99 | p2 (List): List of the vertices of a polygon 100 | """ 101 | 102 | P1 = [np.array(v, 'float64') for v in P1] 103 | P2 = [np.array(v, 'float64') for v in P2] 104 | 105 | _, norms1 = find_edges_norms(P1) 106 | _, norms2 = find_edges_norms(P2) 107 | norms = norms1 + norms2 108 | for n in norms: 109 | if is_separating_axis(n, P1, P2): 110 | return False 111 | 112 | return True 113 | 114 | 115 | # three ways of collision checking 116 | # 1. 名可夫斯基距离 117 | # 2....collision avoidance paper 118 | # 3. 分离轴定理 -------------------------------------------------------------------------------- /PGame/scripts/mpp_node.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy as np 3 | import time 4 | 5 | from motion_primitive_planner.mpp import MPP 6 | from gtm_planner.utils.guideline import create_laneletmap, get_guidelines 7 | import os 8 | import rospkg 9 | from vehicle_msgs.srv import GetAction, GetActionResponse, Reset 10 | from vehicle_msgs.msg import State 11 | from visualization_msgs.msg import Marker 12 | from geometry_msgs.msg import Point 13 | from std_msgs.msg import ColorRGBA 14 | 15 | 16 | class MPPNode: 17 | def __init__(self, lane_dict): 18 | self._lane_dict = lane_dict 19 | self._vehicle_states = {} 20 | self._mpp = MPP(self._lane_dict, self._vehicle_states) 21 | 22 | self._get_action_srv = rospy.Service('/motion_planner/get_ev_action', GetAction, self._get_action_callback) 23 | self._reset_srv = rospy.Service('/potential_game/reset', Reset, self.reset) 24 | self._waypoints_pub = rospy.Publisher("/visualizer/waypoints", Marker, queue_size=10) 25 | 26 | def reset(self, req): 27 | self._desired_velocities = {} 28 | self._averaged_desired_velocities = {} 29 | self._mpp.reset() 30 | return True 31 | 32 | @staticmethod 33 | def average1(vel, av_vel_pre, vel_des): 34 | if av_vel_pre == None: 35 | av_vel = 0.8*vel + 0.2*vel_des 36 | else: 37 | av_vel = 0.6*vel + 0.4*av_vel_pre 38 | # av_vel = 0.7*av_vel + 0.3*vel_des 39 | av_vel = 0.3*av_vel + 0.7*vel_des 40 | return av_vel 41 | 42 | @staticmethod 43 | def average2(vel, av_vel_pre, vel_des): 44 | if av_vel_pre == None: 45 | av_vel = 0.9*vel + 0.1*vel_des 46 | else: 47 | av_vel = 0.6*vel + 0.4*av_vel_pre 48 | av_vel = 0.9*av_vel + 0.1*vel_des 49 | 50 | return av_vel 51 | 52 | def _get_action_callback(self, req): 53 | num_vehicles = len(req.veh_states) 54 | inter_axle_lengths = [req.veh_states[i].wheel_base for i in range(num_vehicles)] 55 | vehicle_lengths = [req.veh_states[i].length for i in range(num_vehicles)] 56 | vehicle_widths = [req.veh_states[i].width for i in range(num_vehicles)] 57 | 58 | for i in range(num_vehicles): 59 | heading = req.veh_states[i].heading 60 | self._vehicle_states[i] = [ 61 | req.veh_states[i].x - inter_axle_lengths[i] * np.cos(heading) / 2.0, 62 | req.veh_states[i].y - inter_axle_lengths[i] * np.sin(heading) / 2.0, 63 | heading, 64 | req.veh_states[i].vel, 65 | req.veh_states[i].acc 66 | ] 67 | self._desired_velocities[i] = req.veh_states[i].vel_des 68 | 69 | if len(self._averaged_desired_velocities) == 0: 70 | for key in self._desired_velocities: 71 | vel = self._vehicle_states[key][3] 72 | if key == 0: 73 | self._averaged_desired_velocities[key] = self.average1(vel, None, self._desired_velocities[key]) 74 | else: 75 | self._averaged_desired_velocities[key] = self.average2(vel, None, self._desired_velocities[key]) 76 | else: 77 | for key in self._desired_velocities: 78 | vel = self._vehicle_states[key][3] 79 | if key == 0: 80 | self._averaged_desired_velocities[key] = self.average1(vel, self._averaged_desired_velocities[key], self._desired_velocities[key]) 81 | else: 82 | self._averaged_desired_velocities[key] = self.average2(vel, self._averaged_desired_velocities[key], self._desired_velocities[key]) 83 | 84 | 85 | rospy.loginfo("desired_velocities: %s", self._desired_velocities) 86 | rospy.loginfo("averaged_desired_velocities: %s", self._averaged_desired_velocities) 87 | start_time = time.time() 88 | acc, steer, best_trajectory = self._mpp.run_step(inter_axle_lengths, 89 | self._averaged_desired_velocities, 90 | vehicle_lengths, vehicle_widths) 91 | computation_time = time.time() - start_time 92 | rospy.loginfo("Total time for planning: %.2f s", computation_time) 93 | print('\n') 94 | if best_trajectory is not None: 95 | self.visualize(best_trajectory, inter_axle_lengths[0]) 96 | next_ego_state = State() 97 | next_ego_state.x = best_trajectory.x[1] 98 | next_ego_state.y = best_trajectory.y[1] 99 | next_ego_state.heading = best_trajectory.heading[1] 100 | next_ego_state.vel = best_trajectory.vel[1] 101 | next_ego_state.acc = best_trajectory.acc_long[1] 102 | next_ego_state.steer = best_trajectory.steer[1] 103 | next_ego_state = State() 104 | # print(f"x_traj:\n{best_trajectory.x}") 105 | # print(f"y_traj:\n{best_trajectory.y}") 106 | # print(f"d_traj:\n{best_trajectory.d}") 107 | # print(f"d_dot_traj:\n{best_trajectory.d_d}") 108 | 109 | # print(f"vel:\n{best_trajectory.vel}") 110 | # # print(f"acc:\n{best_trajectory.acc_long}") 111 | # print(f"steer:\n{best_trajectory.steer}") 112 | # print(f"heading:\n{best_trajectory.heading}") 113 | # print(f"\n\n") 114 | 115 | # next_ego_state.x = best_trajectory.x[2] 116 | # next_ego_state.y = best_trajectory.y[2] 117 | # next_ego_state.heading = best_trajectory.heading[2] 118 | # next_ego_state.vel = best_trajectory.vel[2] 119 | # next_ego_state.acc = best_trajectory.acc_long[2] 120 | # next_ego_state.steer = best_trajectory.steer[2] 121 | return GetActionResponse(acc, steer, computation_time, next_ego_state) 122 | 123 | def visualize(self, traj, inter_axle_length): 124 | waypoints_marker = Marker() 125 | waypoints_marker.id = 0; 126 | waypoints_marker.type = Marker.SPHERE_LIST; 127 | waypoints_marker.header.stamp = rospy.Time.now(); 128 | waypoints_marker.header.frame_id = "map"; 129 | waypoints_marker.pose.orientation.w = 1.00; 130 | waypoints_marker.action = Marker.ADD; 131 | waypoints_marker.ns = "waypoints"; 132 | waypoints_marker.color.r = 0.36; 133 | waypoints_marker.color.g = 0.74; 134 | waypoints_marker.color.b = 0.89; 135 | waypoints_marker.color.a = 0.50; 136 | waypoints_marker.scale.x = 0.5; 137 | waypoints_marker.scale.y = 0.5; 138 | waypoints_marker.scale.z = 0.5; 139 | 140 | for x, y, heading in zip(traj.x, traj.y, traj.heading): 141 | point = Point() 142 | point.x = x + inter_axle_length * np.cos(heading) / 2.0 143 | point.y = y + inter_axle_length * np.sin(heading) / 2.0 144 | point.z = 0.2 145 | waypoints_marker.points.append(point) 146 | 147 | self._waypoints_pub.publish(waypoints_marker) 148 | 149 | 150 | if __name__ == '__main__': 151 | rospy.init_node('mpp_node') 152 | root_dir = os.path.dirname(rospkg.RosPack().get_path('sim_env')) 153 | maps_dir = os.path.join(root_dir, "maps") 154 | scenario_name = 'DR_CHN_Merging_ZS0' 155 | lanelet_map_ending = ".osm" 156 | lanelet_map_file = os.path.join(maps_dir, scenario_name + lanelet_map_ending) 157 | laneletmap = create_laneletmap(lanelet_map_file) 158 | current_guideline, target_guideline = get_guidelines(laneletmap) 159 | lane_dict = {'current_guideline': current_guideline.get_waypoints(), 160 | 'target_guideline': target_guideline.get_waypoints()} 161 | mpp_node = MPPNode(lane_dict) 162 | rospy.spin() 163 | -------------------------------------------------------------------------------- /PGame/scripts/motion_primitive_planner/math/cubic_spline.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | This script is adapted from the work of Atsushi Sakai and his PythonRobotics project (https://github.com/AtsushiSakai/PythonRobotics). 5 | """ 6 | 7 | import math 8 | import numpy as np 9 | import bisect 10 | import matplotlib.pyplot as plt 11 | 12 | class Spline: 13 | u""" 14 | Cubic Spline class 15 | """ 16 | 17 | def __init__(self, x, y): 18 | self.b, self.c, self.d, self.w = [], [], [], [] 19 | 20 | self.x = x 21 | self.y = y 22 | 23 | self.nx = len(x) # dimension of x 24 | h = np.diff(x) 25 | 26 | # calc coefficient c 27 | self.a = [iy for iy in y] 28 | 29 | # calc coefficient c 30 | A = self.__calc_A(h) 31 | B = self.__calc_B(h) 32 | self.c = np.linalg.solve(A, B) 33 | # print(self.c1) 34 | 35 | # calc spline coefficient b and d 36 | for i in range(self.nx - 1): 37 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 38 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 39 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 40 | self.b.append(tb) 41 | 42 | def calc(self, t): 43 | u""" 44 | Calc position 45 | 46 | if t is outside of the input x, return None 47 | 48 | """ 49 | 50 | if t < self.x[0]: 51 | return None 52 | elif t > self.x[-1]: 53 | return None 54 | 55 | i = self.__search_index(t) 56 | dx = t - self.x[i] 57 | result = self.a[i] + self.b[i] * dx + \ 58 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 59 | return result 60 | 61 | def calcd(self, t): 62 | u""" 63 | Calc first derivative 64 | 65 | if t is outside of the input x, return None 66 | """ 67 | 68 | if t < self.x[0]: 69 | return None 70 | elif t > self.x[-1]: 71 | return None 72 | 73 | i = self.__search_index(t) 74 | dx = t - self.x[i] 75 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 76 | return result 77 | 78 | def calcdd(self, t): 79 | u""" 80 | Calc second derivative 81 | """ 82 | 83 | if t < self.x[0]: 84 | return None 85 | elif t > self.x[-1]: 86 | return None 87 | 88 | i = self.__search_index(t) 89 | dx = t - self.x[i] 90 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 91 | return result 92 | 93 | def calcddd(self, t): 94 | u""" 95 | Calc third derivative 96 | """ 97 | 98 | if t < self.x[0]: 99 | return None 100 | elif t > self.x[-1]: 101 | return None 102 | 103 | i = self.__search_index(t) 104 | result = 6.0 * self.d[i] 105 | return result 106 | 107 | def __search_index(self, x): 108 | u""" 109 | search data segment index 110 | """ 111 | res = bisect.bisect(self.x, x) 112 | if res == len(self.x): 113 | res -= 1 114 | return res - 1 115 | 116 | def __calc_A(self, h): 117 | u""" 118 | calc matrix A for spline coefficient c 119 | """ 120 | A = np.zeros((self.nx, self.nx)) 121 | A[0, 0] = 1.0 122 | for i in range(self.nx - 1): 123 | if i != (self.nx - 2): 124 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 125 | A[i + 1, i] = h[i] 126 | A[i, i + 1] = h[i] 127 | 128 | A[0, 1] = 0.0 129 | A[self.nx - 1, self.nx - 2] = 0.0 130 | A[self.nx - 1, self.nx - 1] = 1.0 131 | # print(A) 132 | return A 133 | 134 | def __calc_B(self, h): 135 | u""" 136 | calc matrix B for spline coefficient c 137 | """ 138 | B = np.zeros(self.nx) 139 | for i in range(self.nx - 2): 140 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 141 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 142 | # print(B) 143 | return B 144 | 145 | 146 | class Spline2D: 147 | u""" 148 | 2D Cubic Spline class 149 | 150 | """ 151 | 152 | def __init__(self, x, y): 153 | self.s = self.__calc_s(x, y) 154 | self.sx = Spline(self.s, x) 155 | self.sy = Spline(self.s, y) 156 | 157 | def __calc_s(self, x, y): 158 | dx = np.diff(x) 159 | dy = np.diff(y) 160 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 161 | for (idx, idy) in zip(dx, dy)] 162 | s = [0] 163 | s.extend(np.cumsum(self.ds)) 164 | return s 165 | 166 | def calc_position(self, s): 167 | u""" 168 | calc position 169 | """ 170 | x = self.sx.calc(s) 171 | y = self.sy.calc(s) 172 | 173 | return x, y 174 | 175 | def calc_curvature(self, s): 176 | u""" 177 | calc curvature 178 | """ 179 | dx = self.sx.calcd(s) 180 | ddx = self.sx.calcdd(s) 181 | dy = self.sy.calcd(s) 182 | ddy = self.sy.calcdd(s) 183 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) ** (3 / 2) 184 | 185 | return k 186 | 187 | def calc_curvature_d(self, s): 188 | u""" 189 | calc curvature 190 | """ 191 | dx = self.sx.calcd(s) 192 | ddx = self.sx.calcdd(s) 193 | dddx = self.sx.calcddd(s) 194 | dy = self.sy.calcd(s) 195 | ddy = self.sy.calcdd(s) 196 | dddy = self.sy.calcddd(s) 197 | 198 | k_d = ( 199 | (dddy * dx - dddx * dy) * (dx ** 2 + dy ** 2) - 200 | 3 * (dx * ddy - ddx * dy) * (dx * ddx + dy * ddy) 201 | ) / ((dx ** 2 + dy ** 2) ** (5 / 2)) 202 | 203 | return k_d 204 | 205 | def calc_yaw(self, s): 206 | u""" 207 | calc yaw 208 | """ 209 | dx = self.sx.calcd(s) 210 | dy = self.sy.calcd(s) 211 | yaw = math.atan2(dy, dx) 212 | return yaw 213 | 214 | 215 | class Spline3D: 216 | """ 217 | 3D Cubic Spline class 218 | """ 219 | 220 | def __init__(self, x, y, z): 221 | self.s = self.__calc_s(x, y, z) 222 | self.sx = Spline(self.s, x) 223 | self.sy = Spline(self.s, y) 224 | self.sz = Spline(self.s, z) 225 | 226 | def __calc_s(self, x, y, z): 227 | dx = np.diff(x) 228 | dy = np.diff(y) 229 | dz = np.diff(z) 230 | self.ds = [math.sqrt(idx ** 2 + idy ** 2 + idz ** 2) for (idx, idy, idz) in zip(dx, dy, dz)] 231 | s = [0] 232 | s.extend(np.cumsum(self.ds)) 233 | return s 234 | 235 | def calc_position(self, s): 236 | u""" 237 | calc position 238 | """ 239 | x = self.sx.calc(s) 240 | y = self.sy.calc(s) 241 | z = self.sz.calc(s) 242 | return x, y, z 243 | 244 | def calc_curvature(self, s): 245 | u""" 246 | calc curvature 247 | """ 248 | dx = self.sx.calcd(s) 249 | ddx = self.sx.calcdd(s) 250 | dy = self.sy.calcd(s) 251 | ddy = self.sy.calcdd(s) 252 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) ** (3 / 2) 253 | return k 254 | 255 | def calc_yaw(self, s): 256 | u""" 257 | calc yaw 258 | """ 259 | dx = self.sx.calcd(s) 260 | dy = self.sy.calcd(s) 261 | yaw = math.atan2(dy, dx) 262 | return yaw 263 | 264 | def calc_pitch(self, s): 265 | """ 266 | calc pitch - this function needs to be double checked 267 | """ 268 | dx = self.sx.calcd(s) 269 | dz = self.sz.calcd(s) 270 | pitch = math.atan2(dz, dx) 271 | return pitch 272 | 273 | 274 | def calc_spline_course(x, y, ds=0.1): 275 | sp = Spline2D(x, y) 276 | s = np.arange(0, sp.s[-1], ds) 277 | 278 | rx, ry, ryaw, rk = [], [], [], [] 279 | for i_s in s: 280 | ix, iy = sp.calc_position(i_s) 281 | rx.append(ix) 282 | ry.append(iy) 283 | ryaw.append(sp.calc_yaw(i_s)) 284 | rk.append(sp.calc_curvature(i_s)) 285 | 286 | return rx, ry, ryaw, rk, s -------------------------------------------------------------------------------- /PGame/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(motion_primitive_planner) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | vehicle_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs# vehicle_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES motion_primitive_planner 108 | # CATKIN_DEPENDS rospy std_msgs vehicle_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/motion_primitive_planner.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/motion_primitive_planner_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | catkin_install_python(PROGRAMS 163 | scripts/motion_primitive_planner/math/cubic_spline.py 164 | scripts/motion_primitive_planner/math/polynomial.py 165 | scripts/motion_primitive_planner/__init__.py 166 | scripts/motion_primitive_planner/mpp.py 167 | scripts/motion_primitive_planner/utils.py 168 | scripts/motion_primitive_planner/parameters.py 169 | scripts/mpp_node.py 170 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | ) 172 | 173 | ## Mark executables for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 175 | # install(TARGETS ${PROJECT_NAME}_node 176 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark libraries for installation 180 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 181 | # install(TARGETS ${PROJECT_NAME} 182 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 183 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 185 | # ) 186 | 187 | ## Mark cpp header files for installation 188 | # install(DIRECTORY include/${PROJECT_NAME}/ 189 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 190 | # FILES_MATCHING PATTERN "*.h" 191 | # PATTERN ".svn" EXCLUDE 192 | # ) 193 | 194 | ## Mark other files for installation (e.g. launch and bag files, etc.) 195 | # install(FILES 196 | # # myfile1 197 | # # myfile2 198 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 199 | # ) 200 | 201 | ############# 202 | ## Testing ## 203 | ############# 204 | 205 | ## Add gtest based cpp test target and link libraries 206 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_motion_primitive_planner.cpp) 207 | # if(TARGET ${PROJECT_NAME}-test) 208 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 209 | # endif() 210 | 211 | ## Add folders to be run by python nosetests 212 | # catkin_add_nosetests(test) 213 | -------------------------------------------------------------------------------- /PGame/scripts/motion_primitive_planner/mpp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | from itertools import combinations 4 | import time 5 | import rospy 6 | from scipy.spatial import KDTree 7 | from motion_primitive_planner.math.cubic_spline import Spline2D 8 | from motion_primitive_planner.math.polynomial import QuarticPolynomial, QuinticPolynomial 9 | from motion_primitive_planner.utils import Trajectory, collide, find_vehicle_vertices 10 | import motion_primitive_planner.parameters as parameters 11 | from copy import deepcopy 12 | 13 | import pickle 14 | 15 | def pi_2_pi(angle): 16 | return (angle + math.pi) % (2 * math.pi) - math.pi 17 | 18 | class MPP: 19 | def __init__(self, lane_dict, vehicle_states): 20 | self._lane_dict = lane_dict 21 | self._set_reference_line() 22 | self._vehicle_states_cartesian = vehicle_states 23 | self._planner_name = None 24 | # self._all_ego_decisions = None 25 | # self._target_vehicle = None 26 | # self._vehicle_wheel_bases = None 27 | # self._vehicle_lengths = None 28 | # self._vehicle_widths = None 29 | # self._desired_velocities = None 30 | # self._last_vehicle_states_cartesian = None 31 | # self._vehicle_states_frenet = {} 32 | 33 | def reset(self): 34 | self._all_ego_decisions = None 35 | self._target_vehicle = None 36 | self._vehicle_wheel_bases = None 37 | self._vehicle_lengths = None 38 | self._vehicle_widths = None 39 | self._desired_velocities = None 40 | self._last_vehicle_states_cartesian = None 41 | self._vehicle_states_frenet = {} 42 | 43 | def run_step(self, vehicle_wheel_bases, desired_velocities, vehicle_lengths, vehicle_widths): 44 | self._vehicle_lengths = vehicle_lengths 45 | self._vehicle_widths = vehicle_widths 46 | self._desired_velocities = desired_velocities 47 | self._vehicle_wheel_bases = vehicle_wheel_bases 48 | start_time = time.time() 49 | for name in self._vehicle_states_cartesian: 50 | if self._last_vehicle_states_cartesian is not None: 51 | self._vehicle_states_frenet[name] = self._get_frenet_state(self._vehicle_states_cartesian[name], self._last_vehicle_states_cartesian[name]) 52 | else: 53 | self._vehicle_states_frenet[name] = self._get_frenet_state(self._vehicle_states_cartesian[name], None) 54 | rospy.loginfo("vehicle_states_cartesian %s", self._vehicle_states_cartesian) 55 | rospy.loginfo("vehicle_states_frenet %s", self._vehicle_states_frenet) 56 | all_trajectories = self._get_trajectory_by_motion_primitive() 57 | rospy.loginfo("Time for getting trajectory: %.2f s", time.time() - start_time) 58 | start_time = time.time() 59 | best_trajectory = self._trajectory_selection(all_trajectories) 60 | if best_trajectory is None: 61 | rospy.logwarn("No valid trajectory found, brake forcefully!") 62 | acc = -2 63 | steer = 0 64 | return acc, steer, None 65 | rospy.loginfo("Time for trajectory selection: %.2f s", time.time() - start_time) 66 | rospy.loginfo("(unweighted) cost_jerk: %.4f, cost_target_lane: %.4f, cost_efficiency: %.4f", best_trajectory.cost_jerk, best_trajectory.cost_target_lane, best_trajectory.cost_efficiency) 67 | acc = best_trajectory.acc_long[1] 68 | steer = best_trajectory.steer[1] 69 | # acc = best_trajectory.acc_long[5] 70 | # steer = best_trajectory.steer[5] 71 | # acc = best_trajectory.acc_long[0] 72 | # steer = best_trajectory.steer[0] 73 | self._last_vehicle_states_cartesian = deepcopy(self._vehicle_states_cartesian) 74 | 75 | return acc, steer, best_trajectory 76 | 77 | def _set_reference_line(self): 78 | self.csp = Spline2D(self._lane_dict["target_guideline"][:, 0], 79 | self._lane_dict["target_guideline"][:, 1]) 80 | self.s = np.arange(0, self.csp.s[-1], 0.1) 81 | self.rx, self.ry, self.ryaw, self.rk = [], [], [], [] 82 | for i_s in self.s: 83 | ix, iy = self.csp.calc_position(i_s) 84 | self.rx.append(ix) 85 | self.ry.append(iy) 86 | self.ryaw.append(self.csp.calc_yaw(i_s)) 87 | self.rk.append(self.csp.calc_curvature(i_s)) 88 | point_xy = np.array([item for item in zip(self.rx, self.ry)]) 89 | self.ref_kdtree = KDTree(point_xy) 90 | 91 | def _get_frenet_state(self, state_cartesian, state_cartesian_prev): 92 | # _, idx_r = self.ref_kdtree.query([state_cartesian[0], state_cartesian[1]]) 93 | # if idx_r >= len(self.s): # TODO 94 | # idx_r = len(self.s) - 1 95 | # s_r = self.s[idx_r] 96 | # x_r, y_r = self.csp.calc_position(s_r) 97 | # k_r = self.csp.calc_curvature(s_r) 98 | # yaw_r = self.csp.calc_yaw(s_r) 99 | # dyaw_r = self.csp.calc_curvature_d(s_r) 100 | # delta_theta = state_cartesian[2] - yaw_r 101 | # # k_x: curvature of vehicle's route 102 | # if state_cartesian is not None and state_cartesian_prev is not None: 103 | # dx = state_cartesian[0] - state_cartesian_prev[0] 104 | # dy = state_cartesian[1] - state_cartesian_prev[1] 105 | # dyaw = state_cartesian[2] - state_cartesian_prev[2] 106 | # ds = math.hypot(dx, dy) 107 | # if 0 < ds: 108 | # k_x = dyaw / math.hypot(dx, dy) 109 | # else: 110 | # k_x = None 111 | # else: 112 | # k_x = None 113 | # s = s_r 114 | # x_delta = state_cartesian[0] - x_r 115 | # y_delta = state_cartesian[1] - y_r 116 | # d = np.sign(y_delta * math.cos(yaw_r) - x_delta * math.sin(yaw_r)) * math.hypot(x_delta, y_delta) 117 | # d_d = state_cartesian[3] * math.sin(delta_theta) 118 | # coeff_1 = 1 - k_r * d 119 | # acc = 0 120 | # if state_cartesian_prev is not None: 121 | # acc = (state_cartesian[3] - state_cartesian_prev[3]) / parameters.dt 122 | # d_dd = acc * math.sin(delta_theta) 123 | # s_d = state_cartesian[3] * math.cos(delta_theta) / coeff_1 124 | 125 | # if k_x is None: 126 | # s_dd = 0 127 | # else: 128 | # # s_ds = coeff_1 * math.tan(delta_theta) 129 | # # coeff_2 = coeff_1 / math.cos(delta_theta) * k_x - k_r 130 | # # coeff_3 = dyaw_r * d + yaw_r * s_ds 131 | # # s_dd = state_cartesian[4] * math.cos(delta_theta) - \ 132 | # # (s_d ** 2) * (s_ds * coeff_2 - coeff_3) / coeff_1 133 | 134 | # kappa_r = self.csp.calc_curvature(s_r) 135 | # s_ds = coeff_1 * math.tan(delta_theta) 136 | # coeff_2 = coeff_1 / math.cos(delta_theta) * k_x - k_r 137 | # coeff_3 = dyaw_r * d + kappa_r * s_ds 138 | # s_dd = (acc * math.cos(delta_theta) - 139 | # (s_d ** 2) * (s_ds * coeff_2 - coeff_3)) / coeff_1 140 | 141 | ## Referring to 142 | # https://blog.csdn.net/u013468614/article/details/108748016 143 | _, idx_r = self.ref_kdtree.query([state_cartesian[0], state_cartesian[1]]) 144 | if idx_r >= len(self.s): # TODO 145 | idx_r = len(self.s) - 1 146 | x_x = state_cartesian[0] 147 | y_x = state_cartesian[1] 148 | theta_x = state_cartesian[2] 149 | v_x = state_cartesian[3] 150 | # a_x = state_cartesian[4] 151 | a_x = 0 152 | if state_cartesian_prev is not None: 153 | a_x = (v_x - state_cartesian_prev[3]) / parameters.dt 154 | k_x = 0 155 | if state_cartesian_prev is not None: 156 | distance = math.hypot(x_x - state_cartesian_prev[0], y_x - state_cartesian_prev[1]) 157 | dtheta = theta_x - state_cartesian_prev[2] 158 | if distance != 0: 159 | k_x = dtheta / distance 160 | 161 | s_r = self.s[idx_r] 162 | x_r, y_r = self.csp.calc_position(s_r) 163 | theta_r = self.csp.calc_yaw(s_r) 164 | k_r = self.csp.calc_curvature(s_r) 165 | k_r_d = self.csp.calc_curvature_d(s_r) 166 | 167 | delta_x = x_x - x_r 168 | delta_y = y_x - y_r 169 | delta_theta = theta_x - theta_r 170 | 171 | C_drdx = delta_y * math.cos(theta_r) - delta_x * math.sin(theta_r) 172 | 173 | d = np.sign(C_drdx) * math.hypot(delta_x, delta_y) 174 | C_4 = 1 - k_r * d 175 | d_p = C_4 * math.tan(delta_theta) 176 | C_6 = k_r_d * d + k_r * d_p 177 | k_x = 0 178 | if state_cartesian_prev is not None: 179 | distance = math.hypot(x_x - state_cartesian_prev[0], y_x - state_cartesian_prev[1]) 180 | dtheta = pi_2_pi(theta_x - state_cartesian_prev[2]) 181 | if distance != 0: 182 | # print(f"distance: {distance}") 183 | # print(f"dtheta: {dtheta}") 184 | # print(f"theta_x: {theta_x}") 185 | # print(f"state_cartesian_prev[2]: {state_cartesian_prev[2]}") 186 | k_x = dtheta / distance 187 | d_pp = -C_6 * math.tan(delta_theta) + C_4 / math.cos(delta_theta)**2 * (k_x * C_4 / math.cos(delta_theta) - k_r) 188 | 189 | s = s_r 190 | s_d = v_x * math.cos(delta_theta) / C_4 191 | C_10 = k_x * C_4 / math.cos(delta_theta) - k_r 192 | s_dd = (a_x * math.cos(delta_theta) - (s_d**2) * (d_p * C_10 - C_6)) / C_4 193 | 194 | d_d = s_d * d_p 195 | d_dd = s_dd * d_p + s_d**2 * d_pp 196 | 197 | # print(f"a_x: {a_x}") 198 | # print(f"delta_theta: {delta_theta}") 199 | # print(f"d_p: {d_p}") 200 | # print(f"C_4: {C_4}") 201 | # print(f"C_6: {C_6}") 202 | # print(f"C_10: {C_10}") 203 | # print(f"k_x: {k_x}") 204 | # print(f"k_r: {k_r}") 205 | # print(f"s_d**2: {s_d**2}") 206 | # print(f"s: {s}, s_d: {s_d}, s_dd: {s_dd}, d: {d}, d_d: {d_d}, d_dd: {d_dd}\n\n") 207 | 208 | return s, s_d, s_dd, d, d_d, d_dd 209 | 210 | # def _get_cartesian_trajectories(self, trajectory_list, vehicle_wheel_base): 211 | # valid_trajectory_list = [] 212 | # for trajectory in trajectory_list: 213 | # for s, s_d, s_dd, d, d_d, d_dd in zip(trajectory.s, trajectory.s_d, trajectory.s_dd, trajectory.d, trajectory.d_d, trajectory.d_dd): 214 | # ix, iy = self.csp.calc_position(s) 215 | # if ix is None or iy is None: 216 | # rospy.logwarn("ix or iy is None with s: %s", s) 217 | # break 218 | # i_yaw = self.csp.calc_yaw(s) 219 | # trajectory.x.append(ix - d * math.sin(i_yaw)) 220 | # trajectory.y.append(iy + d * math.cos(i_yaw)) 221 | # trajectory.x_d.append(s_d * math.cos(i_yaw) - d_d * math.sin(i_yaw)) 222 | # trajectory.y_d.append(s_d * math.sin(i_yaw) + d_d * math.cos(i_yaw)) 223 | # trajectory.x_dd.append(s_dd * math.cos(i_yaw) - d_dd * math.sin(i_yaw)) 224 | # trajectory.y_dd.append(s_dd * math.sin(i_yaw) + d_dd * math.cos(i_yaw)) 225 | # trajectory.get_state_from_flat_output(vehicle_wheel_base) 226 | # valid_trajectory_list.append(trajectory) 227 | # return valid_trajectory_list 228 | 229 | def _get_cartesian_trajectories(self, trajectory_list, vehicle_wheel_base): 230 | valid_trajectory_list = [] 231 | for trajectory in trajectory_list: 232 | is_valid = True 233 | for s, s_d, s_dd, d, d_d, d_dd in zip(trajectory.s, trajectory.s_d, trajectory.s_dd, trajectory.d, trajectory.d_d, trajectory.d_dd): 234 | ix, iy = self.csp.calc_position(s) 235 | if ix is None or iy is None: 236 | rospy.logwarn("ix or iy is None with s: %s", s) 237 | break 238 | if s_d == 0: raise ValueError("s_d is 0") 239 | d_p = d_d / s_d 240 | d_pp = (d_dd - s_dd * d_p) / s_d**2 241 | 242 | i_yaw = self.csp.calc_yaw(s) 243 | trajectory.x.append(ix - d * math.sin(i_yaw)) 244 | trajectory.y.append(iy + d * math.cos(i_yaw)) 245 | 246 | i_kappa = self.csp.calc_curvature(s) 247 | vel = math.sqrt((1 - d * i_kappa)**2 * s_d**2 + d_d**2) 248 | trajectory.vel.append(vel) 249 | 250 | delta_theta = math.asin(d_d / vel) 251 | heading = i_yaw + delta_theta 252 | trajectory.heading.append(heading) 253 | 254 | i_kappa_d = self.csp.calc_curvature_d(s) 255 | M1 = 1 - d * i_kappa 256 | M2 = math.cos(delta_theta) 257 | M3 = M1 / M2 258 | M4 = M1 / M2**2 259 | curvature = ((d_pp + (i_kappa_d * d + i_kappa * d_p) * math.tan(delta_theta)) / M4 + i_kappa) / M3 260 | trajectory.curvature.append(curvature) 261 | steer = math.atan(vehicle_wheel_base * curvature) 262 | if steer > math.pi / 6: 263 | is_valid = False 264 | break 265 | trajectory.steer.append(steer) 266 | 267 | M5 = (1 - d * i_kappa) * math.tan(delta_theta) 268 | M6 = curvature * M3 - i_kappa 269 | acc = s_dd * M3 + s_d**2 / M2 * (M5 * M6 - (i_kappa_d * d + i_kappa * d_p)) 270 | trajectory.acc_long.append(acc) 271 | if is_valid: 272 | valid_trajectory_list.append(trajectory) 273 | return valid_trajectory_list 274 | 275 | def _get_trajectory_by_motion_primitive(self): 276 | all_trajectories = {} 277 | for name, state_frenet in self._vehicle_states_frenet.items(): 278 | if name == 0: 279 | all_trajectories[name] = self._get_cartesian_trajectories(self._get_motion_primitives(state_frenet, True), 280 | vehicle_wheel_base=self._vehicle_wheel_bases[name]) 281 | else: 282 | all_trajectories[name] = self._get_cartesian_trajectories(self._get_motion_primitives(state_frenet, False), 283 | vehicle_wheel_base=self._vehicle_wheel_bases[name]) 284 | return all_trajectories 285 | 286 | def _get_motion_primitives(self, state_frenet, sampling_lateral: bool): 287 | s, s_d, s_dd, d, d_d, d_dd = state_frenet 288 | trajectory_list = [] 289 | def get_trajectory_properties(long_qp, trajectory): 290 | trajectory.s = [long_qp.calc_point(t) for t in trajectory.t] 291 | trajectory.s_d = [long_qp.calc_first_derivative(t) for t in trajectory.t] 292 | trajectory.s_dd = [long_qp.calc_second_derivative(t) for t in trajectory.t] 293 | trajectory.s_ddd = [long_qp.calc_third_derivative(t) for t in trajectory.t] 294 | for v_terminal in np.arange(parameters.MIN_SPEED, parameters.MAX_SPEED + 0.5*parameters.D_V, parameters.D_V): 295 | # for Ti in np.arange(parameters.MIN_T, parameters.MAX_T + 0.5 * parameters.DT, parameters.DT): 296 | long_qp = QuarticPolynomial(s, s_d, s_dd, v_terminal, 0, parameters.T) 297 | # long_qp = QuarticPolynomial(s, s_d, s_dd, v_terminal, 0, Ti) 298 | time_points = np.arange(0, parameters.T, parameters.dt).tolist() 299 | # time_points = np.arange(0, Ti, parameters.dt).tolist() 300 | if sampling_lateral: 301 | for y_terminal in np.arange(parameters.MIN_LAT_DIST, parameters.MAX_LAT_DIST + 0.5*parameters.D_LAT_DIST, parameters.D_LAT_DIST): 302 | lat_qp = QuinticPolynomial(d, d_d, d_dd, y_terminal, 0, 0, parameters.T) 303 | # lat_qp = QuinticPolynomial(d, d_d, d_dd, y_terminal, 0, 0, Ti) 304 | trajectory = Trajectory() 305 | trajectory.y_terminal = y_terminal 306 | trajectory.t = time_points 307 | trajectory.d = [lat_qp.calc_point(t) for t in trajectory.t] 308 | trajectory.d_d = [lat_qp.calc_first_derivative(t) for t in trajectory.t] 309 | trajectory.d_dd = [lat_qp.calc_second_derivative(t) for t in trajectory.t] 310 | trajectory.d_ddd = [lat_qp.calc_third_derivative(t) for t in trajectory.t] 311 | trajectory.v_terminal = v_terminal 312 | get_trajectory_properties(long_qp, trajectory) 313 | trajectory_list.append(trajectory) 314 | trajectory.cost_target_lane = sum(abs(np.array(trajectory.d))) / len(time_points) # assume the reference line is the target lane 315 | else: 316 | trajectory = Trajectory() 317 | trajectory.t = time_points 318 | trajectory.v_terminal = v_terminal 319 | trajectory.d = [d for _ in trajectory.t] 320 | trajectory.d_d = [0 for _ in trajectory.t] 321 | trajectory.d_dd = [0 for _ in trajectory.t] 322 | trajectory.d_ddd = [0 for _ in trajectory.t] 323 | get_trajectory_properties(long_qp, trajectory) 324 | trajectory_list.append(trajectory) 325 | return trajectory_list 326 | 327 | def _trajectory_selection(self, all_trajectories): 328 | if len(all_trajectories[0]) == 0: 329 | return None 330 | time_start = time.time() 331 | cost_matrix, potential_function_matrix = self._get_trajectory_cost(all_trajectories) 332 | # print("Time for calculating cost matrix: %.2f s" % (time.time() - time_start)) 333 | # non_ego_decision_id, ego_decision_id = self._get_ne_potential_game(cost_matrix, potential_function_matrix) 334 | all_decision_ids = self._get_ne_matrix_game(cost_matrix) 335 | time_start = time.time() 336 | # print("Time for getting Nash Equilibrium: %.2f s" % (time.time() - time_start)) 337 | best_ego_decision_id = all_decision_ids[0] 338 | # rospy.loginfo("vehicle 0 decision lateral: %s, longitudinal: %s", all_trajectories[0][best_ego_decision_id].y_terminal, all_trajectories[0][best_ego_decision_id].v_terminal) 339 | for i in [0, 1, 2]: 340 | rospy.loginfo("vehicle %s decision lateral: %s, longitudinal: %s", i, all_trajectories[i][all_decision_ids[i]].y_terminal, all_trajectories[i][all_decision_ids[i]].v_terminal) 341 | rospy.loginfo('potential function value: %s', potential_function_matrix[all_decision_ids]) 342 | # best_ego_decision_id = self._get_naive_solution(all_trajectories[0]) 343 | if potential_function_matrix[all_decision_ids] > 1000: # TODO: tmp hard code 344 | return None 345 | return all_trajectories[0][best_ego_decision_id] 346 | 347 | def _get_trajectory_cost(self, all_trajectories): 348 | assert all_trajectories.keys() == {0, 1, 2} 349 | num_players = 3 350 | cost_matrix = np.zeros((len(all_trajectories[0]), len(all_trajectories[1]), len(all_trajectories[2]), num_players)) 351 | potential_function_matrix = np.zeros((len(all_trajectories[0]), len(all_trajectories[1]), len(all_trajectories[2]), 1)) 352 | 353 | for name, trajectories in all_trajectories.items(): 354 | for trajectory in trajectories: 355 | trajectory.self_oriented_cost = self._get_self_oriented_cost(trajectory, name) 356 | 357 | mutual_cost_matrix_01 = np.zeros((len(all_trajectories[0]), len(all_trajectories[1]))) 358 | mutual_cost_matrix_02 = np.zeros((len(all_trajectories[0]), len(all_trajectories[2]))) 359 | mutual_cost_matrix_12 = np.zeros((len(all_trajectories[1]), len(all_trajectories[2]))) 360 | for i in range(len(all_trajectories[0])): 361 | for j in range(len(all_trajectories[1])): 362 | pair_action_map = {0: i, 1: j} 363 | mutual_cost_matrix_01[i][j] = self._get_mutual_cost(all_trajectories, pair_action_map) 364 | for i in range(len(all_trajectories[0])): 365 | for k in range(len(all_trajectories[2])): 366 | pair_action_map = {0: i, 2: k} 367 | mutual_cost_matrix_02[i][k] = self._get_mutual_cost(all_trajectories, pair_action_map) 368 | for j in range(len(all_trajectories[1])): 369 | for k in range(len(all_trajectories[2])): 370 | pair_action_map = {1: j, 2: k} 371 | mutual_cost_matrix_12[j][k] = self._get_mutual_cost(all_trajectories, pair_action_map) 372 | 373 | alpha = 1 374 | beta = 1 375 | for i in range(len(all_trajectories[0])): 376 | for j in range(len(all_trajectories[1])): 377 | for k in range(len(all_trajectories[2])): 378 | cost_matrix[i][j][k][0] = alpha * all_trajectories[0][i].self_oriented_cost + beta * (mutual_cost_matrix_01[i][j] + mutual_cost_matrix_02[i][k]) 379 | cost_matrix[i][j][k][1] = alpha * all_trajectories[1][j].self_oriented_cost + beta * (mutual_cost_matrix_01[i][j] + mutual_cost_matrix_12[j][k]) 380 | cost_matrix[i][j][k][2] = alpha * all_trajectories[2][k].self_oriented_cost + beta * (mutual_cost_matrix_02[i][k] + mutual_cost_matrix_12[j][k]) 381 | potential_function_matrix[i][j][k] = alpha * (all_trajectories[0][i].self_oriented_cost + all_trajectories[1][j].self_oriented_cost + all_trajectories[2][k].self_oriented_cost) + \ 382 | beta * (mutual_cost_matrix_01[i][j] + mutual_cost_matrix_02[i][k] + mutual_cost_matrix_12[j][k]) 383 | # pickle.dump(all_trajectories, open("/home/kasm-user/workspace/tcst_ws/all_trajectories.pkl", "wb")) 384 | # pickle.dump(self._vehicle_lengths, open("/home/kasm-user/workspace/tcst_ws/vehicle_lengths.pkl", "wb")) 385 | # pickle.dump(self._vehicle_widths, open("/home/kasm-user/workspace/tcst_ws/vehicle_widths.pkl", "wb")) 386 | # pickle.dump(self._vehicle_wheel_bases, open("/home/kasm-user/workspace/tcst_ws/vehicle_wheel_bases.pkl", "wb")) 387 | # np.save("/home/kasm-user/workspace/tcst_ws/cost_matrix.npy", cost_matrix) 388 | # np.save("/home/kasm-user/workspace/tcst_ws/potential_function_matrix.npy", potential_function_matrix) 389 | return cost_matrix, potential_function_matrix 390 | 391 | def _get_self_oriented_cost(self, trajectory, name): 392 | cost_weights = parameters.all_cost_weights[name] 393 | num_sim_step = len(trajectory.x) 394 | cost_jerk = sum([((acc - acc_prev)/parameters.dt)**2 for acc, acc_prev in zip(trajectory.acc_long, trajectory.acc_long[1:])]) / num_sim_step 395 | target_speed = self._desired_velocities[name] 396 | cost_efficiency = sum([(vel - target_speed)**2 for vel in trajectory.vel]) / num_sim_step 397 | trajectory.jerk = [abs((acc - acc_prev)/parameters.dt) for acc, acc_prev in zip(trajectory.acc_long, trajectory.acc_long[1:])] 398 | trajectory.angular_acc = [abs((heading - heading_prev)/parameters.dt) for heading, heading_prev in zip(trajectory.heading, trajectory.heading[1:])] 399 | trajectory.cost_jerk = cost_jerk 400 | trajectory.cost_efficiency = cost_efficiency 401 | return trajectory.cost_jerk * cost_weights["jerk"] + trajectory.cost_target_lane * cost_weights["target_lane"] \ 402 | + trajectory.cost_efficiency * cost_weights["efficiency"] 403 | 404 | def _get_mutual_cost(self, all_trajectories, pair_action_map): 405 | name_0, name_1 = pair_action_map.keys() 406 | num_sim_step = min(len(all_trajectories[name_0][pair_action_map[name_0]].x), len(all_trajectories[name_1][pair_action_map[name_1]].x)) 407 | mutual_cost = 0 408 | prox_cost = 0 409 | dist_prox = 1 410 | for i in range(num_sim_step): 411 | pos_0 = np.array([all_trajectories[name_0][pair_action_map[name_0]].x[i], all_trajectories[name_0][pair_action_map[name_0]].y[i]]) 412 | pos_1 = np.array([all_trajectories[name_1][pair_action_map[name_1]].x[i], all_trajectories[name_1][pair_action_map[name_1]].y[i]]) 413 | prox_cost += 1 / (np.linalg.norm(pos_0 - pos_1) + 1e-4) 414 | # dist = np.linalg.norm(pos_0 - pos_1) 415 | 416 | prox_cost /= num_sim_step 417 | 418 | collision_cost = 0 419 | for i in range(num_sim_step): 420 | vertices_0 = find_vehicle_vertices((all_trajectories[name_0][pair_action_map[name_0]].x[i], all_trajectories[name_0][pair_action_map[name_0]].y[i]), 421 | all_trajectories[name_0][pair_action_map[name_0]].heading[i], 422 | self._vehicle_lengths[name_0], 423 | self._vehicle_widths[name_0], 424 | self._vehicle_wheel_bases[name_0]) 425 | vertices_1 = find_vehicle_vertices((all_trajectories[name_1][pair_action_map[name_1]].x[i], all_trajectories[name_1][pair_action_map[name_1]].y[i]), 426 | all_trajectories[name_1][pair_action_map[name_1]].heading[i], 427 | self._vehicle_lengths[name_1], 428 | self._vehicle_widths[name_1], 429 | self._vehicle_wheel_bases[name_1]) 430 | if collide(vertices_0, vertices_1): 431 | collision_cost = 1000 432 | break 433 | 434 | mutual_cost = prox_cost * parameters.all_cost_weights["proximity"] + collision_cost 435 | return mutual_cost 436 | 437 | def _get_naive_solution(self, ego_trajectories: list) -> int: 438 | best_idx = None 439 | best_cost = float('inf') 440 | for idx, traj in enumerate(ego_trajectories): 441 | if traj.self_oriented_cost < best_cost: 442 | best_idx = idx 443 | best_cost = traj.self_oriented_cost 444 | return best_idx 445 | 446 | def _get_ne_matrix_game(self, cost_matrix: np.ndarray) -> tuple: 447 | NEs = [] 448 | num_rows, num_cols, num_depth = cost_matrix.shape[:3] 449 | for i in range(num_rows): 450 | for j in range(num_cols): 451 | for k in range(num_depth): 452 | player1_cost = cost_matrix[i, j, k][0] 453 | player2_cost = cost_matrix[i, j, k][1] 454 | player3_cost = cost_matrix[i, j, k][2] 455 | is_best_response_p1 = all(player1_cost <= cost_matrix[x, j, k][0] for x in range(num_rows)) 456 | is_best_response_p2 = all(player2_cost <= cost_matrix[i, y, k][1] for y in range(num_cols)) 457 | is_best_response_p3 = all(player3_cost <= cost_matrix[i, j, z][2] for z in range(num_depth)) 458 | if is_best_response_p1 and is_best_response_p2 and is_best_response_p3: 459 | social_cost = player1_cost + player2_cost + player3_cost 460 | NEs.append(((i, j, k), social_cost)) 461 | if not NEs: 462 | assert False, "No Nash Equilibrium found" 463 | return None 464 | # Return the NE with the lowest social cost 465 | return min(NEs, key=lambda x: x[1])[0] 466 | 467 | def _get_ne_potential_game(self, cost_matrix, potential_function_matrix): 468 | NEs = [] 469 | num_rows = len(potential_function_matrix) 470 | num_cols = len(potential_function_matrix[0]) if num_rows > 0 else 0 471 | for i in range(num_rows): 472 | for j in range(num_cols): 473 | # Check if current (i, j) is a local minimum in the potential function 474 | local_min = True 475 | # Check all neighboring entries in the potential matrix 476 | for di in [-1, 0, 1]: 477 | for dj in [-1, 0, 1]: 478 | ni, nj = i + di, j + dj 479 | # Ensure indices are within the bounds of the matrix 480 | if 0 <= ni < num_rows and 0 <= nj < num_cols: 481 | if potential_function_matrix[ni][nj] < potential_function_matrix[i][j]: 482 | local_min = False 483 | break 484 | if not local_min: 485 | break 486 | if local_min: 487 | NEs.append(((i, j), sum(cost_matrix[i][j]))) 488 | return min(NEs, key=lambda x: x[1])[0] 489 | 490 | --------------------------------------------------------------------------------