#### Go to the next way point and loop #############################################################
for j in range(NUM_DRONES):
wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (
NUM_WP - 1) else 0
#### Log the simulation ############################################################################
for j in range(NUM_DRONES):
logger.log(drone=j,
timestamp=i / env.SIM_FREQ,
state=obs[str(j)]["state"],
control=np.hstack([
TARGET_POS[wp_counters[j], :], INIT_XYZS[j, 2],
np.zeros(9)
]))
#### 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()
target_pos=TARGET_POS[wp_counter, :],
)
#### 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()