var = 3 # control exploration
if __name__ == '__main__':
control_gain = 20
# Experiment constants
iterations = 1000
N = 3
#Limit maximum linear speed of any robot
magnitude_limit = 0.4
#initialization
r = robotarium.Robotarium(number_of_robots=N,
show_figure=False,
sim_in_real_time=True)
si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary(
)
si_to_uni_dyn = create_si_to_uni_dynamics()
x = r.get_poses()
d1, a1, d2, a2, d3, a3, d12, a12, d13, a13, d21, a21, d23, a23, d31, a31, d32, a32 = get_d(
x)
dxi = np.zeros((2, N))
o_n1 = np.zeros(6)
o_n2 = np.zeros(6)
o_n3 = np.zeros(6)
sess = tf.Session()
sess.run(tf.global_variables_initializer())
saver = tf.train.Saver()
import rps.robotarium as robotarium
from rps.utilities.transformations import *
from rps.utilities.barrier_certificates import *
from rps.utilities.misc import *
from rps.utilities.controllers import *
import numpy as np
import time
# Instantiate Robotarium object
N = 5
initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0'))
r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=False)
# Define goal points by removing orientation from poses
goal_points = generate_initial_conditions(N, width=r.boundaries[2]-2*r.robot_diameter, height = r.boundaries[3]-2*r.robot_diameter, spacing=0.5)
# Create single integrator position controller
single_integrator_position_controller = create_si_position_controller()
# Create barrier certificates to avoid collision
#si_barrier_cert = create_single_integrator_barrier_certificate()
si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary()
_, uni_to_si_states = create_si_to_uni_mapping()
# Create mapping from single integrator velocity commands to unicycle velocity commands
si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion()
# define x initially
import rps.robotarium as robotarium
import rps.utilities.graph as graph
import rps.utilities.transformations as transformations
from rps.utilities.barrier_certificates import *
from rps.utilities.controllers import *
import numpy as np
# How many robots we want to use in the simulation
N = 10
# Instantiate the Robotarium object with these parameters
r = robotarium.Robotarium(number_of_agents=N,
show_figure=True,
save_data=True,
update_time=0.1)
# How many iterations do we want (about 165 seconds)
iterations = 5000
# Retieve a cyclic graph Laplacian from the utilities
L = graph.cycle_GL(N)
# We're working in single-integrator dynamics, and we don't want the robots
# to collide. Thus, we're going to use barrier certificates
si_barrier_cert = create_single_integrator_barrier_certificate(N)
# This portion of the code generates points on a circle enscribed in a 6x6 square
# that's centered on the origin. The robots switch positions on the circle.
radius = 1
xybound = radius * np.array([-1, 1, -1, 1])
p_theta = 2 * np.pi * (np.arange(0, 2 * N, 2) / (2 * N))
p_circ = np.vstack([
def __init__(self,
num_robots=12,
store_state_history=True,
max_iterations=1000,
use_barriers=True):
# Instantiate Robotarium object
self.num_robots = num_robots
assert self.num_robots == 12 or self.num_robots == 4 or self.num_robots == 2
initial_conditions = np.array([0.])
if self.num_robots == 12:
initial_conditions = np.array([[
-1.3, -1.3, -1.3, -1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 1.3, 1.3,
1.3
], [
0.5, 0.0, -0.5, 0.5, 0.0, -0.5, 0.5, 0.0, -0.5, 0.5, 0.0, -0.5
],
[
0., 0., 0., 0., 0., 0., np.pi,
np.pi, np.pi, np.pi, np.pi,
np.pi
]])
elif self.num_robots == 4:
initial_conditions = np.array([[-1.0, -1.0, 1.0, 1.0],
[0.5, -0.5, 0.5, -0.5],
[0., 0., np.pi, np.pi]])
elif self.num_robots == 2:
initial_conditions = np.array([[-1.0, 1.0], [0.0, 0.0],
[0., np.pi]])
self.robo_inst = robotarium.Robotarium(
number_of_robots=num_robots,
show_figure=False,
initial_conditions=initial_conditions,
sim_in_real_time=False)
# Create barrier certificates to avoid collision
self.uni_barrier_cert = create_unicycle_barrier_certificate()
# Flags, true if present
self.purple_flags = [True, True]
self.purple_flag_locs = [(-1.45, -0.85), (-1.45, 0.85)]
self.orange_flags = [True, True]
self.orange_flag_locs = [(1.45, -0.85), (1.45, 0.85)]
self.purple_home = [-1.5, 0.0, 0.0]
self.orange_home = [1.5, 0.0, np.pi]
self.purple_points = 0
self.orange_points = 0
# Robot tag cooldowns
self.tag_cooldowns = np.zeros(self.num_robots)
self.robot_states = [None] * self.num_robots
self.robot_policies = [None] * self.num_robots
self.store_state_history = store_state_history
self.state_history = list()
# Create barrier certificates to avoid collision
self.uni_barrier_cert = create_unicycle_barrier_certificate()
self.use_barriers = use_barriers
self.max_iterations = max_iterations
self.iterations = 0
self.flag_states = list()
self.flag_states.append(
copy.deepcopy(self.purple_flags + self.orange_flags))
self.score_history = list()
self.score_history.append([0., 0.])
def EXPERIMENT06(priority_cav1, priority_cav2, with_priority):
#Size of the Matrix of states
w, h, N = 8, 5, 20
#Creating States
number_of_states = w * h
states = [None] * number_of_states
#Defining the positions of each State
for i in range(number_of_states):
states[i] = automata.State('S' + str(i))
# Creating events
number_of_events = 4 * w * h + 2 * (w + h - 2)
events = [None] * number_of_events
for i in range(number_of_events):
events[i] = automata.Event(('e' + str(i)), 1, True)
#Creating the automaton itself and its positions
trans = dict()
Matrix_states = [[0 for x in range(w)] for y in range(h)]
#Data about positions
wsize = 3.2
whalf = wsize / 2
hsize = 2
hhalf = hsize / 2
Initial_point = np.array([0, 0, 0])
wdif = wsize / (w)
hdif = hsize / (h)
positions = [[0 for x in range(w)] for y in range(h)]
DIC_POSITIONS = dict()
counter_states = 0
for i in range(h):
for j in range(w):
Matrix_states[i][j] = states[counter_states]
trans[states[counter_states]] = dict()
positions[i][j] = [
x + y
for x, y in zip(Initial_point, [(j - (w - 1) / 2) *
wdif, (-i +
(h - 1) / 2) * hdif, 0])
]
DIC_POSITIONS[states[counter_states]] = positions[i][j]
counter_states += 1
counter_events = 0
for i in range(h):
for j in range(w):
if i < (h - 1):
trans[Matrix_states[i][j]][
events[counter_events]] = Matrix_states[i + 1][j]
counter_events += 1
if i > (0):
trans[Matrix_states[i][j]][
events[counter_events]] = Matrix_states[i - 1][j]
counter_events += 1
if j < (w - 1):
trans[Matrix_states[i][j]][
events[counter_events]] = Matrix_states[i][j + 1]
counter_events += 1
if j > (0):
trans[Matrix_states[i][j]][
events[counter_events]] = Matrix_states[i][j - 1]
counter_events += 1
G = automata.Automaton(trans, events[0])
#Creating inputs for robotarium
RADIUS = 0.06
# Experiment 3 - Compound movement
Initial_pos = (Matrix_states[0][:] + Matrix_states[4][:] +
[Matrix_states[2][0]] + [Matrix_states[2][3]] +
[Matrix_states[2][4]] + [Matrix_states[2][7]])
Final_pos = (Matrix_states[4][:] + Matrix_states[0][:] +
[Matrix_states[2][3]] + [Matrix_states[2][0]] +
[Matrix_states[2][7]] + [Matrix_states[2][4]])
real_state = Initial_pos
pivot_state = [[]] * N
past_state = [[]] * N
past_state2 = [[]] * N
#Path planning variables
N = len(Final_pos)
T = [None] * N
S = [None] * N
T_optimal = [None] * N
S_optimal = [None] * N
T_dj = [None] * N
S_dj = [None] * N
logical_state = [None] * N
priority_radius = [2] * N
if with_priority:
priority_radius[priority_cav1] = priority_radius[priority_cav1] + 1
priority_radius[priority_cav2] = priority_radius[priority_cav2] + 1
buffer = [0] * N
communication_radius = [5] * N
blacklist = dict()
blacklist_individual = dict()
calculating = [True] * N
defined_path = dict()
calculating = [True] * N
for i in range(N):
blacklist[i] = []
defined_path[i] = []
#Control variables
possible = rc.FC_POSSIBLE_STATES_ARRAY(DIC_POSITIONS)
goal_points = np.ones([3, N])
# Initializing the states list
initial_points = rc.FC_SET_ALL_POSITIONS(DIC_POSITIONS, Initial_pos)
# Initializing the robotarium
r = robotarium.Robotarium(number_of_robots=N,
show_figure=False,
initial_conditions=initial_points,
sim_in_real_time=False)
single_integrator_position_controller = create_si_position_controller()
__, uni_to_si_states = create_si_to_uni_mapping()
si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion()
x = r.get_poses()
x_si = uni_to_si_states(x) #"""
r.step()
RUN = True
first = [True] * N
unfinished = True
unfinished2 = True
# Creating an structure of past states during actual order
past = dict()
for s in range(N):
past[s] = []
string_size = list()
while real_state != Final_pos:
x = r.get_poses()
x_si = uni_to_si_states(x)
# Update Real State
for i in range(N):
blacklist[i] = []
past_state[i] = real_state[i]
real_state[i] = rc.FC_MAKE_REAL_TRANSITION(possible, states,
real_state[i], x, i,
RADIUS)
if past_state[i] != real_state[i]:
past_state2[i] = past_state[i]
for i in range(N):
if real_state[i] != Final_pos[i]:
if real_state[i] == pivot_state[i]:
#Recalculus of route is necessary
calculating[i] = True
elif real_state[i] == logical_state[i]:
#Update Robotarium state orders, no recalculus is needed
defined_path[i].pop(0)
logical_state[i] = defined_path[i][1]
if calculating[i]:
for j in range(N):
if j != i:
d_real = dk.DIST(G, real_state[j],
communication_radius[j])
if real_state[i] in d_real:
#Update blacklist[i]
if S[j] != None and len(defined_path[j]) > 1:
start_black = True
for k in range(len(defined_path[j])):
if start_black:
blacklist[
i] = rc.add_black_real_logical(
G, blacklist[i],
defined_path[j][k],
defined_path[j][k + 1])
start_black = False
else:
blacklist[i] = rc.add_black3(
G, blacklist[i],
defined_path[j][k])
start_black = False
else:
blacklist[i] = rc.add_black3(
G, blacklist[i], real_state[j])
#Update Path[i]
(T_optimal[i],
S_optimal[i]) = dk.PATH2(G, [], real_state[i],
Final_pos[i])
try:
(T[i], S[i]) = dk.PATH2(G, blacklist[i], real_state[i],
Final_pos[i])
if len(S[i]) > priority_radius[i]:
index = list(range(priority_radius[i]))
else:
index = list(range(len(S[i])))
defined_path[i] = list()
for j in index:
defined_path[i].append(S[i][j])
if len(S[i]) > len(S_optimal[i]):
if len(defined_path[i]) > 2:
for j in reversed(
range(2, len(defined_path[i]))):
if defined_path[i][j] not in S_optimal[i]:
defined_path[i].pop(j)
pivot_state[i] = defined_path[i][-1]
logical_state[i] = S[i][1]
if logical_state[i] == past_state2[i]:
try:
blacklist[i] = rc.add_black3(
G, blacklist[i], past_state2[i])
(T[i],
S[i]) = dk.PATH2(G, blacklist[i],
real_state[i], Final_pos[i])
if len(S[i]) > priority_radius[i]:
index = list(range(priority_radius[i]))
else:
index = list(range(len(S[i])))
defined_path[i] = list()
for j in index:
defined_path[i].append(S[i][j])
pivot_state[i] = defined_path[i][-1]
logical_state[i] = S[i][1]
except:
pass
except:
blocked = rc.check_block(G.transitions, real_state[i],
blacklist[i])
if blocked:
S[i] = [real_state[i]]
T[i] = [None]
defined_path[i] = [S[i][0]]
pivot_state[i] = S[i][0]
logical_state[i] = S[i][0]
else:
white_auto = G.transitions[real_state[i]]
white_keys = white_auto.keys()
white_list = list()
for j in white_keys:
# print(j)
if j not in blacklist[i]:
if white_auto[j] != past_state2[i]:
white_list.append(j)
else:
past_event = j
if len(white_list) >= 1:
white_event = random.choice(white_list)
elif past_event not in blacklist[i]:
white_event = past_event
S[i] = [real_state[i], white_auto[white_event]]
T[i] = [white_event]
defined_path[i] = [
real_state[i], white_auto[white_event]
]
pivot_state[i] = S[i][1]
logical_state[i] = S[i][1]
calculating[i] = False
else:
#Reached final position
# Reached final position
logical_state[i] = real_state[i]
if unfinished and i == priority_cav1:
time_of_execution_with_priority = r._iterations * 0.033
unfinished = False
if unfinished2 and i == priority_cav2:
time_of_execution_with_priority2 = r._iterations * 0.033
unfinished2 = False
for j in range(N):
if logical_state[j] != None:
rc.FC_SET_GOAL_POINTS(DIC_POSITIONS, goal_points, j,
logical_state[j])
else:
rc.FC_SET_GOAL_POINTS(DIC_POSITIONS, goal_points, j,
real_state[j])
dxi = single_integrator_position_controller(x_si, goal_points[:2][:])
dxu = si_to_uni_dyn(dxi, x)
r.set_velocities(np.arange(N), dxu)
r.step()
r.call_at_scripts_end()
time_of_execution = r._iterations * 0.033
return (time_of_execution, time_of_execution_with_priority,
time_of_execution_with_priority2)
def main():
with open(ARGS.config) as f:
model_params = json.load(f)
model_params['pred_steps'] = ARGS.pred_steps
seg_len = 2 * len(model_params['cnn']['filters']) + 1
cnn_multistep_regressor = tf.estimator.Estimator(model_fn=model_fn,
params=model_params,
model_dir=ARGS.log_dir)
# Prediction
if ARGS.test:
if model_params.get('edge_types', 0) > 1:
test_data, test_edge = load_data(ARGS.data_dir,
ARGS.data_transpose,
edge=True,
prefix='test')
test_edge = gnn.utils.one_hot(test_edge,
model_params['edge_types'],
np.float32)
features = {'time_series': test_data, 'edge_type': test_edge}
else:
test_data = load_data(ARGS.data_dir,
ARGS.data_transpose,
edge=False,
prefix='test')
features = {'time_series': test_data}
if not ARGS.dynamic_update:
curr_time = time.time()
predict_input_fn = input_fn(features, seg_len, ARGS.pred_steps,
ARGS.batch_size, 'test')
prediction = cnn_multistep_regressor.predict(
input_fn=predict_input_fn)
prediction = np.array([pred['next_steps'] for pred in prediction])
prediction = np.swapaxes(prediction, 0, 1)
print('GNN execution time = %f' % (-curr_time + time.time()))
#print(prediction.shape)
print("===========================================================")
print("===========================================================")
# Instantiate Robotarium object
N = test_data.shape[2]
r = robotarium.Robotarium(number_of_agents=N,
show_figure=False,
save_data=False,
update_time=0.3)
# Create barrier certificates to avoid collision
si_barrier_cert = create_single_integrator_barrier_certificate(N)
# ------------------------- initalizing initial positions ------------------------
# initialize the the agents according to the simulation
initial_goal_points = np.squeeze(
np.swapaxes(test_data, 2, 3)[:, 0, :2, :])
initial_orientations = np.zeros((1, 20))
initial_goal_states = np.concatenate(
[initial_goal_points, initial_orientations], axis=0)
move_to_goal(r, N, si_barrier_cert, initial_goal_states)
r.call_at_scripts_end()
time.sleep(0.03)
print('Step 1')
print('error = %f' %
(np.linalg.norm(r.get_poses()[:2, :] - initial_goal_points)))
current_position = r.get_poses()[:2, :]
current_velocities = unicycle_to_single_integrator(
r.velocities, r.get_poses())
current_pos_vel = np.concatenate(
[current_position, current_velocities], axis=0)
pos_vel_log = np.swapaxes(
np.expand_dims(np.expand_dims(current_pos_vel, axis=0), axis=0), 2,
3)
for i in range(1, 8):
print('Step %d' % (i + 1))
goal_points = np.squeeze(np.swapaxes(test_data, 2, 3)[:, i, :2, :])
goal_velocities = np.squeeze(
np.swapaxes(test_data, 2, 3)[:, i, 2:, :])
apply_control(r, N, goal_velocities)
current_position = r.get_poses()[:2, :]
current_velocities = unicycle_to_single_integrator(
r.velocities, r.get_poses())
current_pos_vel = np.concatenate(
[current_position, current_velocities], axis=0)
pos_vel_log = np.concatenate([
pos_vel_log,
np.swapaxes(
np.expand_dims(np.expand_dims(current_pos_vel, axis=0),
axis=0), 2, 3)
],
axis=1)
print('error = %f' %
(np.linalg.norm(r.get_poses()[:2, :] - goal_points)))
# Always call this function at the end of your scripts! It will accelerate the
# execution of your experiment
# r.call_at_scripts_end()
#------------- intialization end ----------------------
# -------------------- Prediction from Graph neural network -------------------------
pos_vel_log = pos_vel_log.astype(dtype=np.float32)
for i in range(8, test_data.shape[1]):
if ARGS.dynamic_update:
curr_time = time.time()
features = {'time_series': pos_vel_log[:, i - 8:i, :, :]}
predict_input_fn = input_fn(features, seg_len, ARGS.pred_steps,
ARGS.batch_size, 'test')
prediction = cnn_multistep_regressor.predict(
input_fn=predict_input_fn)
prediction = np.array(
[pred['next_steps'] for pred in prediction])
print(
"------------------------------------------------------------------"
)
print('GNN execution time = %f' % (-curr_time + time.time()))
goal_velocities = np.squeeze(
np.swapaxes(prediction, 2, 3)[:, :, 2:, :])
else:
print(prediction.shape)
goal_velocities = np.squeeze(
np.swapaxes(prediction, 2, 3)[:, i - 8, 2:, :])
print('Step %d' % (i + 1))
goal_points = np.squeeze(np.swapaxes(test_data, 2, 3)[:, i, :2, :])
#goal_velocities = np.squeeze(np.swapaxes(prediction,2,3)[:,:,2:,:])
apply_control(r, N, goal_velocities)
current_position = r.get_poses()[:2, :]
current_velocities = unicycle_to_single_integrator(
r.velocities, r.get_poses())
current_pos_vel = np.concatenate(
[current_position, current_velocities], axis=0)
pos_vel_log = np.concatenate([
pos_vel_log,
np.swapaxes(
np.expand_dims(np.expand_dims(current_pos_vel, axis=0),
axis=0), 2, 3)
],
axis=1)
pos_vel_log = pos_vel_log.astype(dtype=np.float32)
print('error = %f' %
(np.linalg.norm(r.get_poses()[:2, :] - goal_points)))
# Always call this function at the end of your scripts! It will accelerate the
# execution of your experiment
r.call_at_scripts_end()
def __init__(self,
num_agents,
max_vel,
max_acc,
max_ang_vel,
radius,
time_step,
model_params,
log_dir,
show_figure=True,
Kp=1,
Kd=0,
Ki=0):
self._next_positions = np.zeros((2, num_agents))
self.velocities = np.zeros((2, num_agents))
self.orientations = np.zeros((1, num_agents))
self.poses_robotarium = np.zeros((3, num_agents))
self.num_agents = num_agents
self.time_step = time_step
self.max_vel = max_vel
self.max_acc = max_acc
self.max_ang_vel = max_ang_vel
self.Kp = Kp
self.Ki = Ki * time_step
self.Kd = Kd / time_step
for i in range(num_agents):
alpha = i * 2 * np.pi / num_agents # angle from the center
theta = alpha + np.pi / 2.0 # heading angle
if theta > 2 * np.pi:
theta = theta - 2 * np.pi
if theta > np.pi:
theta = theta - 2 * np.pi
x = radius * np.cos(alpha)
y = radius * np.sin(alpha)
self.poses_robotarium[:, i] = np.array([x, y, theta]).transpose()
self.r = robotarium.Robotarium(
number_of_agents=num_agents,
show_figure=show_figure,
save_data=False,
update_time=1) #, update_time= time_step)
self.go_to_point_and_pose(self.poses_robotarium)
self.uni_barrier_cert = create_unicycle_barrier_certificate(
num_agents,
barrier_gain=100,
safety_radius=0.1650,
projection_distance=0.05)
#pos_vel_log
current_position = self.r.get_poses()[:2, :]
current_velocities = np.zeros((2, self.num_agents))
self.r.set_velocities(np.arange(self.num_agents), current_velocities)
self.r.step()
current_pos_vel = np.concatenate(
[current_position, current_velocities], axis=0)
self.pos_vel_log = np.swapaxes(
np.expand_dims(np.expand_dims(current_pos_vel, axis=0), axis=0), 2,
3)
#define GNN
self.seg_len = 2 * len(model_params['cnn']['filters']) + 1
self.cnn_multistep_regressor = tf.estimator.Estimator(
model_fn=model_fn, params=model_params, model_dir=log_dir)
