test_env.AGGR_PHY_STEPS),
num_drones=1)
obs = test_env.reset()
start = time.time()
for i in range(
6 * int(test_env.SIM_FREQ / test_env.AGGR_PHY_STEPS)): # Up to 6''
action, _states = model.predict(
obs,
deterministic=True # OPTIONAL 'deterministic=False'
)
obs, reward, done, info = test_env.step(action)
test_env.render()
if OBS == ObservationType.KIN:
logger.log(drone=0,
timestamp=i / test_env.SIM_FREQ,
state=np.hstack(
[obs[0:3], obs[3:15],
np.resize(action, (4))]),
control=np.zeros(12))
sync(np.floor(i * test_env.AGGR_PHY_STEPS), start, test_env.TIMESTEP)
# if done: obs = test_env.reset() # OPTIONAL EPISODE HALT
test_env.close()
logger.save_as_csv("sa") # Optional CSV save
logger.plot()
# with np.load(ARGS.exp+'/evaluations.npz') as data:
# print(data.files)
# print(data['timesteps'])
# print(data['results'])
# print(data['ep_lengths'])
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()
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()
#### Log the trace #########################################
logger.log(drone=0,
timestamp=TRACE_TIMESTAMPS[i],
state=trace_obs,
control=np.hstack([TRACE_CTRL_REFERENCE[i, :], np.zeros(6)])
)
#### Log the simulation ####################################
logger.log(drone=1,
timestamp=i/env.SIM_FREQ,
state=obs["0"]["state"],
control=np.hstack([TRACE_CTRL_REFERENCE[i, :], np.zeros(6)])
)
#### 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()
#### Plot the simulation results ###########################
logger.plot(pwm=True)