Trajectory Planner

Package used to find a collision less trajectory between a number of agents.

Algorithm

The algorithm used is the one presented in this paper: [CIT3]

Exemple

A1 = Agent([0.0, 2.0, 0.0])
A1.set_goal([4.0, 2.0, 0.0])

A2 = Agent([4.0, 1.9999, 0.0], goal=[0.0, 1.9999, 0.0])

solver = TrajectorySolver([A1, A2])
solver.set_obstacle(obstacles)

solver.set_wait_for_input(False)
solver.set_slow_rate(1.0)

solver.solve_trajectories()

Run Demo Scripts

It’s possible to test the trajectory algorithm by running demo scripts (.../trajectory_planner/demos.)

To execute a demo, simply uncomment the desired demonstration in demos.py -> demo and run the script (demo.py).

It’s possible to add new demonstrations by adding them in either: demos_formation.py, demos_positions.py or demos_random.py.

Benchmark Algo Performances

The algorithm benchmarks can be calculated using .../trajectory_planner/demos/performance.py. The script will output:

• The success rate (%)
• Total compute time (sec)
• Total travel time (sec)

Those statistics are always calculated by running the same 9 demos.

Todo

Script to compare and analyse results of different configurations (issue #86). With a graphic interface?

Profile algorithm

To find hotspots in the algorithm:

$ cd .../trajectory_planner/demos
$ python -m cProfile -o perfo.prof performance.py
$ snakeviz perfo.prof

Note

Snakeviz is required to visualize results: pip install snakeviz

Solver Class

class trajectory_solver.TrajectorySolver(agent_list=None, solver_args=None, verbose=True)

To solve trajectories of all agents

Parameters:

• agent_list (list of Agents) – Compute trajectory of all agents in the list
• verbose (bool) – If True, prints information

Variables:

• horizon_time (float) – Horizon to predict trajectory [sec]
• step_interval (float) – Time steps interval (h) [sec]
• steps_in_horizon (int) – Number of time steps in horizon (k)
• interp_time_step (float) – Interpolation time step [sec]
• k_t (int) – Current time step
• k_max (int) – Max time step
• at_goal (bool) – True when all agents reached their goal
• in_collision (bool) – True when a collision is detected between two agents
• agents (list of Agent) – List of all agents
• n_agents (int) – Number of agents
• all_agents_traj (3k x n_agents array) – Latest predicted trajectory of each agent over horizon
• agents_distances (list of float) –
• kapa (int) – For more aggressive trajectories
• error_weight (float) – Error weigth
• effort_weight (float) – Effort weigth
• input_weight (float) – Input variation weigth
• relaxation_weight_p (float) – Quadratic relaxation weigth
• relaxation_weight_q (float) – Linear relaxation weigth
• relaxation_max_bound (float) – Maximum relaxation, 0
• relaxation_min_bound (float) – Minimum relaxation
• r_min (float) – Mimimum distance between agents [m]
• a_max (float) – Maximum acceleration of an agent [m/s^2]
• a_min (float) – Minimum acceleration of an agent [m/s^2]
• has_fix_obstacle (bool) – True if there are fix obstacles to avoid
• obstacle_positions (list of tuple) – Coordinates of obstacles to avoid (x, y, z)

set_obstacles(obstacle_positions)

Add obstacle has an agent with a cst position

Parameters:obstacle_positions (list of tuple) – Position of each obstacle (x, y, z)

initialize_agents()

Initialize positions and starting trajectory of all agents

initialize_matrices()

Compute matrices used to determine new states

\begin{align*} A &= \begin{bmatrix} I_3 & hI_3\\ 0_3 & I_3 \end{bmatrix}\\ \\ B &= \begin{bmatrix} \frac{h^2}{2}I_3\\ hI_3 \end{bmatrix}\\ \\ \Lambda &= \begin{bmatrix} B & 0_3 & ... & 0_3\\ AB & B & ... & 0_3\\ ... & ... & ... & ...\\ A^{k-1}B & A^{k-2}B & ... & B \end{bmatrix}\\ \\ A_0 &= \begin{bmatrix} A^T & (A^2)^T & ... & (A^k)^T \end{bmatrix}^T\\ \end{align*}

update_agents_info()

To update agents information

Has to be called when starting position or goal of an agent changed

solve_trajectories()

Compute a collision free trajectory for each agent.

Core of the algorithm.

Returns:If the algorithm was succesfull float: Total trajectory time Return type:bool

print_final_positions()

Print final position of all agents

plot_trajectories()

Plot all computed trajectories

Agent Class

class agent.Agent(agent_args, start_pos=None, goal=None)

Represents a single agent

agent_idx = None

Index of agent in positions

n_steps = None

Number of steps in horizon

Type:int

final_traj = None

Agent final trajectory

close_agents = None

float): Distance of each agent within a certain radius

Type:(dict of int

collision_step = None

Step of prediction where collision happens

set_starting_position(position)

Set starting position

Parameters:position (list of float) – [x, y, z]

set_goal(goal)

Set agent goal

Parameters:goal (list of float) – [x, y, z]

set_all_traj(all_trajectories)

Set last predicted trajectories of all agents

Parameters:all_trajectories (6*k x n_agents array) –

add_state(new_state)

Add new state to list of positions

Parameters:new_state (array) – Trajectory at time step

initialize_position(n_steps, all_agents_traj)

Initialize position of the agent.

Sets first horizon as a straight line to goal at a cst speed

Parameters:

• n_steps (int) – Number of time steps of horizon
• all_agents_traj (3k x n_agents array) – Last predicted traj of each agent (ptr)

check_goal()

Check if agent reached it’s goal.

Goal is considered reach when the agent is in a radius smaller than goal_dist_thres at a speed lower than goal_speed_thres.

Returns:True if goal reached Return type:bool

check_collisions()

Check current predicted trajectory for collisions.

1 - For all predicted trajectory, check distance of all the other agents

2 - If distance < Rmin: In collision

3 - If collision: Find all close agents

Returns:Step of collision (-1 if no collision) (dict of float): Close agents and their distance at collision step Return type:(int)

interpolate_traj(time_step_initial, time_step_interp)

Interpolate agent’s trajectory using a Bezier curbe.

Parameters:

• time_step_initial (float) – Period between samples
• time_step_interp (float) – Period between interpolation samples

Trajectory Plotter

Module to plot the trajectories of agents.

Circles represent the agents, dashed line the predicted trajectory over the horizon

class trajectory_plotting.TrajPlot(agent_list, time_step, interp_time_step, wait_for_input=False, plot_dots=False)

To plot trajectories of agents

slow_rate = None

To slow animation

Type:int

set_wait_for_input(to_wait)

To wait or not for input before switching frame

Parameters:to_wait (bool) – To wait

set_slow_rate(slow_rate)

Set slow rate of animation.

Rate of 1 is real time. Rate of 2 is twice slower

Parameters:slow_rate (float) – Rate of slow

set_axes_limits(xmax, ymax)

Set x and y axes max limits

Parameters:

• xmax (float) –
• ymax (float) –

set_dot_plotting(to_plot)

To plot or not agent’s predicted trajectory over horizon as dots

Parameters:to_wait (bool) – To plot dots

update_objects(agent_list)

Update agents

Parameters:agent_list (list of Agent) – All agents with their trajectories and goal

init_animated_objects()

Creates all objects to animate.

Each agent has:

• A circle (current position)
• A dashed line (predicted trajectory)
• An X (goal)

Notes

Structure of animated object. Idx:

0: circle of agent 1 1: line of agent 1 2: circle of agent 2 3: line of agent 2 … -1: time text

init_animation()

Initialize animation

animate(frame)

Animate

Parameters:frame (int) – Current frame

run()

Start animation

plot_obstacle(obstacles)

Plot obstacle