2.2.2.2. local_motion package

2.2.2.2.1. Submodules

2.2.2.2.2. local_motion.debug_motion_planning module

local_motion.debug_motion_planning.find_nearest_centerpoint(centerpoints, vehicle_position)

local_motion.debug_motion_planning.limit_global_centerpoints_by_cutting(centerpoints, vehicle_position, max_length: bool = True)

local_motion.debug_motion_planning.limit_global_centerpoints_by_rolling(centerpoints, vehicle_position)

2.2.2.2.3. local_motion.local_motion_planner module

Module to find the best path for a given local track and it’s respective velocity profile.

class local_motion.local_motion_planner.LocalMotionPlanner(local_motion_planner_config: LocalMotionPlannerConfig, velocity_profile_config: VelocityProfileConfig, centerpoints: ndarray[Any, dtype[ScalarType]], track_width: ndarray[Any, dtype[ScalarType]], mission_completed: bool = False, vehicle_v: float = 0, vehicle_psi: float = 0, vehicle_position: ndarray[Any, dtype[ScalarType]] = array([0, 0]))

Bases: object

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.

config

Config for local motion planning.

Type:

LocalMotionPlannerConfig

velocity_profile_config

Config for calcuting the velocity profile.

Type:

VelocityProfileConfig

centerpoints

Centerpoints of the inputed track, shape: (n, 2) with n equal number if centerpoints.

Type:

npt.NDArray

track_width

Track width of the inputed track, shape: (n, 1) with n equal number if centerpoints.

Type:

npt.NDArray

vehicle.psi

Current heading of the vehicle.

Type:

float

vehicle_position

Current position of the vehicle, shape: (2,).

Type:

npt.NDArray

vehicle_v

Current velocity of the vehicle.

Type:

float

mission_completed

Indicates whether the mission is completed and the vehicle should come to a standstill.

Type:

bool

costs

Costs of the edges of the graph.

Type:

Optional[npt.NDArray]

graph

Graph object representing the graph used for finding the best path.

Type:

Optional[igraph.Graph]

track_width_interpolated

Interpolated track width, shape: (support_points_for_initial_interpolation_n, 2).

Type:

Optional[npt.NDArray]

centerpoints_interpolated

Interpolated centerpoints, shape: (support_points_for_initial_interpolation_n, 2).

Type:

Optional[npt.NDArray]

normal_vectors

Normal vectors for interpolated centerpoints, shape: (support_points_for_initial_interpolation_n, 2).

Type:

Optional[npt.NDArray]

layer_n

Number of layers in graph.

Type:

Optional[int]

layer_tangents

Tangents of the different layers, shape: (layer_n, 2).

Type:

Optional[npt.NDArray]

node_positions

Positions of nodes of the graph exclusive the root node, shape: (layer_n, nodes_per_layer, 2).

Type:

Optional[npt.NDArray]

node_positions_flattened

Positions of nodes of the graph exclusive the root node, shape: (layer_n * nodes_per_layer, 2).

Type:

Optional[npt.NDArray]

spline_n

Number of splines aka number of graph edges: nodes_per_layer + (layer_n - 1) * nodes_per_layer**2.

Type:

Optional[int]

spline_poly_cofficients

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.

Type:

Optional[npt.NDArray]

best_curvature

Curvature of best path, shape: (layer_n * support_points_between_nodes_n, ).

Type:

Optional[npt.NDArray]

best_path

Optimized path calculated by motion planning, shape: (layer_n * support_points_between_nodes_n, 2).

Type:

Optional[npt.NDArray]

improved_path

Improved optimized path calculated by motion planning, shape: (layer_n * support_points_between_nodes_n, 2).

Type:

Optional[npt.NDArray]

vp

Velocity profile object for calcuting the respective velocity profile for the best or improved path.

Type:

Optional[VelocityProfile]

velocity_profile

Calculated velocity profile for best or improved path, shape: (layer_n * support_points_between_nodes_n, ).

Type:

Optional[npt.NDArray]

trajectory

Combined best or improved path and velocity profile, shape: (layer_n * support_points_between_nodes_n, 3).

Type:

Optional[npt.NDArray]

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

calculate_best_path_from_centerpoints()

Calculate best path based on the multiple centerpoints.

Todo

Explain how algorithm works.

calculate_costs()

Calculate total cost based on respective costs for length and average and peak squared curvature.

calculate_path_from_single_centerpoint()

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.

calculate_spline_coefficients()

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.

calculate_spline_curvatures()

Calculate curvature for all splines.

calculate_spline_lengths()

Calculate real and relative length of splines.

calculate_splines()

Calculate splines based on its polynomial coefficients.

calculate_track() → ndarray[Any, dtype[ScalarType]]

Calculate global track bounds based on track widths and normal vectors.

Returns:

Track bounds, shape: (2m, 2) with m equal number of interpolated center points.

Return type:

npt.NDArray

calculate_trajectory()

Calculate velocity profile for previously calculated best path.

create_graph()

Create a graph object based on the previously calculated nodes and the costs between those on subsequent layers.

Using iGraph as underlying library.

get_best_path()

Get best path from graph by searching for graph with smallest total cost.

improve_path()

Improve best path by interpolating it with CubicSplines.

plan()

Plan path and calculate velocity profile based on it.

plot()

Plot results and informations of planned trajectory.

prepare_node_grid()

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.

solve_spline_coefficient_problem(splines_sub_n: int, node_start_positions: ndarray[Any, dtype[ScalarType]] | float, node_start_tangents: ndarray[Any, dtype[ScalarType]] | float, node_end_positions: ndarray[Any, dtype[ScalarType]] | float, node_end_tangents: ndarray[Any, dtype[ScalarType]] | float) → ndarray[Any, dtype[ScalarType]]

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:

Resulting coeffiecients for the third order polynomial, shape: (n, 3) with n equal the number of problems to solve.

Return type:

npt.NDArray

class local_motion.local_motion_planner.LocalMotionPlannerConfig(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)

Bases: object

Configuration Container for LocalMotionPlanner.

max_length_between_layer

Maximal length between two layers.

Type:

float

support_points_for_initial_interpolation_n

Support Points for the initial interpolation of the track.

Type:

int

nodes_per_layer

Number of nodes per layer.

Type:

int

vehicle_width

Width of vehicle.

Type:

float

buffer_to_trackbounds

Buffer to subtract from trackbounds to account for e.g. cone width and uncertainty.

Type:

float

support_points_between_nodes_n

Number of support points for each spline between subsequent nodes.

Type:

int

support_points_between_nodes

X Value of support points, precalculated to save some time.

Type:

npt.NDArray

support_points_for_trajectory_n

Number of support points for the resulting trajectory.

Type:

int

smooth_final_path

Indicates whether the best path should be further improved by smoothing it.

Type:

bool

plot_path

Whether the planned motion should be plotted. Deprecated.

Type:

bool

plot_velocity_profile

Whether the velocity should be plotted. Deprecated.

Type:

bool

weight_length

Weight of relative length for optimization.

Type:

float

weight_max_curvature

Weight of maximal squared curvature for optimization.

Type:

float

weight_average_curvature

Weight of average squared curvature for optimization.

Type:

float

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

spline_solve_coefficient_matrix = array([[ 1,  1,  1],        [ 3,  4,  5],        [ 6, 12, 20]])

2.2.2.2.4. local_motion.velocity_profile module

Module to calculate a velocity profile for a local planned path.

class local_motion.velocity_profile.VelocityProfile(path: ~numpy.ndarray[~typing.Any, ~numpy.dtype[~numpy._typing._generic_alias.ScalarType]], mission_completed: bool = False, velocity_profile_config: ~local_motion.velocity_profile.VelocityProfileConfig = <local_motion.velocity_profile.VelocityProfileConfig object>)

Bases: object

Class to calculate a velocity profile for a given path.

g

Gravitational constant.

Type:

float

a_y_max

Maximal allowed lateral acceleration.

Type:

float

a_x_max

Maximal allowed longitudinal acceleration.

Type:

float

stretch_time_for_mpc

Whether the velocity profile should be stretched for the mpc.

Type:

bool

mpc_min_time_horizont

Time the velocity profile should be stretched to for the mpc. Deprecated.

Type:

float

v_max

Maximal allowed velocity.

Type:

float

v_end

Velocity for the last point of the trajectory.

Type:

float

a_x_max_completed

Maximal allowed longitudinal velocity for braking when mission is completed to come to a standstill.

Type:

float

v_initial

Velocity at first path point.

Type:

Optional[float]

mission_completed

Indicates whether the mission is completed and the vehicle should come to a standstill.

Type:

bool

path

Path for which the velocity profile should be calculated for, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

curvature

Curvature of the path, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

radius

Radius of the path, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

delta_distance

Distance between each subsequent pair of path points, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

cum_distance

Cumulative distance of path, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

apex

List of apex points, shape: (m, 2) with m equal number of apex points.

Type:

npt.NDArray

velocity_max

Maximal possible velocity based on the path curvature and maximal longitudinal acceleration, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

brake_velocity

Maximal possible velocity based on accelerating reverse from one support point to the previous one, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

acceleration_velocity

Maximal possible velocity based on accelerating forward from one support point to the next one, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

velocity

Resulting velocity profile by combining maximal possible velocity based on curvature, braking and accelerating, shape: (n, ) with n equal number of path points.

Type:

npt.NDArray

Initialize VelocityProfile instance.

Parameters:

• path (npt.NDArray) – Path for which the velocity profile should be calculated, shaoe: (n, 2) with n equal number of path points.

• mission_completed (bool, optional) – Indicates whether the mission is completed and the vehicle should come to a standstill, by default False

• velocity_profile_config (VelocityProfileConfig, optional) – Configuration for the velocity profile, by default VelocityProfileConfig()

acceleration_forward()

Accelerate forward from one support point to the next.

Todo

This Code should be reprogrammed and the mathematic approaches documented. When doing so, you could also implement a GGV-Map

acceleration_reverse()

Accelerate reverse from one support point to the previous.

Todo

This Code should be reprogrammed and the mathematic approaches documented. When doing so, you could also implement a GGV-Map

brake() → ndarray[Any, dtype[ScalarType]]

Calculate velocity profile by delecerating forward from first point.

Uses maximal allowed longitudinal acceleration for braking.

Returns:

Velocity profile, shape: (n, ) with n equal number of path points.

Return type:

npt.NDArray

calculate(v_initial: float | None = None, v_end: float | None = None) → ndarray[Any, dtype[ScalarType]]

Calculate a velocity profile for the previously set path.

Calculates apex points with their respective maximal allowed velocities according to the maximal lateral velocity and their radius. Calculates velocities between support points (first, last and apex points) constraint by the maximal longitudinal velocity.

Parameters:

• v_initial (float, optional) – Velocity for the first path point., by default None

• v_end (float, optional) – Velocity for the last path point., by default None

Returns:

Calculated velocity profile, shape: (n, ) with n equal number of path points.

Return type:

npt.NDArray

distance()

Calculate distance between each path points and cumulative distance between path points.

find_apex()

Find apexes in path by finding peaks in the curvature.

find_longitudinal_speed()

Calculate maximal possible longitudinal velocity based on radius and the maximal allowed lateral velocity.

limit_v_max_for_skidpad()

Limit the maximum allowed velocity for skidpad.

Uses maximal allowed lateral acceleration for calculating the maximal allowed velocity.

plot()

Plot velocity profile and their statistics.

real_speed() → ndarray[Any, dtype[ScalarType]]

Calculate resulting speed by combining maximal velocity, brake velocity and acceleration velocity.

Returns:

Velocity profile, shape: (n, ) with n equal numbe rof path points.

Return type:

npt.NDArray

class local_motion.velocity_profile.VelocityProfileConfig(g: float = 9.81, a_y_factor: float = 0.8, a_x_factor: float = 0.9, stretch_time_for_mpc: bool = False, mpc_min_time_horizont: float = 1.1, v_max: float = 15.5, v_end: float = 0, a_x_factor_completed: float = 0.8)

Bases: object

Configuration Container for VelocityProfile.

g

Gravitational constant.

Type:

float

a_y_factor

Factor for lateral acceleration.

Type:

float

a_x_factor

Factor for longitudinal acceleration.

Type:

float

stretch_time_for_mpc

Whether the velocity profile should be stretched for the mpc.

Type:

bool

mpc_min_time_horizont

Time the velocity profile should be stretched to for the mpc. Deprecated.

Type:

float

v_max

Maximal allowed velocity.

Type:

float

v_end

Velocity for the last point of the trajectory.

Type:

float

a_x_factor_completed

Factor for longitudinal velocity for braking when mission is completed to come to a standstill.

Type:

float

Initialize Configuration for VelocityProfile.

Parameters:

• g (float, optional) – Gravitational constant, by default 9.81

• a_y_factor (float, optional) – Factor for lateral acceleration, by default 0.8

• a_x_factor (float, optional) – Factor for longitudinal acceleration, by default 0.9

• stretch_time_for_mpc (bool, optional) – Whether the velocity profile should be stretched for the mpc, by default False

• mpc_min_time_horizont (float, optional) – Time the velocity profile should be stretched to for the mpc. Deprecated, by default 1.1

• v_max (float, optional) – Maximal allowed velocity, by default 15.5

• v_end (float, optional) – Velocity for the last point of the trajectory, by default 0

• a_x_factor_completed (float, optional) – Factor for longitudinal velocity for braking when mission is completed to come to a standstill, by default 0.8