H_STEP = .05
R = .3
INIT_XYZS = np.array([[
R * np.cos((i / 6) * 2 * np.pi + np.pi / 2),
R * np.sin((i / 6) * 2 * np.pi + np.pi / 2) - R, H + i * H_STEP
] for i in range(ARGS.num_drones)])
AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz /
ARGS.control_freq_hz) if ARGS.aggregate else 1
#### Create the environment with or without video capture part of each drone's state ###############
if ARGS.vision:
env = VisionCtrlAviary(drone_model=ARGS.drone,
num_drones=ARGS.num_drones,
initial_xyzs=INIT_XYZS,
physics=ARGS.physics,
neighbourhood_radius=10,
freq=ARGS.simulation_freq_hz,
aggregate_phy_steps=AGGR_PHY_STEPS,
gui=ARGS.gui,
record=ARGS.record_video,
obstacles=ARGS.obstacles)
else:
env = CtrlAviary(drone_model=ARGS.drone,
num_drones=ARGS.num_drones,
initial_xyzs=INIT_XYZS,
physics=ARGS.physics,
neighbourhood_radius=10,
freq=ARGS.simulation_freq_hz,
aggregate_phy_steps=AGGR_PHY_STEPS,
gui=ARGS.gui,
record=ARGS.record_video,
obstacles=ARGS.obstacles,
DURATION_SEC = 15
CONTROL_FREQ_HZ = 48
AGGR_PHY_STEPS = int(SIMULATION_FREQ_HZ /
CONTROL_FREQ_HZ) if AGGREGATE else 1
#### Initialize the simulation #####################################################################
H = .1
H_STEP = .05
R = .3
INIT_XYZS = np.array([[
R * np.cos((i / 6) * 2 * np.pi + np.pi / 2),
R * np.sin((i / 6) * 2 * np.pi + np.pi / 2) - R, H + i * H_STEP
] for i in range(NUM_DRONES)])
if VISION:
env = VisionCtrlAviary(drone_model=DRONE, num_drones=NUM_DRONES, initial_xyzs=INIT_XYZS, physics=PHYSICS, visibility_radius=10, \
freq=SIMULATION_FREQ_HZ, aggregate_phy_steps=AGGR_PHY_STEPS, gui=GUI, record=False, obstacles=True)
else:
env = CtrlAviary(drone_model=DRONE, num_drones=NUM_DRONES, initial_xyzs=INIT_XYZS, physics=PHYSICS, visibility_radius=10, \
freq=SIMULATION_FREQ_HZ, aggregate_phy_steps=AGGR_PHY_STEPS, gui=GUI, record=False, obstacles=True)
#### Initialize a circular trajectory ##############################################################
PERIOD = 10
NUM_WP = int(CONTROL_FREQ_HZ) * PERIOD
TARGET_POS = np.zeros((NUM_WP, 3))
for i in range(NUM_WP):
TARGET_POS[i, :] = R * np.cos(
(i / NUM_WP) *
(2 * np.pi) + np.pi / 2) + INIT_XYZS[0, 0], R * np.sin(
(i / NUM_WP) *
(2 * np.pi) + np.pi / 2) - R + INIT_XYZS[0,
1], INIT_XYZS[0,
NUM_DRONES = 10
GUI = True
PHYSICS = Physics.PYB
VISION = False
RECORD_VIDEO = False
SIMULATION_FREQ_HZ = 240
CONTROL_FREQ_HZ = 48
DURATION_SEC = 10
if __name__ == "__main__":
#### Initialize the simulation #####################################################################
H = .1; H_STEP = .05; R = .3; INIT_XYZS = np.array([ [R*np.cos((i/6)*2*np.pi+np.pi/2), R*np.sin((i/6)*2*np.pi+np.pi/2)-R, H+i*H_STEP] for i in range(NUM_DRONES) ])
#### Create the environment with or without video capture part of each drone's state ###############
if VISION: env = VisionCtrlAviary(drone_model=DRONE, num_drones=NUM_DRONES, initial_xyzs=INIT_XYZS, physics=PHYSICS, \
visibility_radius=10, freq=SIMULATION_FREQ_HZ, gui=GUI, record=RECORD_VIDEO, obstacles=True)
else: env = CtrlAviary(drone_model=DRONE, num_drones=NUM_DRONES, initial_xyzs=INIT_XYZS, physics=PHYSICS, \
visibility_radius=10, freq=SIMULATION_FREQ_HZ, gui=GUI, record=RECORD_VIDEO, obstacles=True)
#### Initialize a circular trajectory ##############################################################
PERIOD = 10; NUM_WP = CONTROL_FREQ_HZ*PERIOD; TARGET_POS = np.zeros((NUM_WP,3))
for i in range(NUM_WP): TARGET_POS[i,:] = R*np.cos((i/NUM_WP)*(2*np.pi)+np.pi/2)+INIT_XYZS[0,0], R*np.sin((i/NUM_WP)*(2*np.pi)+np.pi/2)-R+INIT_XYZS[0,1], INIT_XYZS[0,2]
wp_counters = np.array([ int((i*NUM_WP/6)%NUM_WP) for i in range(NUM_DRONES) ])
#### Initialize the logger #########################################################################
logger = Logger(logging_freq_hz=SIMULATION_FREQ_HZ, num_drones=NUM_DRONES, duration_sec=DURATION_SEC)
#### Initialize the controllers ####################################################################
ctrl = [DSLPIDControl(env) for i in range(NUM_DRONES)]
# ctrl = [SimplePIDControl(env) for i in range(NUM_DRONES)]
parser.add_argument('--part', default=1, type=int, help='Which of the 2 blocks in the _dev script to execute (default: 1)', metavar='')
ARGS = parser.parse_args()
####################################################################################################
#### Part 1 of 2 of _dev.py: control with CtrlAviary and printout of MARLFlockAviary's RL functions
####################################################################################################
if ARGS.part==1:
AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz/ARGS.control_freq_hz) if ARGS.aggregate else 1
#### Initialize the simulation #####################################################################
H = .1; H_STEP = .05; R = .3; INIT_XYZS = np.array([ [R*np.cos((i/6)*2*np.pi+np.pi/2), R*np.sin((i/6)*2*np.pi+np.pi/2)-R, H+i*H_STEP] for i in range(ARGS.num_drones) ])
if ARGS.vision:
env = VisionCtrlAviary(drone_model=ARGS.drone, num_drones=ARGS.num_drones, initial_xyzs=INIT_XYZS, physics=ARGS.physics, neighbourhood_radius=10,
freq=ARGS.simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, gui=ARGS.gui, record=False, obstacles=True)
elif ARGS.dyn_ctrl:
env = DynCtrlAviary(drone_model=DroneModel.CF2X, num_drones=ARGS.num_drones, initial_xyzs=INIT_XYZS, physics=ARGS.physics, neighbourhood_radius=10,
freq=ARGS.simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, gui=ARGS.gui, record=False, obstacles=True)
else:
env = CtrlAviary(drone_model=ARGS.drone, num_drones=ARGS.num_drones, initial_xyzs=INIT_XYZS, physics=ARGS.physics, neighbourhood_radius=10,
freq=ARGS.simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, gui=ARGS.gui, record=False, obstacles=True)
#### Initialize a circular trajectory ##############################################################
PERIOD = 10; NUM_WP = int(ARGS.control_freq_hz)*PERIOD; TARGET_POS = np.zeros((NUM_WP,3))
for i in range(NUM_WP): TARGET_POS[i,:] = R*np.cos((i/NUM_WP)*(2*np.pi)+np.pi/2)+INIT_XYZS[0,0], R*np.sin((i/NUM_WP)*(2*np.pi)+np.pi/2)-R+INIT_XYZS[0,1], INIT_XYZS[0,2]
wp_counters = np.array([ int((i*NUM_WP/6)%NUM_WP) for i in range(ARGS.num_drones) ])
#### Initialize the logger #########################################################################
if ARGS.log: logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz/AGGR_PHY_STEPS), num_drones=ARGS.num_drones)