def main():
N = 2
ctrl = Grid()
ob_next = {14: [14, 10], 10: [6, 10, 14], 6: [2, 6, 10], 2: [2, 6]}
dests_0, dests_1 = ([], [])
# Instantiate Robotarium object
r = robotarium.Robotarium(number_of_agents=N,
show_figure=True,
save_data=True,
update_time=1)
# This consensus algorithm uses single-integrator dynamics, so we'll need these mappings.
init = False
init_dest_0 = d2c(0)
init_dest_1 = d2c(14)
si_barrier_cert = create_single_integrator_barrier_certificate(N)
dest_pose = np.zeros((3, N))
for k in range(100000):
# Initialize the single-integrator control inputs
si_velocities = np.zeros((2, N))
x = r.get_poses()
x_si = x[:2, :]
if not init:
dest_pose[:, 0] = init_dest_0
dest_pose[:, 1] = init_dest_1
while (np.size(at_pose(x, dest_pose, rotation_error=100)) != N):
si_velocities = single_integrator_position_controller(
x_si, dest_pose[:2, :], magnitude_limit=0.08)
# Set the velocities of agents 1,...,N
si_velocities = si_barrier_cert(si_velocities, x_si)
r.set_velocities(
np.arange(N),
transformations.single_integrator_to_unicycle2(
si_velocities, x))
# Iterate the simulation
r.step()
init = True
else:
x = r.get_poses()
x1 = x[:2, 1]
dest_pose[:, 0] = d2c(ctrl.move(c2d(x1)))
dest_pose[:, 1] = d2c(random.choice(ob_next[c2d(x1)]))
while (np.size(at_pose(x, dest_pose, rotation_error=100)) != N):
si_velocities = single_integrator_position_controller(
x_si, dest_pose[:2, :], magnitude_limit=0.08)
# Set the velocities of agents 1,...,N
# si_velocities = si_barrier_cert(si_velocities, x_si)
r.set_velocities(
np.arange(N),
transformations.single_integrator_to_unicycle2(
si_velocities, x))
# Iterate the simulation
r.step()
# Always call this function at the end of your script!!!!
r.call_at_scripts_end()
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()
# Always call this at the end of your scripts!! It will acccelerate your execution time.
r.call_at_scripts_end()