"""Module to find the best path for a given local track and it's respective velocity profile."""
from typing import Union, Optional
import numpy as np
import numpy.typing as npt
from scipy import interpolate
import igraph
from .velocity_profile import VelocityProfile, VelocityProfileConfig
[docs]class LocalMotionPlannerConfig():
"""
Configuration Container for LocalMotionPlanner.
Attributes
----------
max_length_between_layer : float
Maximal length between two layers.
support_points_for_initial_interpolation_n : int
Support Points for the initial interpolation of the track.
nodes_per_layer : int
Number of nodes per layer.
vehicle_width : float
Width of vehicle.
buffer_to_trackbounds : float
Buffer to subtract from trackbounds to account for e.g. cone width and uncertainty.
support_points_between_nodes_n : int
Number of support points for each spline between subsequent nodes.
support_points_between_nodes : npt.NDArray
X Value of support points, precalculated to save some time.
support_points_for_trajectory_n : int
Number of support points for the resulting trajectory.
smooth_final_path : bool
Indicates whether the best path should be further improved by smoothing it.
plot_path : bool
Whether the planned motion should be plotted. Deprecated.
plot_velocity_profile : bool
Whether the velocity should be plotted. Deprecated.
weight_length : float
Weight of relative length for optimization.
weight_max_curvature : float
Weight of maximal squared curvature for optimization.
weight_average_curvature : float
Weight of average squared curvature for optimization.
"""
spline_solve_coefficient_matrix = np.array([[1, 1, 1], [3, 4, 5], [6, 12, 20]])
def __init__(self, max_length_between_layer: float = 1.5, support_points_for_initial_interpolation_n: int = 200,
nodes_per_layer: int = 9, vehicle_width: float = 1.4, buffer_to_trackbounds: float = 0.1,
support_points_between_nodes_n: int = 40, support_points_for_trajectory_n: int = 100,
smooth_final_path: bool = False, plot_path: bool = False,
plot_velocity_profile: bool = False, weight_length: float = 5,
weight_max_curvature: float = 10, weight_average_curvature: float = 100) -> None:
"""
Initialize Configuration for Local Motion Planner.
Parameters
----------
max_length_between_layer : float, optional
Maximal length between two layers., by default 1.5
support_points_for_initial_interpolation_n : int, optional
Support Points for the initial interpolation of the track, by default 200
nodes_per_layer : int, optional
Number of nodes per layer, by default 9
vehicle_width : float, optional
Width of vehicle, by default 1.4
buffer_to_trackbounds : float, optional
Buffer to subtract from trackbounds to account for e.g. cone width and uncertainty, by default 0.1
support_points_between_nodes_n : int, optional
Number of support points for each spline between subsequent nodes, by default 40
support_points_for_trajectory_n : int, optional
Number of support points for the resulting trajectory, by default 100
smooth_final_path : bool, optional
Indicates whether the best path should be further improved by smoothing it, by default False
plot_path : bool, optional
Whether the planned motion should be plotted. Deprecated, by default False
plot_velocity_profile : bool, optional
Whether the velocity should be plotted. Deprecated, by default False
weight_length : float, optional
Weight of relative length for optimization, by default 5
weight_max_curvature : float, optional
Weight of maximal squared curvature for optimization, by default 10
weight_average_curvature : float, optional
Weight of average squared curvature for optimization, by default 100
"""
self.max_length_between_layer = max_length_between_layer
self.support_points_for_initial_interpolation_n = support_points_for_initial_interpolation_n
self.nodes_per_layer = nodes_per_layer
self.vehicle_width = vehicle_width
self.buffer_to_trackbounds = buffer_to_trackbounds
self.support_points_between_nodes_n = support_points_between_nodes_n
support_points_between_nodes_i = np.linspace(0, 1, support_points_between_nodes_n)
support_points_between_nodes = np.c_[np.full(support_points_between_nodes_n, 1), support_points_between_nodes_i,
support_points_between_nodes_i, support_points_between_nodes_i,
support_points_between_nodes_i, support_points_between_nodes_i]
self.support_points_between_nodes = np.cumprod(support_points_between_nodes, axis=1)
self.weight_length = weight_length
self.weight_max_curvature = weight_max_curvature
self.weight_average_curvature = weight_average_curvature
self.support_points_for_trajectory_n = support_points_for_trajectory_n
self.smooth_final_path = smooth_final_path
self.plot_path = plot_path
self.plot_velocity_profile = plot_velocity_profile
[docs]class LocalMotionPlanner():
"""
Class to find best path for a local track and calculate a respective velocity profile.
Using a graph based approach for finding the best path by optimizing length and curvature.
..todo:: Document the attributes not initialized in init. An initialize them in the init. Obviously.
Attributes
----------
config : LocalMotionPlannerConfig
Config for local motion planning.
velocity_profile_config : VelocityProfileConfig
Config for calcuting the velocity profile.
centerpoints : npt.NDArray
Centerpoints of the inputed track, shape: (n, 2) with n equal number if centerpoints.
track_width : npt.NDArray
Track width of the inputed track, shape: (n, 1) with n equal number if centerpoints.
vehicle.psi : float
Current heading of the vehicle.
vehicle_position : npt.NDArray
Current position of the vehicle, shape: (2,).
vehicle_v : float
Current velocity of the vehicle.
mission_completed : bool
Indicates whether the mission is completed and the vehicle should come to a standstill.
costs : Optional[npt.NDArray]
Costs of the edges of the graph.
graph : Optional[igraph.Graph]
Graph object representing the graph used for finding the best path.
track_width_interpolated : Optional[npt.NDArray]
Interpolated track width, shape: (support_points_for_initial_interpolation_n, 2).
centerpoints_interpolated : Optional[npt.NDArray]
Interpolated centerpoints, shape: (support_points_for_initial_interpolation_n, 2).
normal_vectors : Optional[npt.NDArray]
Normal vectors for interpolated centerpoints, shape: (support_points_for_initial_interpolation_n, 2).
layer_n : Optional[int]
Number of layers in graph.
layer_tangents : Optional[npt.NDArray]
Tangents of the different layers, shape: (layer_n, 2).
node_positions : Optional[npt.NDArray]
Positions of nodes of the graph exclusive the root node, shape: (layer_n, nodes_per_layer, 2).
node_positions_flattened : Optional[npt.NDArray]
Positions of nodes of the graph exclusive the root node, shape: (layer_n * nodes_per_layer, 2).
spline_n : Optional[int]
Number of splines aka number of graph edges: nodes_per_layer + (layer_n - 1) * nodes_per_layer**2.
spline_poly_cofficients : Optional[npt.NDArray]
Polynomial coefficients of all splines for edges between nodes on subsequent layers,
shape: (2, spline_n, 6). First dimension is for x and y coordinate.
best_curvature : Optional[npt.NDArray]
Curvature of best path, shape: (layer_n * support_points_between_nodes_n, ).
best_path : Optional[npt.NDArray]
Optimized path calculated by motion planning, shape: (layer_n * support_points_between_nodes_n, 2).
improved_path : Optional[npt.NDArray]
Improved optimized path calculated by motion planning, shape: (layer_n * support_points_between_nodes_n, 2).
vp: Optional[VelocityProfile]
Velocity profile object for calcuting the respective velocity profile for the best or improved path.
velocity_profile : Optional[npt.NDArray]
Calculated velocity profile for best or improved path, shape: (layer_n * support_points_between_nodes_n, ).
trajectory : Optional[npt.NDArray]
Combined best or improved path and velocity profile, shape: (layer_n * support_points_between_nodes_n, 3).
"""
def __init__(self, local_motion_planner_config: LocalMotionPlannerConfig,
velocity_profile_config: VelocityProfileConfig,
centerpoints: npt.NDArray, track_width: npt.NDArray,
mission_completed: bool = False, vehicle_v: float = 0,
vehicle_psi: float = 0, vehicle_position: npt.NDArray = np.array([0, 0])) -> None:
"""
Initialize local motion planner.
Parameters
----------
local_motion_planner_config : LocalMotionPlannerConfig
Config for local motion planning.
velocity_profile_config : VelocityProfileConfig
Config for calcuting the velocity profile.
centerpoints : npt.NDArray
Centerpoints of the inputed track, shape: (n, 2) with n equal number if centerpoints.
track_width : npt.NDArray
Track width of the inputed track, shape: (n, 1) with n equal number if centerpoints.
mission_completed : bool, optional
Indicates whether the mission is completed and the vehicle should come to a standstill, by default False
vehicle_v : float, optional
Current velocity of the vehicle, by default 0
vehicle_psi : float, optional
Current heading of the vehicle, by default 0
vehicle_position : npt.NDArray, optional
Current position of the vehicle, shape: (2,), by default np.array([0, 0])
"""
self.config = local_motion_planner_config
self.velocity_profile_config = velocity_profile_config
self.centerpoints = centerpoints
self.track_width = track_width
self.vehicle_psi = vehicle_psi
self.vehicle_position = vehicle_position
self.vehicle_v = vehicle_v
self.mission_completed = mission_completed
self.costs: Optional[npt.NDArray] = None
self.graph: Optional[igraph.Graph] = None
self.track_width_interpolated: Optional[npt.NDArray] = None
self.centerpoints_interpolated: Optional[npt.NDArray] = None
self.normal_vectors: Optional[npt.NDArray] = None
self.layer_n: Optional[int] = None
self.layer_tangents: Optional[npt.NDArray] = None
self.node_positions: Optional[npt.NDArray] = None
self.node_positions_flattened: Optional[npt.NDArray] = None
self.spline_n: Optional[int] = None
self.spline_poly_cofficients: Optional[npt.NDArray] = None
self.best_curvature: Optional[npt.NDArray] = None
self.best_path: Optional[npt.NDArray] = None
self.improved_path: Optional[npt.NDArray] = None
self.vp: Optional[VelocityProfile] = None
self.velocity_profile: Optional[npt.NDArray] = None
self.trajectory: Optional[npt.NDArray] = None
[docs] def create_graph(self):
"""
Create a graph object based on the previously calculated nodes and the costs between those on subsequent layers.
Using iGraph as underlying library.
"""
self.graph = igraph.Graph(directed=True)
# Add nodes / vertices for possible path nodes
self.graph.add_vertices(self.node_positions_flattened.shape[0],
attributes={'pos_x': self.node_positions_flattened[:, 0],
'pos_y': self.node_positions_flattened[:, 1]})
# Add node / vertex for vehicle position
self.graph.add_vertex(pos_x=self.vehicle_position[0], pos_y=self.vehicle_position[1])
# Add virtual node to enable finding best path independent of the last path node
self.graph.add_vertex()
edge_name_array = np.empty((self.spline_n + self.config.nodes_per_layer, 2), dtype=int)
normal_nodes_n = (self.layer_n) * self.config.nodes_per_layer
edge_name_array[:self.config.nodes_per_layer, 0] = normal_nodes_n
edge_name_array[:self.config.nodes_per_layer, 1] = np.arange(self.config.nodes_per_layer)
edge_name_array[self.config.nodes_per_layer:-self.config.nodes_per_layer, 0] = \
np.repeat(np.arange(normal_nodes_n - self.config.nodes_per_layer), self.config.nodes_per_layer)
edge_name_array[self.config.nodes_per_layer:-self.config.nodes_per_layer, 1] = \
np.tile(np.arange(self.config.nodes_per_layer, normal_nodes_n).reshape(-1, self.config.nodes_per_layer),
self.config.nodes_per_layer).flatten()
edge_name_array[-self.config.nodes_per_layer:, 0] = \
np.arange(normal_nodes_n - self.config.nodes_per_layer, normal_nodes_n)
edge_name_array[-self.config.nodes_per_layer:, 1] = normal_nodes_n + 1
self.graph.add_edges(es=edge_name_array.tolist(), attributes={'weight': self.costs.tolist()})
[docs] def solve_spline_coefficient_problem(self, splines_sub_n: int,
node_start_positions: Union[npt.NDArray, float],
node_start_tangents: Union[npt.NDArray, float],
node_end_positions: Union[npt.NDArray, float],
node_end_tangents: Union[npt.NDArray, float]) -> npt.NDArray:
"""
Solve the linear equation system for the third order polynomial for the given constraints.
Parameters
----------
splines_sub_n : int
Number of problems to solve.
node_start_positions : Union[npt.NDArray, float]
Value at x=0, shape: (n) with n equal the number of problems to solve.
node_start_tangents : Union[npt.NDArray, float]
Deriviate at x=0, shape: (n) with n equal the number of problems to solve.
node_end_positions : Union[npt.NDArray, float]
Value at x=1, shape: (n) with n equal the number of problems to solve.
node_end_tangents : Union[npt.NDArray, float]
Deriviate at x=1, shape: (n) with n equal the number of problems to solve.
Returns
-------
npt.NDArray
Resulting coeffiecients for the third order polynomial,
shape: (n, 3) with n equal the number of problems to solve.
"""
right = np.empty((splines_sub_n, 3))
right[:, 1] = node_end_tangents - node_start_tangents
right[:, 2] = 0
right[:, 0] = node_end_positions - node_start_positions - node_start_tangents
return np.linalg.solve(self.config.spline_solve_coefficient_matrix[np.newaxis, :, :], right)
[docs] def calculate_spline_coefficients(self):
"""
Calculate polynomial coefficients for a sixth order polynomial based on the constaints.
Position and tangent of nodes are constrained. Also the second deriviate must be zero.
"""
self.spline_poly_cofficients = np.empty((2, self.spline_n, 6))
# calculate spline coefficients between vehicle position and 0th layer
self.spline_poly_cofficients[:, :self.config.nodes_per_layer, 0] = self.vehicle_position[:, np.newaxis]
self.spline_poly_cofficients[0, :self.config.nodes_per_layer, 1] = np.cos(self.vehicle_psi)
self.spline_poly_cofficients[1, :self.config.nodes_per_layer, 1] = np.sin(self.vehicle_psi)
self.spline_poly_cofficients[:, :self.config.nodes_per_layer, 2] = 0
self.spline_poly_cofficients[0, :self.config.nodes_per_layer, 3:6] = self.solve_spline_coefficient_problem(
splines_sub_n=self.config.nodes_per_layer,
node_start_positions=self.vehicle_position[0], node_start_tangents=np.cos(self.vehicle_psi),
node_end_positions=self.node_positions[0, :, 0], node_end_tangents=self.layer_tangents[0, 0])
self.spline_poly_cofficients[1, :self.config.nodes_per_layer, 3:6] = self.solve_spline_coefficient_problem(
splines_sub_n=self.config.nodes_per_layer,
node_start_positions=self.vehicle_position[1], node_start_tangents=np.sin(self.vehicle_psi),
node_end_positions=self.node_positions[0, :, 1], node_end_tangents=self.layer_tangents[0, 1])
# calculate all spline coefficients between the different layers
node_start_positions = np.repeat(
self.node_positions_flattened[:self.config.nodes_per_layer * (self.layer_n - 1)],
self.config.nodes_per_layer, axis=0)
node_end_positions = np.repeat(self.node_positions[1:], self.config.nodes_per_layer, axis=0).reshape((-1, 2))
node_start_tangents = np.repeat(self.layer_tangents[:-1], self.config.nodes_per_layer**2, axis=0)
node_end_tangents = np.repeat(self.layer_tangents[1:], self.config.nodes_per_layer**2, axis=0)
self.spline_poly_cofficients[:, self.config.nodes_per_layer:, 0] = node_start_positions.swapaxes(0, 1)
self.spline_poly_cofficients[:, self.config.nodes_per_layer:, 1] = node_start_tangents.swapaxes(0, 1)
self.spline_poly_cofficients[:, self.config.nodes_per_layer:, 2] = 0
self.spline_poly_cofficients[0, self.config.nodes_per_layer:, 3:6] = self.solve_spline_coefficient_problem(
splines_sub_n=(self.layer_n - 1) * self.config.nodes_per_layer**2,
node_start_positions=node_start_positions[:, 0], node_start_tangents=node_start_tangents[:, 0],
node_end_positions=node_end_positions[:, 0], node_end_tangents=node_end_tangents[:, 0])
self.spline_poly_cofficients[1, self.config.nodes_per_layer:, 3:6] = self.solve_spline_coefficient_problem(
splines_sub_n=(self.layer_n - 1) * self.config.nodes_per_layer**2,
node_start_positions=node_start_positions[:, 1], node_start_tangents=node_start_tangents[:, 1],
node_end_positions=node_end_positions[:, 1], node_end_tangents=node_end_tangents[:, 1])
self.spline_poly_cofficients_derivation1 = self.spline_poly_cofficients[:, :, 1:] * np.array([1, 2, 3, 4, 5])
self.spline_poly_cofficients_derivation2 = self.spline_poly_cofficients[:, :, 3:] * np.array([6, 12, 20])
[docs] def calculate_splines(self):
"""Calculate splines based on its polynomial coefficients."""
self.splines = np.einsum('ab,dcb->acd', self.config.support_points_between_nodes,
self.spline_poly_cofficients)
self.splines_derivation1 = np.einsum('ab,dcb->acd', self.config.support_points_between_nodes[:, :5],
self.spline_poly_cofficients_derivation1)
self.splines_derivation2 = np.einsum('ab,dcb->acd', self.config.support_points_between_nodes[:, 1:4],
self.spline_poly_cofficients_derivation2)
[docs] def calculate_spline_curvatures(self):
"""Calculate curvature for all splines."""
self.curvature = np.abs(np.divide(
np.multiply(self.splines_derivation1[:, :, 0], self.splines_derivation2[:, :, 1])
- np.multiply(self.splines_derivation1[:, :, 1], self.splines_derivation2[:, :, 0]),
np.power(np.sum(np.square(self.splines_derivation1), axis=2), 3 / 2)))
self.average_sqaured_curvature = np.average(np.power(self.curvature, 2), axis=0)
self.average_sqaured_curvature_min = np.amin(self.average_sqaured_curvature)
self.average_sqaured_curvature_max = np.amax(self.average_sqaured_curvature)
self.average_sqaured_curvature_delta = self.average_sqaured_curvature_max - self.average_sqaured_curvature_min
if self.average_sqaured_curvature_delta != 0:
self.average_sqaured_curvature_prozentual = \
((self.average_sqaured_curvature - self.average_sqaured_curvature_min)
/ self.average_sqaured_curvature_delta)
else:
self.average_sqaured_curvature_prozentual = np.zeros_like(self.average_sqaured_curvature)
self.peak_sqaured_curvature = np.amax(np.power(self.curvature, 2), axis=0)
self.peak_sqaured_curvature_min = np.amin(self.peak_sqaured_curvature)
self.peak_sqaured_curvature_max = np.amax(self.peak_sqaured_curvature)
self.peak_sqaured_curvature_delta = self.peak_sqaured_curvature_max - self.peak_sqaured_curvature_min
if self.peak_sqaured_curvature_delta != 0:
self.peak_sqaured_curvature_prozentual = \
(self.peak_sqaured_curvature - self.peak_sqaured_curvature_min) / self.peak_sqaured_curvature_delta
else:
self.peak_sqaured_curvature_prozentual = np.zeros_like(self.peak_sqaured_curvature)
[docs] def calculate_spline_lengths(self):
"""Calculate real and relative length of splines."""
self.length = np.sum(np.sqrt(np.sum(np.power(np.diff(self.splines, axis=0), 2), axis=2)), axis=0)
self.length_min = np.amin(self.length)
self.length_max = np.amax(self.length)
self.length_delta = self.length_max - self.length_min
if self.length_delta != 0:
self.length_prozentual = (self.length - self.length_min) / self.length_delta
else:
self.length_prozentual = np.zeros_like(self.length)
[docs] def calculate_costs(self):
"""Calculate total cost based on respective costs for length and average and peak squared curvature."""
self.costs = self.length_prozentual * self.config.weight_length \
+ self.average_sqaured_curvature_prozentual * self.config.weight_average_curvature \
+ self.peak_sqaured_curvature_prozentual * self.config.weight_max_curvature
[docs] def get_best_path(self):
"""Get best path from graph by searching for graph with smallest total cost."""
normal_nodes_n = (self.layer_n) * self.config.nodes_per_layer
sub_spline_indices = self.graph.get_shortest_paths(normal_nodes_n, to=normal_nodes_n + 1,
weights='weight', mode='out', output='epath')[0][:-1]
self.best_path = self.splines.swapaxes(0, 1)[sub_spline_indices][:, :-1].reshape((-1, 2))
self.best_curvature = self.curvature.swapaxes(0, 1)[sub_spline_indices][:, :-1].flatten()
[docs] def improve_path(self):
"""Improve best path by interpolating it with CubicSplines."""
path_i = np.linspace(0, self.best_path.shape[0] - 1, self.layer_n + 1, dtype=int)
path_i2 = np.linspace(0, 1, self.layer_n + 1)
spline_i2 = np.linspace(0, 1, 300)
spline = interpolate.CubicSpline(path_i2, self.best_path[path_i], extrapolate=False,
bc_type=((2, (0, 0)), (1, self.layer_tangents[-1].tolist())))
self.improved_path = spline(spline_i2)
[docs] def calculate_trajectory(self):
"""Calculate velocity profile for previously calculated best path."""
self.vp = VelocityProfile(self.improved_path, velocity_profile_config=self.velocity_profile_config,
mission_completed=self.mission_completed)
self.velocity_profile = self.vp.calculate(v_initial=self.vehicle_v)
self.trajectory = np.c_[self.improved_path, self.velocity_profile]
[docs] def prepare_node_grid(self):
"""
Prepare node grid: calculting path points for nodes.
Interpolates path and centerpoints with cubic splines. Calculate tangents, normal vectors for support points.
Choose equidistant point on interpolated centerpoints, they will be used as layers.
Calculate k equidistant points on the normal for those chosen centerpoints,
they will be used as nodes for the graph.
"""
# Interpolate trackwidth linearly and centerpoints with cubic splines.
centerpoints_i = np.arange(0, self.centerpoints.shape[0], 1)
spline_right_track_width = interpolate.interp1d(centerpoints_i, self.track_width[:, 0])
spline_left_track_width = interpolate.interp1d(centerpoints_i, self.track_width[:, 0])
spline_x = interpolate.CubicSpline(centerpoints_i, self.centerpoints[:, 0], bc_type='natural')
spline_y = interpolate.CubicSpline(centerpoints_i, self.centerpoints[:, 1], bc_type='natural')
support_points_i = np.linspace(0, centerpoints_i.shape[0] - 1,
self.config.support_points_for_initial_interpolation_n, endpoint=True)
self.track_width_interpolated = np.c_[spline_left_track_width(support_points_i),
spline_right_track_width(support_points_i)]
reduced_track_width_interpolated = np.maximum(
self.track_width_interpolated - (self.config.vehicle_width / 2 + self.config.buffer_to_trackbounds), 0)
# Generate new interpolated centerpoints an their first derivation, to be able to calculate tangente and normal
centerpoints_interpolated_x = spline_x(support_points_i)
centerpoints_interpolated_y = spline_y(support_points_i)
self.centerpoints_interpolated = np.c_[centerpoints_interpolated_x, centerpoints_interpolated_y]
centerpoints_interpolated_x_1st_derivation = spline_x(support_points_i, 1)
centerpoints_interpolated_y_1st_derivation = spline_y(support_points_i, 1)
# calculate tangente and normal
tangente = \
([centerpoints_interpolated_x_1st_derivation, centerpoints_interpolated_y_1st_derivation]
/ np.sqrt(centerpoints_interpolated_x_1st_derivation**2 + centerpoints_interpolated_y_1st_derivation**2)).T
self.normal_vectors = \
([-centerpoints_interpolated_y_1st_derivation, centerpoints_interpolated_x_1st_derivation]
/ np.sqrt(centerpoints_interpolated_x_1st_derivation**2 + centerpoints_interpolated_y_1st_derivation**2)).T
# calculate track length, so that layer points are nearly equally distanced
track_length = np.cumsum(np.sqrt(np.sum(np.power(
np.diff(self.centerpoints_interpolated, axis=0, prepend=self.centerpoints_interpolated[:1]), 2), axis=1)))
total_track_length = track_length[-1]
# calculate the indices of the interpolated centerpoints for equally distanced layer points.
layer_lengths = np.linspace(0, total_track_length,
int(np.ceil(total_track_length / self.config.max_length_between_layer)))
layer_point_indices = np.searchsorted(track_length, layer_lengths)
# calculate relative nodes for every layer based on the respective track width
layer_track_widths = np.linspace(-(reduced_track_width_interpolated[layer_point_indices, 1]),
reduced_track_width_interpolated[layer_point_indices, 0],
self.config.nodes_per_layer).T
relative_node_positions = np.einsum('ab, ad -> abd', layer_track_widths,
self.normal_vectors[layer_point_indices])
# calculate nodes based on relative nodes and the global selected center points
self.node_positions = relative_node_positions + self.centerpoints_interpolated[layer_point_indices,
np.newaxis, :]
self.node_positions_flattened = self.node_positions.reshape((-1, 2))
self.layer_tangents = tangente[layer_point_indices]
self.layer_n = self.node_positions.shape[0]
self.spline_n = self.config.nodes_per_layer + (self.layer_n - 1) * self.config.nodes_per_layer**2
[docs] def calculate_best_path_from_centerpoints(self):
"""
Calculate best path based on the multiple centerpoints.
.. todo:: Explain how algorithm works.
"""
self.prepare_node_grid()
self.calculate_spline_coefficients()
self.calculate_splines()
self.calculate_spline_curvatures()
self.calculate_spline_lengths()
self.calculate_costs()
self.create_graph()
self.get_best_path()
if self.config.smooth_final_path:
self.improve_path()
else:
self.improved_path = self.best_path
[docs] def calculate_path_from_single_centerpoint(self):
"""
Calculate best path based on velocity pose and one centerpoint.
Calculating single sixth order polynomial between current vehicle position and single centerpoint.
Start and end heading should be the current vehicle heading.
"""
spline_poly_cofficients = np.empty((2, 6))
spline_poly_cofficients[:, 0] = self.vehicle_position
spline_poly_cofficients[0, 1] = np.cos(self.vehicle_psi)
spline_poly_cofficients[1, 1] = np.sin(self.vehicle_psi)
spline_poly_cofficients[:, 2] = 0
spline_poly_cofficients[0, 3:6] = self.solve_spline_coefficient_problem(
splines_sub_n=1,
node_start_positions=self.vehicle_position[0], node_start_tangents=np.cos(self.vehicle_psi),
node_end_positions=self.centerpoints[0, 0], node_end_tangents=np.cos(self.vehicle_psi)
)
spline_poly_cofficients[1, 3:6] = self.solve_spline_coefficient_problem(
splines_sub_n=1,
node_start_positions=self.vehicle_position[1], node_start_tangents=np.sin(self.vehicle_psi),
node_end_positions=self.centerpoints[0, 1], node_end_tangents=np.sin(self.vehicle_psi)
)
spline_poly_cofficients_derivation1 = spline_poly_cofficients[:, 1:] * np.array([1, 2, 3, 4, 5])
spline_poly_cofficients_derivation2 = spline_poly_cofficients[:, 3:] * np.array([6, 12, 20])
splines_derivation1 = np.einsum('ab,cb->ac', self.config.support_points_between_nodes[:, :5],
spline_poly_cofficients_derivation1)
splines_derivation2 = np.einsum('ab,cb->ac', self.config.support_points_between_nodes[:, 1:4],
spline_poly_cofficients_derivation2)
self.best_curvature = np.abs(np.divide(
np.multiply(splines_derivation1[:, 0], splines_derivation2[:, 1]) - np.multiply(splines_derivation1[:, 1],
splines_derivation2[:, 0]),
np.power(np.sum(np.square(splines_derivation1), axis=1), 3 / 2)))
self.improved_path = np.einsum('ab,cb->ac', self.config.support_points_between_nodes, spline_poly_cofficients)
[docs] def plan(self):
"""Plan path and calculate velocity profile based on it."""
if self.centerpoints.shape[0] > 1:
self.calculate_best_path_from_centerpoints()
else:
self.calculate_path_from_single_centerpoint()
self.calculate_trajectory()
[docs] def calculate_track(self) -> npt.NDArray:
"""
Calculate global track bounds based on track widths and normal vectors.
Returns
-------
npt.NDArray
Track bounds, shape: (2m, 2) with m equal number of interpolated center points.
"""
track_width = self.track_width_interpolated.copy()
track_width[:, 0] = - track_width[:, 0]
relative_trackbounds = np.einsum('ab, ad -> abd', track_width, self.normal_vectors)
track_bounds = (relative_trackbounds + self.centerpoints_interpolated[:, np.newaxis])
ordered_track_bounds = np.r_[track_bounds[:, 0, :], track_bounds[::-1, 1, :]]
return np.r_[ordered_track_bounds, ordered_track_bounds[:1]]
[docs] def plot(self):
"""Plot results and informations of planned trajectory."""
import matplotlib.pyplot as plt
from matplotlib import gridspec
import matplotlib
matplotlib.rc('font', size=15)
matplotlib.rc('axes', titlesize=20)
gs = gridspec.GridSpec(4, 3)
fig = plt.figure(figsize=(24, 12))
ax_path = plt.subplot(gs[:3, :2]) # row , col
colors = plt.cm.viridis(np.linspace(0, 1, self.splines.shape[1]))[np.argsort(np.argsort(self.costs))]
colors[:, 3] = 0.4
plt.rcParams['figure.figsize'] = (20, 20)
plt.rcParams['figure.dpi'] = 200
ax_path.scatter(self.node_positions_flattened[:, 0], self.node_positions_flattened[:, 1],
color='b', label='Nodes')
ax_path.scatter(self.vehicle_position[0], self.vehicle_position[1], color='b')
ax_path.axis('equal')
for plot_i in range(self.splines.shape[1]):
ax_path.plot(self.splines[:, plot_i, 0], self.splines[:, plot_i, 1], color=colors[plot_i])
# ax_path.plot(self.splines[:, :, 0], self.splines[:, :, 1]) # noqa: E800
ax_path.arrow(self.vehicle_position[0], self.vehicle_position[1], np.cos(self.vehicle_psi),
np.sin(self.vehicle_psi), head_width=0.1, head_length=0.2, label='Vehicle Position')
# ax_path.plot(self.best_path[:, 0], self.best_path[:, 1], label='Best path') # noqa: E800
ax_path.plot(self.centerpoints_interpolated[:, 0], self.centerpoints_interpolated[:, 1],
label='Interpolated Centerline', color='magenta')
ax_path.plot(self.improved_path[:, 0], self.improved_path[:, 1], color='r', label='Optimal path')
track_bounds = self.calculate_track()
ax_path.plot(track_bounds[:, 0], track_bounds[:, 1], label='Interpolated Track Boundaries')
fig.colorbar(plt.cm.ScalarMappable(norm=None, cmap='viridis'), ax=ax_path, label='costs (normalized)')
ax_path.legend()
ax_path.set_xlabel('x [m]')
ax_path.set_ylabel('y [m]')
ax_length = plt.subplot(gs[0, 2])
ax_length.set_title(f'Length Histogram: x{self.config.weight_length}')
ax_length.hist(self.length, bins=30, density=True, alpha=1, color='blue')
ax_avg_curvature = plt.subplot(gs[1, 2])
ax_avg_curvature.set_title(f'Average curvature Histogram: x{self.config.weight_average_curvature}')
ax_avg_curvature.hist(np.average(np.power(self.curvature, 2), axis=0), bins=30,
density=True, alpha=1, color='blue')
ax_max_curvature = plt.subplot(gs[2, 2])
ax_max_curvature.set_title(f'Max curvature Histogram: x{self.config.weight_max_curvature}')
ax_max_curvature.hist(np.max(self.curvature, axis=0), bins=30, density=True, alpha=1, color='blue')
ax_velocity = plt.subplot(gs[3, :])
ax_curvature = ax_velocity.twinx()
ax_velocity.plot(self.vp.cum_distance, self.vp.velocity_max,
label='Maximal possible velocity', color='orange', alpha=0.5)
ax_velocity.plot(self.vp.cum_distance, self.vp.acceleration_velocity,
label='Acceleration velocity', color='g', alpha=0.5)
ax_velocity.plot(self.vp.cum_distance, self.vp.brake_velocity,
label='Deceleration velocity', color='k', alpha=0.5)
ax_velocity.scatter([0, self.vp.cum_distance[-1]], [self.vehicle_v, 0],
label='Velocity constraints', color='purple', alpha=0.5, marker='x')
ax_curvature.plot(self.vp.cum_distance, self.vp.curvature, label='Curvature', color='b', alpha=0.5)
# ax3.plot(self.cum_distance, self.radius, label='Radius', color='y', alpha=0.5) # noqa: E800
# ax3.set_ylim(top=10, bottom=-10) # noqa: E800
# ax3.grid() # noqa: E800
ax_velocity.plot(self.vp.cum_distance, self.vp.velocity, label='Resulting velocity', color='r')
ax_curvature.scatter(self.vp.cum_distance[self.vp.apex],
self.vp.curvature[self.vp.apex], label='Apexes', color='pink')
# ax4.plot(self.trajectory[:, 0], self.trajectory[:, 1]) # noqa: E800
ax_curvature.legend(loc=2)
ax_curvature.set_ylabel('v [m^-1]')
box = ax_velocity.get_position()
ax_velocity.set_position([box.x0, box.y0, box.width * 0.9, box.height])
ax_velocity.legend(loc='center left', bbox_to_anchor=(1.05, 0.5))
ax_velocity.set_xlabel('s [m]')
ax_velocity.set_ylabel('v [m/s]')
# plt.savefig('/workspace/as_ros/src/motion_planning2.png', bbox_inches='tight', transparent=True) # noqa: E800
plt.show()