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