state=obs[str(j)]["state"], control=np.hstack([ TARGET_POS[wp_counters[j], 0:2], H + j * H_STEP, np.zeros(9) ])) #### Printout ###################################################################################### if i % env.SIM_FREQ == 0: env.render() #### Print the matrices with the images captured by each drone ##################################### if ARGS.vision: for j in range(ARGS.num_drones): print(obs[str(j)]["rgb"].shape, np.average(obs[str(j)]["rgb"]), obs[str(j)]["dep"].shape, np.average(obs[str(j)]["dep"]), obs[str(j)]["seg"].shape, np.average(obs[str(j)]["seg"])) #### Sync the simulation ########################################################################### if ARGS.gui: sync(i, START, env.TIMESTEP) #### Close the environment ######################################################################### env.close() #### Save the simulation results ################################################################### logger.save() #### Plot the simulation results ################################################################### if ARGS.plot: logger.plot()
LOGGER.log(drone=1, timestamp=i / ENV.SIM_FREQ, state=STATE) #### Compute control for drone 2 ########################### STATE = OBS["2"]["state"] ACTION["2"] = CTRL_2.compute_control( current_position=STATE[0:3], current_velocity=STATE[10:13], current_rpy=STATE[7:10], target_position=TARGET_POSITION[i, :] + np.array([.3, .0, .0]), target_velocity=TARGET_VELOCITY[i, :], target_acceleration=TARGET_ACCELERATION[i, :]) #### Log drone 2 ########################################### LOGGER.log(drone=2, timestamp=i / ENV.SIM_FREQ, state=STATE) #### Printout ############################################## if i % ENV.SIM_FREQ == 0: ENV.render() #### Sync the simulation ################################### if GUI: sync(i, START, ENV.TIMESTEP) #### Close the ENVironment ################################# ENV.close() #### Save the simulation results ########################### LOGGER.save() #### Plot the simulation results ########################### LOGGER.plot()