)
#### Go to the next way point and loop #####################
wp_counter = wp_counter + 1 if wp_counter < (NUM_WP - 1) else 0
#### Log the simulation ####################################
logger.log(drone=0,
timestamp=i / env.SIM_FREQ,
state=obs["0"]["state"],
control=np.hstack([TARGET_POS[wp_counter, :],
np.zeros(9)]))
#### Printout ##############################################
if i % env.SIM_FREQ == 0:
env.render()
#### Sync the simulation ###################################
if ARGS.gui:
sync(i, START, env.TIMESTEP)
#### Close the environment #################################
env.close()
#### Save the simulation results ###########################
logger.save()
logger.save_as_csv("gnd") # Optional CSV 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()