# Trajectory planning for multi-agent systems¶

Multi-agent path finding (MAPF) is a problem that has been studied for a large number of years for applications including robotics, aviation, video games, and more [Dresner and Stone 2008, Ma et al. 2017, Pallottino et al. 2007, Silver 2005, Wurman et al. 2008]. It raises the question of how multiple agents can reach their goal position without colliding with each other.

In this case study we are using contracts defined in pacti to ensure that the resulting path is conflict-free. We are focusing on vertex conflicts, where two or more agents cannot share a grid cell, and swapping conflicts, preventing two agents from swapping positions in two adjacent cells. For a detailed survey on objective functions and conflict types, see [Stern et al. 2019].

In the following section we will explain the problem setup and illustrate our approach of using contracts to find a safe strategy for the robots. The benefit of using contracts for this application is how modularly we can add/remove robots or additional contraints to the problem.

In the video below you can see the implementation of the resulting strategy for five robots on the Robotarium [Wilson et al. 2020]. This case study shows how Pacti can be used to formalize a multiagent navigation problem and synthesize a strategy for the robots to reach their respective goal position without crashing into each other.

We will start by importing the relevant packages.

%matplotlib inline
import numpy as np
from utils.multiagent_utils import (
Coord,
Robot,
distance,
find_move_candidates_three,
get_collision_contract,
get_swapping_contract,
strategy,
strategy_multiple,
)
from utils.plotting_utils import (
Snapshot,
animate,
plot_grid_world,
save_trace
)

from pacti.iocontract import Var
from pacti.contracts import PolyhedralIoContractCompound
from typing import List


### Create gridworld and robots¶

First we will create the grid world with the dimensions $$grid_{n} \times grid_{m}$$ for our robots to move in. Each robot starts at their respective initial condition, with the task to reach its goal position on the grid. To initialize the robots and set up the grid world, we will call the functions that we imported from multiagent_utils.

# Define the grid world dimensions n x m
grid_n = 6
grid_m = 3

# Initialize the robots
r1 = Robot("A", (0, 0), (5, 2))
r2 = Robot("B", (0, 2), (5, 0))
robots = [r1, r2]

plot_grid_world(grid_n, grid_m, robots) ### Define dynamics contracts for each robot per timestep¶

We will break up the execution into individual timesteps as our planning horizon to reduce the complexity of the planning problem. This horizon length can be increased depending on the problem to be solved, but for the purpose of this case study we will do our planning in increments of single time steps.

The figure below will give an overview of the process per timestep, on the right we will see the resulting traces of the robots when following a strategy found using Pacti. This figure shows how we will use the contracts for each time step to place constraints on the movement of the robots. The contracts that we will take into account are $$\mathcal{C}_{\text{dyn}}$$, which describes the dynamics of each robot, $$\mathcal{C}_{\text{collision}}$$, which will ensure that no two robots will occupy the same grid cell, and $$\mathcal{C}_{\text{swapping}}$$, which ensure that two robots cannot swap positions (they would collide during the transition). For each time step we will repeat this process and update the positions of the robots according to the chosen move. Applying this strategy will result in a trajectory as shown on the right side of the figure.

Now we define the contracts for the robots' dynamics, the input variables are the current position of each robot in $$x$$ and $$y$$ coordinates and the current timestep $$t=i$$. The output variables are the next position of the robots $$(x_{i+1},y_{i+1})$$ and the next timestep $$i+1$$.

In the assumptions we define the initial positions and time step as the current position of the robot and the current timestep:

\begin{align*} t_i &= t \\ x_i &= x \\ y_i &= y \end{align*}

The guarantees of the contract are defining the dynamics and ensure that the time step increases by 1.

$t_{i+1} - t_i = 1$

The dynamics allow any single cell vertical or horizontal transitions to an adjacent cell on the grid (but not diagonal) and are specified as

$|x_i - x_{i+1}| + |y_i - y_{i+1}| \leq 1.$

We will break this constraint up into four separate constraints:

\begin{align*} & x_{i+1} - x_i + y_{i+1} - y_i \leq 1 \\ & x_{i+1} - x_i - y_{i+1} + y_i \leq 1 \\ & -x_{i+1} + x_i + y_{i+1} - y_i \leq 1 \\ & -x_{i+1} + x_i - y_{i+1} + y_i \leq 1 \end{align*}

We also ensure that the robot has to remain within the grid, specified as:

\begin{align*} &x_{i+1} < grid_n \\ &y_{i+1} < grid_m \end{align*}

Note that we need one dynamics contract for each robot, as the contract can only describe the dynamics for the robot whose behavior it describes. We are extending the input and output variables to include the variables for the other robots and the other contracts that we will use later, but each robot's dynamics contract only specifies the guarantees for its respecive robot's coordinates for the next timestep. In the code the subscript $$0$$ corresponds to timestep $$i$$ and the subscript $$1$$ corresponds to timestep $$i+1$$.

This contract implementing the constraints explained above is defined in the following function:

def get_dynamics_contract_robot(name: str, init_pos: Coord, timestep: int) -> PolyhedralIoContractCompound:  # noqa: WPS210
"""
Function to set up the contract encoding the dynamics for a single robot for the next timestep.

Args:
name: name of the robot
init_pos: current coordinates of the robot
timestep: current timestep

Returns:
Resulting contract compound that encodes the dynamics for the robot.
"""
x_str_0 = 'x_{0}_0'.format(name)
y_str_0 = 'y_{0}_0'.format(name)
x_str_1 = 'x_{0}_1'.format(name)
y_str_1 = 'y_{0}_1'.format(name)
t_0 = 't_0'
t_1 = 't_1'

contract = PolyhedralIoContractCompound.from_strings(
input_vars=[x_str_0, y_str_0, t_0],
output_vars=[x_str_1, y_str_1, t_1],
assumptions=[
[
"{0} = {1}".format(x_str_0, init_pos.x),
"{0} = {1}".format(y_str_0, init_pos.y),
"{0} = {1}".format(t_0, timestep),
]
],
guarantees=[
[
"{0} - {1} = 1".format(t_1, t_0),
"{0} - {1} + {2} - {3} <= 1".format(x_str_1, x_str_0, y_str_1, y_str_0),
"{0} - {1} - {2} + {3} <= 1".format(x_str_1, x_str_0, y_str_1, y_str_0),
"-{0} + {1} + {2} - {3} <= 1".format(x_str_1, x_str_0, y_str_1, y_str_0),
"-{0} + {1} - {2} + {3} <= 1".format(x_str_1, x_str_0, y_str_1, y_str_0),
"{0} <= {1}".format(x_str_1, grid_n - 1),
"{0} <= {1}".format(y_str_1, grid_m - 1),
"-{0} <= 0".format(x_str_1),
"-{0} <= 0".format(y_str_1),
]
],
)
return contract


### Collision Conflict Contract¶

Next we describe the contracts that will ensure that our robots do not collide. For this we will place constraints on the distance between a pair of robots, and ensure that is will always be larger or equal to one cell - meaning two robots will never share the same cell (i.e. collide).

To describe the this requirement in linear inequalities, we need to break up the collision requirement into four inequalities. Each inequality will place constraints on both robots for a specific configuration with regards to each other (for example robot A is to the top right of robot B).

As the guarantees of these contracts are mutually exclusive (robots can only be in one configuration at a time - Robot A cannot be to the right and to the left of robot B at the same time), we need to consider each of these collision quadrants separately in our analysis. To achieve this, we are using a PolyhedralIoContractCompound which allows for disjoint guarantees (and disjoint assumptions if desired).

The linear inequalities ensuring no collision for two robots $$A$$ and $$B$$ are defined as follows.

We assume that the two robots do not collide:

$\text{distance}(A,B) \geq 1$

The guarantees are that at least one of these inequalities is satisfied:

\begin{align*} &x^A_1 - x^B_1 + y^A_1 - y^B_1 \leq -1 \\ &x^A_1 - x^B_1 - y^A_1 + y^B_1 \leq -1 \\ &x^A_1 + x^B_1 + y^A_1 - y^B_1 \leq -1 \\ &x^A_1 + x^B_1 - y^A_1 + y^B_1 \leq -1 \end{align*}
def collision_contract() -> PolyhedralIoContractCompound:
"""
Function to ensure no collisions between two robots.

Returns:
Resulting contract compound that ensures no collisions.
"""
contract = PolyhedralIoContractCompound.from_strings(
input_vars=["current_distance"],
output_vars=["x_A_1", "y_A_1", "x_B_1", "y_B_1"],
assumptions=[["-current_distance <= -1"]],
guarantees=[
["x_A_1 - x_B_1 + y_A_1 - y_B_1 <= -1"],
["- x_A_1 + x_B_1 - y_A_1 + y_B_1 <= -1"],
["x_A_1 - x_B_1 - y_A_1 + y_B_1 <= -1"],
["- x_A_1 + x_B_1 + y_A_1 - y_B_1 <= -1"],
],
)
return contract


### Swapping (Dynamic Collision) Contract¶

Next we define the contract that determines that the robots will also stay collision free during their transition to another cell, meaning that two robots cannot swap places in the grid, as they would collide while traversing the same edge in opposite directions. We do this by manipulating the $$x$$ and $$y$$ coordinates of the two robots as follows:

\begin{align*} &\delta x = (x^A_i - x^B_i) * (x^A_{i+1} - x^B_{i+1}) \\ &\delta y = (y^A_i - y^B_i) * (y^A_{i+1} - y^B_{i+1}) \\ &\delta_x + \delta_y \geq 1 \end{align*}

Of course this contract also assumes no collision.

We manipulated the variables for this contract to reduce the number of disjoint polyhedra, instead of encoding the requirement as another set of docjoint guarantees on the positions.

def swapping_contract() -> PolyhedralIoContractCompound:
"""
Function to ensure no swapping between two robots.

Returns:
Resulting contract compound that ensures no swapping.
"""
contract = PolyhedralIoContractCompound.from_strings(
input_vars=["delta_x", "delta_y", "current_distance"],
output_vars=[],
assumptions=[
[
"-current_distance <= -1",
]
],
guarantees=[
[
"-delta_x - delta_y <= -1",
]
],
)
return contract


Now we use Pacti to create the contracts from the functions and display them to confirm we set them up as desired.

c_dyn_a = get_dynamics_contract_robot(r1.name, r1.pos, 0)
c_dyn_b = get_dynamics_contract_robot(r2.name, r2.pos, 0)
c_collision = collision_contract()
c_swapping = swapping_contract()

print("Dynamics Contract Robot A:\n {0} \n".format(c_dyn_a))
print("Dynamics Contract Robot B:\n {0} \n".format(c_dyn_b))
print("Collision Contract:\n {0} \n".format(c_collision))
print("Swapping Contract:\n {0}".format(c_swapping))

Dynamics Contract Robot A:
InVars: [x_A_0, y_A_0, t_0]
OutVars:[x_A_1, y_A_1, t_1]
A: [
x_A_0 = -0
y_A_0 = -0
t_0 = -0
]
G: [
-t_0 + t_1 = 1
|-x_A_0 + x_A_1 - y_A_0 + y_A_1| <= 1
|-x_A_0 + x_A_1 + y_A_0 - y_A_1| <= 1
x_A_1 <= 5
y_A_1 <= 2
-x_A_1 <= -0
-y_A_1 <= -0
]

Dynamics Contract Robot B:
InVars: [x_B_0, y_B_0, t_0]
OutVars:[x_B_1, y_B_1, t_1]
A: [
x_B_0 = -0
y_B_0 = 2
t_0 = -0
]
G: [
-t_0 + t_1 = 1
|-x_B_0 + x_B_1 - y_B_0 + y_B_1| <= 1
|-x_B_0 + x_B_1 + y_B_0 - y_B_1| <= 1
x_B_1 <= 5
y_B_1 <= 2
-x_B_1 <= -0
-y_B_1 <= -0
]

Collision Contract:
InVars: [current_distance]
OutVars:[x_A_1, y_A_1, x_B_1, y_B_1]
A: [
-current_distance <= -1
]
G: [
x_A_1 - x_B_1 + y_A_1 - y_B_1 <= -1
]
or
[
-x_A_1 + x_B_1 - y_A_1 + y_B_1 <= -1
]
or
[
x_A_1 - x_B_1 - y_A_1 + y_B_1 <= -1
]
or
[
-x_A_1 + x_B_1 + y_A_1 - y_B_1 <= -1
]

Swapping Contract:
InVars: [delta_x, delta_y, current_distance]
OutVars:[]
A: [
-current_distance <= -1
]
G: [
-delta_x - delta_y <= -1
]


### Merging the contracts¶

We are ready to start merging the contracts. We start by merging the dynamics contracts for each of the robots. This contract now holds all the information for possible steps that both robots can take, but it does not ensure no collisions yet. Now let's take a look at the contract describing the dynamics for both robots.

c_combined_dynamics = c_dyn_a.merge(c_dyn_b)
print(c_combined_dynamics)

InVars: [x_A_0, y_A_0, t_0, x_B_0, y_B_0]
OutVars:[x_A_1, y_A_1, t_1, x_B_1, y_B_1]
A: [
x_A_0 = -0
y_A_0 = -0
t_0 = -0
x_B_0 = -0
y_B_0 = 2
]
G: [
-t_0 + t_1 = 1
|-x_A_0 + x_A_1 - y_A_0 + y_A_1| <= 1
|-x_A_0 + x_A_1 + y_A_0 - y_A_1| <= 1
x_A_1 <= 5
y_A_1 <= 2
-x_A_1 <= -0
-y_A_1 <= -0
|-x_B_0 + x_B_1 - y_B_0 + y_B_1| <= 1
|-x_B_0 + x_B_1 + y_B_0 - y_B_1| <= 1
x_B_1 <= 5
y_B_1 <= 2
-x_B_1 <= -0
-y_B_1 <= -0
]


Next we will merge the collision and swapping conflict contracts with each other and then with the combined dynamics contract c_combined_dynamics to make sure that the robots will never collide and adhere to their dynamics.

c_no_conflicts = c_collision.merge(c_swapping)
c_dynamics_no_conflicts = c_combined_dynamics.merge(c_no_conflicts)
print(c_dynamics_no_conflicts)

InVars: [x_A_0, y_A_0, t_0, x_B_0, y_B_0, current_distance, delta_x, delta_y]
OutVars:[x_A_1, y_A_1, t_1, x_B_1, y_B_1]
A: [
x_A_0 = -0
y_A_0 = -0
t_0 = -0
x_B_0 = -0
y_B_0 = 2
-current_distance <= -1
]
G: [
-t_0 + t_1 = 1
|-x_A_0 + x_A_1 - y_A_0 + y_A_1| <= 1
|-x_A_0 + x_A_1 + y_A_0 - y_A_1| <= 1
x_A_1 <= 5
y_A_1 <= 2
-x_A_1 <= -0
-y_A_1 <= -0
|-x_B_0 + x_B_1 - y_B_0 + y_B_1| <= 1
|-x_B_0 + x_B_1 + y_B_0 - y_B_1| <= 1
x_B_1 <= 5
y_B_1 <= 2
-x_B_1 <= -0
-y_B_1 <= -0
x_A_1 - x_B_1 + y_A_1 - y_B_1 <= -1
-delta_x - delta_y <= -1
]
or
[
-t_0 + t_1 = 1
|-x_A_0 + x_A_1 - y_A_0 + y_A_1| <= 1
|-x_A_0 + x_A_1 + y_A_0 - y_A_1| <= 1
x_A_1 <= 5
y_A_1 <= 2
-x_A_1 <= -0
-y_A_1 <= -0
|-x_B_0 + x_B_1 - y_B_0 + y_B_1| <= 1
|-x_B_0 + x_B_1 + y_B_0 - y_B_1| <= 1
x_B_1 <= 5
y_B_1 <= 2
-x_B_1 <= -0
-y_B_1 <= -0
-x_A_1 + x_B_1 - y_A_1 + y_B_1 <= -1
-delta_x - delta_y <= -1
]
or
[
-t_0 + t_1 = 1
|-x_A_0 + x_A_1 - y_A_0 + y_A_1| <= 1
|-x_A_0 + x_A_1 + y_A_0 - y_A_1| <= 1
x_A_1 <= 5
y_A_1 <= 2
-x_A_1 <= -0
-y_A_1 <= -0
|-x_B_0 + x_B_1 - y_B_0 + y_B_1| <= 1
|-x_B_0 + x_B_1 + y_B_0 - y_B_1| <= 1
x_B_1 <= 5
y_B_1 <= 2
-x_B_1 <= -0
-y_B_1 <= -0
x_A_1 - x_B_1 - y_A_1 + y_B_1 <= -1
-delta_x - delta_y <= -1
]
or
[
-t_0 + t_1 = 1
|-x_A_0 + x_A_1 - y_A_0 + y_A_1| <= 1
|-x_A_0 + x_A_1 + y_A_0 - y_A_1| <= 1
x_A_1 <= 5
y_A_1 <= 2
-x_A_1 <= -0
-y_A_1 <= -0
|-x_B_0 + x_B_1 - y_B_0 + y_B_1| <= 1
|-x_B_0 + x_B_1 + y_B_0 - y_B_1| <= 1
x_B_1 <= 5
y_B_1 <= 2
-x_B_1 <= -0
-y_B_1 <= -0
-x_A_1 + x_B_1 + y_A_1 - y_B_1 <= -1
-delta_x - delta_y <= -1
]


Now we can evaluate the resulting contract c_dynamics_no_conflicts to find all possible moves that the robots can take which will satisfy our specification. From this list we can implement any desired strategy that will lead the robots to their goal.

def find_move_candidates(robot1: Robot, robot2: Robot, T_0: int, contract: PolyhedralIoContractCompound) -> list:  # noqa: N803,WPS210
"""
Function to find the possible next positions for the next timestep.

Args:
robot1: first robot
robot2: second robot
T_0: current timestep
contract: contract that includes the dynamics of all robots and conflict constraints

Returns:
List of conflict-free, possible next positions for both robots.
"""
x_A_0 = Var("x_A_0")
y_A_0 = Var("y_A_0")
x_B_0 = Var("x_B_0")
y_B_0 = Var("y_B_0")
current_distance = Var("current_distance")
t_0 = Var("t_0")
t_1 = Var("t_1")
x_A_1 = Var("x_A_1")
y_A_1 = Var("y_A_1")
x_B_1 = Var("x_B_1")
y_B_1 = Var("y_B_1")
delta_x = Var("delta_x")
delta_y = Var("delta_y")

X_A_0 = robot1.pos.x
Y_A_0 = robot1.pos.y
X_B_0 = robot2.pos.x
Y_B_0 = robot2.pos.y
cur_dist = np.abs(X_A_0 - X_B_0) + np.abs(Y_A_0 - Y_B_0)

T_1 = T_0 + 1

# find possible next position [(x,y),(x,y)] options for robots
possible_moves = []

for x_a in list({max(X_A_0 - 1, 0), X_A_0, min(X_A_0 + 1, grid_n - 1)}):
for y_a in list({max(Y_A_0 - 1, 0), Y_A_0, min(Y_A_0 + 1, grid_m - 1)}):
for x_b in list({max(X_B_0 - 1, 0), X_B_0, min(X_B_0 + 1, grid_n - 1)}):
for y_b in list({max(Y_B_0 - 1, 0), Y_B_0, min(Y_B_0 + 1, grid_m - 1)}):

del_x = (x_a - x_b) * (X_A_0 - X_B_0)
del_y = (y_a - y_b) * (Y_A_0 - Y_B_0)

var_dict = {
x_A_0: X_A_0,
y_A_0: Y_A_0,
x_B_0: X_B_0,
y_B_0: Y_B_0,
current_distance: cur_dist,
t_0: T_0,
t_1: T_1,
x_A_1: x_a,
y_A_1: y_a,
x_B_1: x_b,
y_B_1: y_b,
delta_x: del_x,
delta_y: del_y,
}

if contract.a.contains_behavior(var_dict) and contract.g.contains_behavior(var_dict):

possible_moves.append([(x_a, y_a), (x_b, y_b)])  # noqa: WPS220

return possible_moves

t_0 = 0
move_candidates = find_move_candidates(r1, r2, t_0, c_dynamics_no_conflicts)

# now pick a move and run it again for the next timestep!
move = strategy(move_candidates, (r1.goal, r2.goal))
print("These are the possible positions for the next timestep: {0}".format(move_candidates))

print("The chosen move is {0}".format(move))

These are the possible positions for the next timestep: [[(0, 0), (0, 1)], [(0, 0), (0, 2)], [(0, 0), (1, 2)], [(0, 1), (0, 2)], [(0, 1), (1, 2)], [(1, 0), (0, 1)], [(1, 0), (0, 2)], [(1, 0), (1, 2)]]
The chosen move is [(0, 1), (1, 2)]


Now we can put everything together and run the simulation of our two robots navigating the grid together.

Note: If you do not have ffmpeg install, you will need to install it to create the animation (https://www.ffmpeg.org).

r1 = Robot("A", (0, 0), (5, 2))
r2 = Robot("B", (0, 2), (5, 0))
trace: List[Snapshot] = []
robots = [r1, r2]
num_iter = 15

t_0 = 0
trace = save_trace(
trace, [(r1.pos.x, r1.pos.y), (r2.pos.x, r2.pos.y)], [(r1.goal.x, r1.goal.y), (r2.goal.x, r2.goal.y)]
)

# Create the contracts ensuring no conflicts
c_collision = collision_contract()
c_swapping = swapping_contract()
c_no_conflicts = c_collision.merge(c_swapping)

for _i in range(0, num_iter):  # noqa: WPS122
# set up the dynamics contracts
c_dyn1 = get_dynamics_contract_robot(r1.name, r1.pos, t_0)
c_dyn2 = get_dynamics_contract_robot(r2.name, r2.pos, t_0)
merged_dyn_contract = c_dyn1.merge(c_dyn2)

# merge the dynamics contract with the conflict contracts
c_dynamics_no_conflicts = merged_dyn_contract.merge(c_no_conflicts)

move_candidates = find_move_candidates(r1, r2, t_0, c_dynamics_no_conflicts)

move = strategy(move_candidates, (r1.goal, r2.goal))
r1.move(move)
r2.move(move)

trace = save_trace(
trace, [(r1.pos.x, r1.pos.y), (r2.pos.x, r2.pos.y)], [(r1.goal.x, r1.goal.y), (r2.goal.x, r2.goal.y)]
)

if distance([[r1.pos.x, r1.pos.y], [r2.pos.x, r2.pos.y]], [r1.goal, r2.goal]) == 0:
break

animate(trace, grid_n, grid_m, "2_robots")

Rendering 8 frames...


This is how to set up a multi-agent navigation problem in Pacti with a horizon of a single timestep, if desired this horizon can be extended by composing multiple timesteps to ensure that the robots will not get stuck depending on the obstacle placement and the actions that the robots are taking. For this case study we will continue using a horizon length of one but we will increase the number of robots in our grid world.

### Extension to three robots¶

# set grid world dimensions n x m
grid_n = 5
grid_m = 5

# Intitialize the robots
r1 = Robot("A", (0, 0), (4, 4))
r2 = Robot("B", (0, 2), (4, 2))
r3 = Robot("C", (0, 4), (4, 0))
robots = [r1, r2, r3]
num_iter = 30

# Get the collision contracts
c_collision = get_collision_contract(robots)
c_swapping = get_swapping_contract(robots)

c_no_conflicts = c_collision.merge(c_swapping)

t_0 = 0
trace = []
trace = save_trace(
trace,
[(r1.pos.x, r1.pos.y), (r2.pos.x, r2.pos.y), (r3.pos.x, r3.pos.y)],
[(r1.goal.x, r1.goal.y), (r2.goal.x, r2.goal.y), (r3.goal.x, r3.goal.y)],
)
last_pos = None

for _j in range(0, num_iter):  # noqa: WPS122
# merge the dynamics contracts
c_dyn1 = get_dynamics_contract_robot(r1.name, r1.pos, t_0)
c_dyn2 = get_dynamics_contract_robot(r2.name, r2.pos, t_0)
c_dyn3 = get_dynamics_contract_robot(r3.name, r3.pos, t_0)

merged_dyn_contract_1_2 = c_dyn1.merge(c_dyn2)  # noqa: WPS114
merged_dyn_contract = merged_dyn_contract_1_2.merge(c_dyn3)

# merge dynamics with the no conflict contracts
multiagent_contract = merged_dyn_contract.merge(c_no_conflicts)

moves_list, t_1 = find_move_candidates_three(grid_n, grid_m, robots, t_0, multiagent_contract)

goals = (r1.goal, r2.goal, r3.goal)
cur_pos = (r1.pos.x, r1.pos.y), (r2.pos.x, r2.pos.y), (r3.pos.x, r3.pos.y)
cur_dist = distance(cur_pos, goals)

move = strategy_multiple(moves_list, goals, cur_pos, last_pos)
last_pos = cur_pos

r1.move(move)
r2.move(move)
r3.move(move)

trace = save_trace(
trace,
[(r1.pos.x, r1.pos.y), (r2.pos.x, r2.pos.y), (r3.pos.x, r3.pos.y)],
[(r1.goal.x, r1.goal.y), (r2.goal.x, r2.goal.y), (r3.goal.x, r3.goal.y)],
)

if distance([[r1.pos.x, r1.pos.y], [r2.pos.x, r2.pos.y], [r3.pos.x, r3.pos.y]], [r1.goal, r2.goal, r3.goal]) == 0:
break

animate(trace, grid_n, grid_m, "3_robots")

Rendering 21 frames...


This analysis can be extended by adding obstacles (by adding a contract that ensures no collision with obstacles) and adding more robots to the grid as desired. We showed how to set up our contracts to describe the viewpoint for a single timestep, and how to modularly add contracts describing the dynamics of individual robots and conflicts.

Depending on the obstacle geometry and the number of robots, it may be necessary to consider a longer horizon to reach the goal more efficiently - prevent situations where the robots get stuck. For this we could compose the contracts for multiple timesteps and pick the strategy on the possible action set resulting from this analysis, which we leave to future work.

Dresner and Stone 2008

Dresner, K. and Stone, P., 2008. A multiagent approach to autonomous intersection management. Journal of artificial intelligence research, 31, pp.591-656.

Ma et al. 2017

Ma, H., Yang, J., Cohen, L., Kumar, T.S. and Koenig, S., 2017, September. Feasibility study: Moving non-homogeneous teams in congested video game environments. In Thirteenth Artificial Intelligence and Interactive Digital Entertainment Conference.

Pallottino et al. 2007

Pallottino, L., Scordio, V.G., Bicchi, A. and Frazzoli, E., 2007. Decentralized cooperative policy for conflict resolution in multivehicle systems. IEEE Transactions on Robotics, 23(6), pp.1170-1183.

Silver 2005

Silver, D., 2005. Cooperative pathfinding. In Proceedings of the aaai conference on artificial intelligence and interactive digital entertainment (Vol. 1, No. 1, pp. 117-122).

Stern et al. 2019

Stern, R., Sturtevant, N.R., Felner, A., Koenig, S., Ma, H., Walker, T.T., Li, J., Atzmon, D., Cohen, L., Kumar, T.S. and Barták, R., 2019, July. Multi-agent pathfinding: Definitions, variants, and benchmarks. In Twelfth Annual Symposium on Combinatorial Search.

Wilson et al. 2020

Wilson, S., Glotfelter, P., Wang, L., Mayya, S., Notomista, G., Mote, M. and Egerstedt, M., 2020. The robotarium: Globally impactful opportunities, challenges, and lessons learned in remote-access, distributed control of multirobot systems. IEEE Control Systems Magazine, 40(1), pp.26-44.

Wurman et al. 2008

Wurman, P.R., D'Andrea, R. and Mountz, M., 2008. Coordinating hundreds of cooperative, autonomous vehicles in warehouses. AI magazine, 29(1), pp.9-9.