r = robotarium.Robotarium(number_of_agents=N,
show_figure=True,
save_data=True,
update_time=1)
si_barrier_cert = create_single_integrator_barrier_certificate(N)
for k in range(iterations):
# Get the poses of the robots
x = r.get_poses()
# Initialize a velocity vector
dxi = np.zeros((2, N))
for i in range(N):
for j in graph.topological_neighbors(L, i):
# Perform a weighted consensus to make the rectangular shape
error = x[:2, j] - x[:2, i]
dxi[:, i] += formation_control_gain * (np.power(
np.linalg.norm(error), 2) - np.power(weights[i, j], 2)) * error
# Make sure that the robots don't collide
dxi = si_barrier_cert(dxi, x[:2, :])
# Transform the single-integrator dynamcis to unicycle dynamics
dxu = transformations.single_integrator_to_unicycle2(dxi, x)
# Set the velocities of the robots
r.set_velocities(np.arange(N), dxu)
# Iterate the simulation
r.step()
# Create a single-integrator barrier certificate to avoid collisions.
si_barrier_cert = create_single_integrator_barrier_certificate(N)
for k in range(1000):
# Get the poses of the robots and convert to single-integrator poses
x = r.get_poses()
x_si = uni_to_si_states(x)
# Initialize the single-integrator control inputs
si_velocities = np.zeros((2, N))
# For each robot...
for i in range(N):
# Get the neighbors of robot 'i' (encoded in the graph Laplacian)
j = graph.topological_neighbors(L, i)
# Compute the consensus algorithm
si_velocities[:, i] = np.sum(x_si[:, j] - x_si[:, i, None], 1)
# Use the barrier certificate to avoid collisions
si_velocities = si_barrier_cert(si_velocities, x_si)
# Set the velocities of agents 1,...,N
r.set_velocities(np.arange(N), si_to_uni_dyn(si_velocities, x))
# Iterate the simulation
r.step()
# Always call this function at the end of your script!!!!
r.call_at_scripts_end()