igp2.core package

Submodules

igp2.core.agentstate module

class igp2.core.agentstate.AgentMetadata(width: float, length: float, agent_type: str, initial_time: float | None = None, final_time: float | None = None, height: float | None = None, wheelbase: float | None = None, front_overhang: float | None = None, rear_overhang: float | None = None, front_track: float | None = None, back_track: float | None = None, drag_coefficient: float | None = None, friction_coefficient: float | None = None, max_acceleration: float | None = None, max_angular_vel: float | None = None)

Bases: object

Represents the physical properties of the Agent.

CAR_DEFAULT = {'agent_type': 'car', 'back_track': 1.535, 'drag_coefficient': 0.252, 'friction_coefficient': 0.7, 'front_overhang': 0.91, 'front_track': 1.543, 'height': 1.47, 'length': 4.689, 'max_acceleration': 5.0, 'max_angular_vel': 2.0, 'rear_overhang': 1.094, 'wheelbase': 2.686, 'width': 1.829}

agent_type: str

back_track: float = None

classmethod default_meta_frame(frame: Dict[int, AgentState]) → Dict[int, AgentMetadata]

Create a dictionary of metadata for agents in the given frame using the default agent metadata

drag_coefficient: float = None

final_time: float = None

friction_coefficient: float = None

front_overhang: float = None

front_track: float = None

height: float = None

initial_time: float = None

classmethod interleave(meta_dest: AgentMetadata, meta_src: dict | AgentMetadata) → AgentMetadata

length: float

max_acceleration: float = None

max_angular_vel: float = None

rear_overhang: float = None

wheelbase: float = None

width: float

class igp2.core.agentstate.AgentState(time: float, position: ~numpy.ndarray, velocity: ~numpy.ndarray, acceleration: ~numpy.ndarray, heading: float, metadata: ~igp2.core.agentstate.AgentMetadata = <factory>, macro_action: str | None = None, maneuver: str | None = None)

Bases: object

Dataclass storing data points that describe an exact moment in a trajectory.

The time field may represent either continuous time or time steps. Velocity and acceleration can be given both with vectors or floats.

acceleration: ndarray

heading: float

macro_action: str = None

maneuver: str = None

metadata: AgentMetadata

position: ndarray

property speed

time: float

to_hashable() → Tuple[float, float, float, float, float]

Returns a hashable representation of the state, that is useful for MCTS.

Returns:

5-tuple of the form (x-pos, y-pos, speed, heading, time)

velocity: ndarray

igp2.core.config module

class igp2.core.config.Configuration

Bases: object

Class that serves as central location to get or set global IGP2 parameters, such as maximum speed or swerving distance.

property fps: int

Framerate of simulation.

property max_oncoming_vehicle_dist

The maximum distance for a vehicle to be considered when giving way.

property max_speed: float

Global speed limit.

property min_speed: float

Minimum speed in turns.

property next_lane_offset: float

The lane offset used in AStar for getting the next lane.

classmethod set_properties(**kwargs)

Set any properties of IGP2 using a dictionary.

property target_switch_length

The ideal target length for a lane switch.

property velocity_stop: float

The threshold in m/s to classify velocities as stopped.

igp2.core.cost module

class igp2.core.cost.Cost(factors: Dict[str, float] | None = None, limits: Dict[str, float] | None = None)

Bases: object

Define the exact cost signal of a trajectory. The IGP2 paper refers to this as reward, which can be interpreted as negative cost.

property cost: float | None

The cost from the latest trajectory cost calculation call.

property cost_components: Dict[str, float] | None

Return a dictionary of cost components that were calculated with the last call to trajectory_cost()

cost_difference(trajectory1: Trajectory, trajectory2: Trajectory, goal: Goal) → float

Calculate the sum of the cost elements differences between two trajectories, given a goal. This is not a function that is used in the current implementation.

Parameters:

• trajectory1 – The first trajectory to examine

• trajectory2 – The second trajectory to examine

• goal – The goal to reach

Returns:

A scalar floating-point cost difference value

cost_difference_resampled(trajectory1: Trajectory, trajectory2: Trajectory, goal: Goal) → float

Calculate the sum of the cost elements differences between two trajectories given a goal, at sampled points along the pathlength

Parameters:

• trajectory1 – The first trajectory to examine

• trajectory2 – The second trajectory to examine

• goal – The goal to reach

Returns:

A scalar floating-point cost difference value

property factors: Dict[str, float]

Returns a dictionary of the cost factors.

fix_points(points, eps=0.0001)

property limits: Dict[str, float]

Returns a dictionary of the cost quantities’ absolute limits.

resample_trajectory(trajectory: VelocityTrajectory, n: int, k: int = 3)

trajectory_cost(trajectory: Trajectory, goal: Goal) → float

Calculate the total cost of the trajectory given a goal.

Parameters:

• trajectory – The trajectory to examine

• goal – The goal to reach

Returns:

A scalar floating-point cost value

igp2.core.goal module

class igp2.core.goal.BoxGoal(box: Box)

Bases: Goal

A goal specified with a rectangle.

property box: Box

The box defining the goal

distance(point: ndarray) → float

Calculate the distance of the given point to the Goal

passed_through_goal(trajectory: Trajectory) → bool

Calculate whether the given trajectory has passed through a goal or not.

point_on_lane(lane: Lane) → ndarray | None

Return the point closest to the box center on the given lane midline.

property poly: Polygon

The Polygon describing the bounding box of the Goal.

reached(point: ndarray) → bool

Returns whether a point is within the goal box / threshold radius. Goal boundary is inclusive.

class igp2.core.goal.Goal

Bases: ABC

property center: ndarray

Returns goal center point

abstract distance(point: ndarray) → float

Calculate the distance of the given point to the Goal

abstract passed_through_goal(trajectory: Trajectory) → bool

Calculate whether the given trajectory has passed through a goal or not.

abstract point_on_lane(lane: Lane) → ndarray | None

Return the closest point to the goal on the given lane midline.

abstract reached(point: ndarray) → bool

Returns whether a point is within the goal box / threshold radius. Goal boundary is inclusive.

class igp2.core.goal.PointCollectionGoal(goals: List[PointGoal])

Bases: Goal

A goal that consists of a collection of PointGoals.

property center: ndarray

The center of the point collection goal is defined as the point of the centers of the goals in the collection.

distance(point: ndarray) → float

Calculate the distance of the given point to the Goal

goals() → List[PointGoal]

Return the list of PointGoals in this goal collection.

passed_through_goal(trajectory: Trajectory) → bool

Calculate whether the given trajectory has passed through a goal or not.

point_on_lane(lane: Lane) → ndarray | None

Returns a point on the given lane that is closest to one of the goal points in the collection.

reached(point: ndarray) → bool

Returns whether a point is within the goal box / threshold radius. Goal boundary is inclusive.

class igp2.core.goal.PointGoal(point: ndarray, threshold: float)

Bases: Goal

A goal represented as a circle of with given threshold around a point.

distance(point: ndarray) → float

Calculate the distance of the given point to the Goal

passed_through_goal(trajectory: Trajectory) → bool

Calculate whether the given trajectory has passed through a goal or not.

point_on_lane(lane: Lane) → ndarray | None

Return the closest point to the goal on the given lane midline.

property radius: float

Threshold radius

reached(point: ndarray) → bool

Returns whether a point is within the goal box / threshold radius. Goal boundary is inclusive.

class igp2.core.goal.StoppingGoal(point: ndarray, threshold: float)

Bases: PointGoal

Subclass PointGoal to represent a stopping goal.

igp2.core.results module

class igp2.core.results.AgentResult(true_goal: int, datum: Tuple[int, GoalsProbabilities, float, ndarray] | None = None)

Bases: object

This class will store GoalsProbabilities objects containing goal prediction results belonging to a specific agent

add_data(datum: Tuple[int, GoalsProbabilities, float, ndarray])

Adds a new data point, in the form of the tuple (frame_id, GoalsProbabilities object, inference time, current position)

property goal_accuracy: ndarray

Returns True if the true goal is the most likely, false otherwise, for each data point.

property inference_time: float

Returns inference time for each data point.

property position: ndarray

Returns agent current position for each data point.

property reward_difference: ndarray

Returns reward difference associated with the true goal, for each data point.

property true_goal_probability: ndarray

Returns the probabilities of the true goal for each data point.

property zero_probability: ndarray

Returns true if the true goal has a zero probability (considered unfeasible), false otherwise, for each data point.

class igp2.core.results.AllMCTSResult(mcts_result: MCTSResult | None = None)

Bases: MCTSResultTemplate

Class to store results for all rollouts of an MCTS planning run.

add_data(mcts_result: MCTSResult)

Add a new rollout to the results collection.

property optimal_rollouts: List[MCTSResult]

Return a list of results that match the final plan of the ego.

property optimal_trace: tuple

A tuple of strings that gives the trace of the final selected optimal plan.

plot_q_values(key: Tuple, axis: Axes | None = None) → Axes

class igp2.core.results.EpisodeResult(metadata: EpisodeMetadata, id: int, cost_factors: Dict[str, float], datum: Tuple[int, AgentResult] = None)

Bases: object

This class stores result for an entire episode, where each data point contains an AgentResult object

add_data(datum: Tuple[int, AgentResult])

Adds a new data point, in the form of the tuple (agent_id, AgentResult object)

property goal_accuracy: ndarray

The accuracy across the episode, defined as the fraction of how many times the true goal was most likely, over the total number of data points.

property goal_accuracy_ste: ndarray

The standard error of the goal accuracy.

property inference_time: float

The average inference time accross the episode.

property reward_difference: ndarray

The average reward difference across the episode.

property reward_difference_median: ndarray

The median reward difference.

property reward_difference_std: ndarray

The standard deviation associated with the reward difference.

property true_goal_probability: ndarray

The mean true goal probability across the episode.

property true_goal_ste: ndarray

The standard error of the true goal probability across the episode.

property zero_probability: ndarray

The fraction of times the true goal was considered unfeasible, over the total number of data points.

class igp2.core.results.ExperimentResult(datum: Tuple[int, EpisodeResult] | None = None)

Bases: object

This class stores results for an entire experiment ran across multiple scenarios, each data point contains an EpisodeResult object

add_data(datum: Tuple[int, EpisodeResult])

Adds a data point, in the form of the tuple (recording_id, EpisodeResult object)

property goal_accuracy: ndarray

Calculates the average goal accuracy across all agents evaluated in the experiment.

property inference_time: float

Calculates the average inference_time across all agents evaluated in the experiment.

property inference_time_ste: float

Calculates the standard error of the inference time across all agents evaluated in the experiment.

property reward_difference: ndarray

Calculates the average reward difference across all agents evaluated in the experiment.

property true_goal_probability: ndarray

Calculates the average true goal probability across all agents evaluated in the experiment.

property zero_probability: ndarray

Calculates the average zero true goal probability across all agents evaluated in the experiment.

class igp2.core.results.MCTSResult(tree: Tree = None, samples: dict = None, trace: tuple = None)

Bases: MCTSResultTemplate

Store results for a single MCTS rollout.

property leaf: Node

Return the leaf node of this rollout.

property samples: Dict[int, Tuple[GoalWithType, VelocityTrajectory]]

Dictionary mapping agent IDs to their corresponding goal and trajectory sample in this rollout.

property trace: Tuple[str, ...]

The final trace that was chosen for the ego in this rollout.

property tree: Tree

The MCTS search tree at the completion of the rollout.

class igp2.core.results.MCTSResultTemplate

Bases: object

class igp2.core.results.PlanningResult(scenario_map: Map, mcts_result: MCTSResultTemplate | None = None, timestep: float | None = None, frame: Dict[int, AgentState] | None = None, goals_probabilities: GoalsProbabilities | None = None)

Bases: object

add_data(mcts_result: MCTSResultTemplate | None = None, timestep: float | None = None, frame: Dict[int, AgentState] | None = None, goals_probabilities: GoalsProbabilities | None = None)

class igp2.core.results.RunResult(agents: Dict[int, Agent], ego_id: int, ego_trajectory: Trajectory, collided_agents_ids: List[int], goal_reached: bool, selected_action: MCTSAction)

Bases: object

Class storing results of the simulated rollout in MCTS.

agents: Dict[int, Agent]

collided_agents_ids: List[int]

ego_id: int

property ego_maneuvers: List[str]

property ego_maneuvers_trajectories: List[VelocityTrajectory]

This returns the generated open loop trajectories for each maneuver.

ego_trajectory: Trajectory

goal_reached: bool

plot(t: int, scenario_map: Map, axis: Axes | None = None) → Axes

Plot the current agents and the road layout at timestep t for visualisation purposes. Plots the OPEN LOOP trajectories that the agents will attempt to follow, alongside a colormap representing velocities.

Parameters:

• t – timestep at which the plot should be generated

• scenario_map – The road layout

• axis – Axis to draw on

selected_action: MCTSAction

igp2.core.trajectory module

class igp2.core.trajectory.StateTrajectory(fps: int, states: List[AgentState] | None = None, path: ndarray | None = None, velocity: ndarray | None = None)

Bases: Trajectory

Implements a Trajectory that is built from discreet observations at given time intervals.

add_state(new_state: AgentState, reload_path: bool = True)

Add a new state at the end of the trajectory.

Parameters:

• new_state – AgentState. This should follow the last state of the trajectory in time.

• reload_path – If True then the path and velocity fields are recalculated.

calculate_path_and_velocity()

Recalculate path and velocity fields. May be used when the trajectory is updated.

extend(new_trajectory: StateTrajectory, reload_path: bool = True, reset_times: bool = False)

Extend the current trajectory with the states of the given trajectory. If the last state of the first

trajectory is equal to the first state of the second trajectory then the first state of the second trajectory is dropped.

Parameters:

• new_trajectory – The given trajectory to use for extension.

• reload_path – Whether to recalculate the path and velocity fields.

• reset_times – Whether to reset the timing information on the states.

classmethod from_velocity_trajectory(velocity_trajectory: VelocityTrajectory, fps: int = 20) → StateTrajectory

Convert a velocity trajectory to a StateTrajectory.

Parameters:

• velocity_trajectory – VelocityTrajectory to convert

• fps – Optional framerate argument

property heading: ndarray | None

Heading of vehicle in radians.

Returns:

array of heading value from dataset, or estimate from trajectory path if heading data is not available

property length: float | None

Length of trajectory in metres or None if trajectory is empty.

slice(start_idx: int | None, end_idx: int | None) → StateTrajectory

Return a slice of the original StateTrajectory

property states: List[AgentState]

Return the list of states.

property timesteps: ndarray | None

Time elapsed between two consecutive trajectory points in seconds.

class igp2.core.trajectory.Trajectory(path: ndarray | None = None, velocity: ndarray | None = None)

Bases: ABC

Base class for all Trajectory objects

VELOCITY_STOP = 0.1

property acceleration: ndarray

Accelerations corresponding to each position along the path.

property angular_acceleration: ndarray

Calculates angular acceleration (positive counter-clockwise).

property angular_velocity: ndarray

Calculates angular velocity (positive counter-clockwise), handling discontinuity at theta = pi

property curvature: ndarray

Calculates curvature of the trajectory at each point in the path.

differentiate(x: ndarray, y: ndarray, dx: ndarray | None = None, dy: ndarray | None = None)

Performs backward difference on data x y.

Notes

• First element is computed by forward difference using a continuity assumption.

• Can overload dx and dy if required.

• Will replace nonsensical values (Nan and +/- inf with 0.0).

property duration: float | None

Duration in seconds to cover the path with given velocities.

extend(new_trajectory: Trajectory)

Extend the trajectory in-place.

property final_agent_state: AgentState

AgentState calculated from the final point along the path.

property heading: ndarray

Heading of vehicle in radians.

Returns:

array of heading value from dataset, or estimate from trajectory path if heading data is not available

heading_from_path(path) → ndarray

Calculate headings at each point along the trajectory.

property initial_agent_state: AgentState

AgentState calculated from the first point along the path.

property jerk: ndarray

Jerk values corresponding to each position along the path.

property length: float | None

Length of trajectory in metres or None if trajectory is empty.

property path: ndarray

Sequence of positions along a path.

slice(start_idx: int | None, end_idx: int | None) → Trajectory

Return a slice of the trajectory between the given indices. Follows regular Python indexing standards.

property times: ndarray | None

Time elapsed (from start) along the trajectory in seconds.

property timesteps: ndarray | None

Time elapsed between two consecutive trajectory points in seconds.

trajectory_dt(path, velocity)

Calculate time elapsed between two consecutive points along the trajectory using the mean of the two velocities.

property velocity: ndarray

Velocities corresponding to each position along the path.

property velocity_stop: float

Velocity at or under which the vehicle is considered to be at a stop

class igp2.core.trajectory.VelocityTrajectory(path: ndarray, velocity: ndarray, heading: ndarray | None = None, timesteps: ndarray | None = None)

Bases: Trajectory

Define a trajectory consisting of a 2d paths and corresponding velocities

calculate_pathlength(path) → ndarray

extend(new_trajectory: ndarray | Trajectory)

Extends a Trajectory (in-place) at the end of the VelocityTrajectory object.

classmethod from_agent_state(state: AgentState) → VelocityTrajectory

classmethod from_agent_states(states: List[AgentState]) → VelocityTrajectory

property heading: ndarray

Heading of vehicle in radians.

Returns:

array of heading value from dataset, or estimate from trajectory path if heading data is not available

insert(trajectory: Trajectory)

Inserts a Trajectory (in-place) at the beginning of the VelocityTrajectory object, removing the first element of the original VelocityTrajectory.

property length: float

Length of trajectory in metres or None if trajectory is empty.

property pathlength: ndarray

Length of path travelled at each position along the path in meters.

slice(start_idx: int | None, end_idx: int | None) → VelocityTrajectory

Return a slice of the original VelocityTrajectory

property timesteps: ndarray | None

Time elapsed between two consecutive trajectory points in seconds.

igp2.core.util module

A collection of utility methods and classes used throughout the project.

class igp2.core.util.Box(center: ndarray, length: float, width: float, heading: float)

Bases: object

A class representing a 2D, rotated box in Euclidean space.

property boundary: ndarray

Returns the bounding Polygon of the Box.

calculate_boundary()

Calculate bounding box vertices from centroid, width and length

property diagonal: float

Length of the Box diagonal.

inside(p: ndarray) → bool

Check whether a point lies within the rectangle.

Ref: https://math.stackexchange.com/questions/190111/how-to-check-if-a-point-is-inside-a-rectangle

Parameters:

p – The point to check

Returns:

true iff the point lies in or on the rectangle

overlaps(other: Box) → bool

Check whether self overlaps with another Box.

Ref: https://stackoverflow.com/questions/306316/determine-if-two-rectangles-overlap-each-other

Parameters:

other – Other box to check

Returns:

true iff the two boxes overlap in some region

class igp2.core.util.Circle(centre: ndarray, radius: float)

Bases: object

Class implementing a circle

contains(point: ndarray)

checks whether a 2d point is contained in a circle

igp2.core.util.add_offset_point(trajectory: Trajectory, offset: float)

Add in-place a small step at the end of the trajectory to reach within the boundary of the next lane.

igp2.core.util.all_subclasses(cls)

igp2.core.util.calculate_multiple_bboxes(center_points_x: List[float], center_points_y: List[float], length: float, width: float, rotation: float = 0.0) → ndarray

Calculate bounding box vertices from centroid, width and length.

Parameters:

• center_points_x – center x-coord of bbox

• center_points_y – center y-coord of bbox

• length – length of bbox

• width – width of bbox

• rotation – rotation of main bbox axis (along length)

Returns:

np.ndarray containing the rotated vertices of each box

igp2.core.util.cart2pol(cart: ndarray) → Tuple[ndarray, ndarray]

Transform cartesian to polar coordinates.

Parameters:

cart – Nx2 np.ndarray

Returns:

Pair of Nx1 np.ndarrays

igp2.core.util.copy_agents_dict(agents_dict, agent_id)

igp2.core.util.find_lane_sequence(start_lane: Lane, end_lane: Lane, goal: Goal, max_iter: int = 100) → List[Lane]

Finds the shortest valid sequence of lanes from a starting to ending lane using A*.

igp2.core.util.get_curvature(points: ndarray) → ndarray

Gets the curvature of a 2D path based on https://en.wikipedia.org/wiki/Curvature#In_terms_of_a_general_parametrization

Parameters:

points – nx2 array of points

Returns:

curvature

igp2.core.util.get_linestring_side(ls: LineString, p: Point) → str

Return which side of the LineString is the given point, order by the sequence of the coordinates.

Parameters:

• ls – reference LineString

• p – point to check

Returns:

Either “left” or “right”

igp2.core.util.get_points_parallel(points: ndarray, lane_ls: LineString, lat_distance: float | None = None) → ndarray

Find parallel to lane_ls of given points (also on lane_ls) through a given point specified by points[0].

Parameters:

• points – Points that lie on lane_ls. The first element - points[0] - may lie elsewhere and specifies which location the parallel line must pass through.

• lane_ls – Reference LineString

• lat_distance – Optional latitudinal distance from the linestring. If not specified, it will be infered from the first element of points.

Returns:

A numpy array, where dim(points) is equal to dim(ret). Furthermore, points[0] == ret[0] is always true.

igp2.core.util.list_startswith(list1: list, list2: list) → bool

Compare two lists. If the lengths are equal simply return equality using ==. If lengths are unequal, then check whether the first one has the same element as the second one.

igp2.core.util.pol2cart(theta, r) → ndarray

Transform polar to cartesian coordinates.

Parameters:

• theta – Nx1 ndarray

• r – Nx1 ndarray

Returns:

Nx2 ndarray

igp2.core.vehicle module

class igp2.core.vehicle.Action(acceleration: float, steer_angle: float, target_speed: float | None = None, target_angle: float | None = None)

Bases: object

Represents an action taken by an agent

acceleration: float

steer_angle: float

target_angle: float = None

target_speed: float = None

class igp2.core.vehicle.KinematicVehicle(state: AgentState, meta: AgentMetadata, fps: int)

Bases: Vehicle

Class describing a physical vehicle object based on a bicycle-model.

execute_action(action: Action | None = None, next_state: AgentState | None = None) → Action

Apply acceleration and steering according to the bicycle model centered at the center-of-gravity (i.e. cg) of the vehicle.

Ref: https://dingyan89.medium.com/simple-understanding-of-kinematic-bicycle-model-81cac6420357

Parameters:

• action – Acceleration and steering action to execute

• next_state – Ignored

Returns:

Acceleration and heading action that was executed by the vehicle.

class igp2.core.vehicle.Observation(frame: Dict[int, AgentState], scenario_map: Map)

Bases: object

Represents an observation of the visible environment state and the road layout

frame: Dict[int, AgentState]

plot(ax: Axes | None = None) → Axes

Convenience method to plot the current observation.

scenario_map: Map

class igp2.core.vehicle.TrajectoryVehicle(state: AgentState, meta: AgentMetadata, fps: int)

Bases: Vehicle

execute_action(action: Action | None = None, next_state: AgentState | None = None)

Used next_state to set the state of the vehicle manually as given by an already calculated trajectory.

Parameters:

• action – Ignored

• next_state – Next state of the vehicle

class igp2.core.vehicle.Vehicle(state: AgentState, meta: AgentMetadata, fps: int)

Bases: Box

Base class for physical vehicle control.

execute_action(action: Action | None = None, next_state: AgentState | None = None)

Execute action given to the vehicle.

Parameters:

• action – Acceleration and steering action to execute

• next_state – Can be used to override action and set state directly

get_state(time: float | None = None) → AgentState

Return current state of the vehicle.

igp2.core.velocitysmoother module

class igp2.core.velocitysmoother.VelocitySmoother(n: int = 100, dt_s: float = 0.1, amax_m_s2: float = 5.0, vmin_m_s: float = 1.0, vmax_m_s: float = 10.0, lambda_acc: float = 10.0, horizon_threshold: float = 1.2, min_n: int = 5)

Bases: object

Runs optimisation routine on a VelocityTrajectory object to return realistic velocities according to constraints. This accounts for portions of the trajectory where the vehicle is stopped.

property amax: float

Returns the acceleration limit used in smoothing optimisation in m/s2

property dt: float

Returns the timestep size used in smoothing optimisation in seconds

property horizon_threshold: float

Returns the multiple of the estimated trajectory horizon that is used to limit the maximum value of n in the optimiser

property lambda_acc: float

Returns the lambda parameter used to control the weighting of the acceleration penalty in smoothing optimisation

lin_interpolant(x: ndarray, y: ndarray) → interpolant

Creates a differentiable Casadi interpolant object to linearly interpolate y from x

load_trajectory(trajectory: VelocityTrajectory)

property min_n: int

Returns minimum value for n

property n: int

Returns the number of points used in smoothing optimisation

optimiser(n: int, velocity_interpolant: interpolant, pathvel_interpolant: interpolant, ind_start: float, ind_end: float, x_start: float, v_start: float, options: {})

remove_duplicates(x: ndarray, v: ndarray) → Tuple[ndarray, ndarray]

Removes any duplicates in pathlength and velocity caused by consecutive 0 velocities

smooth_velocity(pathlength: ndarray, velocity: ndarray, debug: bool = False)

Creates a linear interpolants for pathlength and velocity, and use them to recursively run the optimiser, until the optimisation solution bounds the original pathlength. The smoothed velocity is then sampled from the optimisation solution v

split_at_stops()

Split original velocity and pathlength arrays, for the optimisation process to run separately on each splitted array. The splitted array corresponds of trajectory segments between stops. The function will also remove any extended stops from the splitted arrays (parts where consecutive velocities points are 0

property split_pathlength: ndarray

Returns the cummulative length travelled in the trajectory in meters

split_smooth(debug: bool = False) → ndarray

Split the trajectory into “go” and “stop” segments, according to vmin and smoothes the “go” segments

property split_velocity: ndarray

Returns the velocity at each trajectory waypoint in m/s

property vmax: float

Returns the maximum velocity limit used in smoothing optimisation in m/s

property vmin: float

Returns the minimum velocity limit used in smoothing optimisation in m/s. Vehicle will be considered at a stop for velocities below this value. The optimisation smoothing will not be run at points below this velocity

Module contents