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()