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)