def __init__(self, drone_model: DroneModel=DroneModel.CF2X, initial_xyzs=None, initial_rpys=None, physics: Physics=Physics.PYB, freq: int=240, aggregate_phy_steps: int=1, gui=False, record=False, obs: ObservationType=ObservationType.KIN, act: ActionType=ActionType.RPM): self.EPISODE_LEN_SEC = 5 self.OBS_TYPE = obs; self.ACT_TYPE = act #### Before super().__init__ to initialize the proper action/obs spaces ############################ if self.OBS_TYPE==ObservationType.RGB: self.IMG_RES = np.array([64, 48]); self.IMG_FRAME_PER_SEC = 24; self.IMG_CAPTURE_FREQ = int(freq/self.IMG_FRAME_PER_SEC) self.rgb = np.zeros(((1, self.IMG_RES[1], self.IMG_RES[0], 4))); self.dep = np.ones(((1, self.IMG_RES[1], self.IMG_RES[0]))); self.seg = np.zeros(((1, self.IMG_RES[1], self.IMG_RES[0]))) if self.IMG_CAPTURE_FREQ%aggregate_phy_steps!=0: print("[ERROR] in BaseSingleAgentAviary.__init__(), aggregate_phy_steps incompatible with the desired video capture frame rate ({:f}Hz)".format(self.IMG_FRAME_PER_SEC)); exit() super().__init__(drone_model=drone_model, num_drones=1, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles=True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui=False) # Remove of RPM sliders from all single agent learning aviaries #### After super().__init__ to use the proper constants ############################################ if self.ACT_TYPE==ActionType.DYN or self.ACT_TYPE==ActionType.ONE_D_DYN: if self.DRONE_MODEL==DroneModel.CF2X: self.A = np.array([ [1, 1, 1, 1], [.5, .5, -.5, -.5], [-.5, .5, .5, -.5], [-1, 1, -1, 1] ]) elif self.DRONE_MODEL in [DroneModel.CF2P, DroneModel.HB]: self.A = np.array([ [1, 1, 1, 1], [0, 1, 0, -1], [-1, 0, 1, 0], [-1, 1, -1, 1] ]) self.INV_A = np.linalg.inv(self.A); self.B_COEFF = np.array([1/self.KF, 1/(self.KF*self.L), 1/(self.KF*self.L), 1/self.KM]) if self.ACT_TYPE==ActionType.PID or self.ACT_TYPE==ActionType.ONE_D_PID: os.environ['KMP_DUPLICATE_LIB_OK']='True' if self.DRONE_MODEL in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = DSLPIDControl(CtrlAviary(drone_model=DroneModel.CF2X)) elif self.DRONE_MODEL==DroneModel.HB: self.ctrl = SimplePIDControl(CtrlAviary(drone_model=DroneModel.HB)) if self.OBS_TYPE==ObservationType.RGB and self.RECORD: self.ONBOARD_IMG_PATH = os.path.dirname(os.path.abspath(__file__))+"/../../../files/videos/onboard-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+"/"; os.makedirs(os.path.dirname(self.ONBOARD_IMG_PATH), exist_ok=True)
def __init__(self, drone_model: DroneModel = DroneModel.CF2X, num_drones: int = 2, neighbourhood_radius: float = np.inf, initial_xyzs=None, initial_rpys=None, physics: Physics = Physics.PYB, freq: int = 240, aggregate_phy_steps: int = 1, gui=False, record=False, obs: ObservationType = ObservationType.KIN, act: ActionType = ActionType.RPM): if num_drones < 2: print( "[ERROR] in BaseMultiagentAviary.__init__(), num_drones should be >= 2" ) exit() vision_attributes = True if obs == ObservationType.RGB else False dynamics_attributes = True if act in [ ActionType.DYN, ActionType.ONE_D_DYN ] else False self.OBS_TYPE = obs self.ACT_TYPE = act self.EPISODE_LEN_SEC = 5 #### Create integrated controllers ######################### if act in [ActionType.PID, ActionType.ONE_D_PID]: os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' if self.DRONE_MODEL in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = [ DSLPIDControl(BaseAviary(drone_model=DroneModel.CF2X)) for i in range(self.NUM_DRONES) ] elif self.DRONE_MODEL == DroneModel.HB: self.ctrl = [ SimplePIDControl(BaseAviary(drone_model=DroneModel.HB)) for i in range(self.NUM_DRONES) ] super().__init__( drone_model=drone_model, num_drones=num_drones, neighbourhood_radius=neighbourhood_radius, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles= True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui= False, # Remove of RPM sliders from all single agent learning aviaries vision_attributes=vision_attributes, dynamics_attributes=dynamics_attributes)
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 ######################################################################### logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz / AGGR_PHY_STEPS), num_drones=ARGS.num_drones) #### Initialize the controllers #################################################################### ctrl = [DSLPIDControl(env) for i in range(ARGS.num_drones)] # ctrl = [SimplePIDControl(env) for i in range(ARGS.num_drones)] #### Run the simulation ############################################################################ CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz)) action = {str(i): np.array([0, 0, 0, 0]) for i in range(ARGS.num_drones)} START = time.time() for i in range(0, int(ARGS.duration_sec * env.SIM_FREQ), AGGR_PHY_STEPS): #### Make it rain rubber ducks ##################################################################### # if i/env.SIM_FREQ>5 and i%10==0 and i/env.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [0+random.gauss(0, 0.3),-0.5+random.gauss(0, 0.3),3], p.getQuaternionFromEuler([random.randint(0,360),random.randint(0,360),random.randint(0,360)]), physicsClientId=PYB_CLIENT) #### Step the simulation ########################################################################### obs, reward, done, info = env.step(action) #### Compute control at the desired frequency @@@@@#################################################
#### 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)] #### Run the simulation ############################################################################ CTRL_EVERY_N_STEPS= int(np.floor(env.SIM_FREQ/CONTROL_FREQ_HZ)) action = { str(i): np.array([0,0,0,0]) for i in range(NUM_DRONES) } START = time.time(); temp_action = {} for i in range(DURATION_SEC*env.SIM_FREQ): #### Step the simulation ########################################################################### obs, reward, done, info = env.step(action) #### Compute control at the desired frequency @@@@@################################################# if i%CTRL_EVERY_N_STEPS==0: #### Compute control for the current way point #####################################################
aggregate_phy_steps=AGGR_PHY_STEPS, gui=ARGS.gui, record=ARGS.record_video, obstacles=ARGS.obstacles, user_debug_gui=ARGS.user_debug_gui) #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() #### Initialize the logger ################################# logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz / AGGR_PHY_STEPS), num_drones=1) #### Initialize the controller ############################# ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) #### Run the simulation #################################### CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz)) action = {"0": np.array([0, 0, 0, 0])} START = time.time() for i in range(0, int(ARGS.duration_sec * env.SIM_FREQ), AGGR_PHY_STEPS): #### Make it rain rubber ducks ############################# # if i/env.SIM_FREQ>5 and i%10==0 and i/env.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [0+random.gauss(0, 0.3),-0.5+random.gauss(0, 0.3),3], p.getQuaternionFromEuler([random.randint(0,360),random.randint(0,360),random.randint(0,360)]), physicsClientId=PYB_CLIENT) #### Step the simulation ################################### obs, reward, done, info = env.step(action) #### Compute control at the desired frequency ############## if i % CTRL_EVERY_N_STEPS == 0:
#### Initialize the trajectories ########################### PERIOD = 5 NUM_WP = ARGS.control_freq_hz * PERIOD TARGET_POS = np.zeros((NUM_WP, 2)) for i in range(NUM_WP): TARGET_POS[i, :] = [0.5 * np.cos(2 * np.pi * (i / NUM_WP)), 0] wp_counters = np.array([0, int(NUM_WP / 2)]) #### Initialize the logger ################################# logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz / AGGR_PHY_STEPS), num_drones=2, duration_sec=ARGS.duration_sec) #### Initialize the controllers ############################ ctrl = [DSLPIDControl(drone_model=ARGS.drone) for i in range(2)] #### Run the simulation #################################### CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz)) action = {str(i): np.array([0, 0, 0, 0]) for i in range(2)} START = time.time() for i in range(0, int(ARGS.duration_sec * env.SIM_FREQ), AGGR_PHY_STEPS): #### Step the simulation ################################### obs, reward, done, info = env.step(action) #### Compute control at the desired frequency ############## if i % CTRL_EVERY_N_STEPS == 0: #### Compute control for the current way point ############# for j in range(2):
def __init__(self, drone_model: DroneModel=DroneModel.CF2X, num_drones: int=2, neighbourhood_radius: float=np.inf, initial_xyzs=None, initial_rpys=None, physics: Physics=Physics.PYB, freq: int=240, aggregate_phy_steps: int=1, gui=False, record=False, obs: ObservationType=ObservationType.KIN, act: ActionType=ActionType.RPM ): """Initialization of a generic multi-agent RL environment. Attributes `vision_attributes` and `dynamics_attributes` are selected based on the choice of `obs` and `act`; `obstacles` is set to True and overridden with landmarks for vision applications; `user_debug_gui` is set to False for performance. Parameters ---------- drone_model : DroneModel, optional The desired drone type (detailed in an .urdf file in folder `assets`). num_drones : int, optional The desired number of drones in the aviary. neighbourhood_radius : float, optional Radius used to compute the drones' adjacency matrix, in meters. initial_xyzs: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. initial_rpys: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). physics : Physics, optional The desired implementation of PyBullet physics/custom dynamics. freq : int, optional The frequency (Hz) at which the physics engine steps. aggregate_phy_steps : int, optional The number of physics steps within one call to `BaseAviary.step()`. gui : bool, optional Whether to use PyBullet's GUI. record : bool, optional Whether to save a video of the simulation in folder `files/videos/`. obs : ObservationType, optional The type of observation space (kinematic information or vision) act : ActionType, optional The type of action space (1 or 3D; RPMS, thurst and torques, waypoint or velocity with PID control; etc.) """ if num_drones < 2: print("[ERROR] in BaseMultiagentAviary.__init__(), num_drones should be >= 2") exit() if act == ActionType.TUN: print("[ERROR] in BaseMultiagentAviary.__init__(), ActionType.TUN can only used with BaseSingleAgentAviary") exit() vision_attributes = True if obs == ObservationType.RGB else False dynamics_attributes = True if act in [ActionType.DYN, ActionType.ONE_D_DYN] else False self.OBS_TYPE = obs self.ACT_TYPE = act self.EPISODE_LEN_SEC = 5 #### Create integrated controllers ######################### if act in [ActionType.PID, ActionType.VEL, ActionType.ONE_D_PID]: os.environ['KMP_DUPLICATE_LIB_OK']='True' if drone_model in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = [DSLPIDControl(drone_model=DroneModel.CF2X) for i in range(num_drones)] elif drone_model == DroneModel.HB: self.ctrl = [SimplePIDControl(drone_model=DroneModel.HB) for i in range(num_drones)] else: print("[ERROR] in BaseMultiagentAviary.__init()__, no controller is available for the specified drone_model") super().__init__(drone_model=drone_model, num_drones=num_drones, neighbourhood_radius=neighbourhood_radius, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles=True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui=False, # Remove of RPM sliders from all single agent learning aviaries vision_attributes=vision_attributes, dynamics_attributes=dynamics_attributes ) #### Set a limit on the maximum target speed ############### if act == ActionType.VEL: self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600)
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 ################################# logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz / AGGR_PHY_STEPS), num_drones=ARGS.num_drones) #### Initialize the controllers ############################ ctrl = [ DSLPIDControl(drone_model=ARGS.drone) for i in range(ARGS.num_drones) ] #### Run the simulation #################################### CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz)) action = {str(i): np.array([0, 0, 0, 0]) for i in range(ARGS.num_drones)} START = time.time() for i in range(0, int(ARGS.duration_sec * env.SIM_FREQ), AGGR_PHY_STEPS): #### Step the simulation ################################### obs, reward, done, info = env.step(action) #### Compute control at the desired frequency @@@@@######### if i % CTRL_EVERY_N_STEPS == 0: #### Compute control for the current way point #############
#### Initialize the trajectories ########################### PERIOD = 10 NUM_WP = ARGS.control_freq_hz * PERIOD TARGET_POS = np.zeros((NUM_WP, 2)) for i in range(NUM_WP): TARGET_POS[i, :] = [0.5 * np.cos(2 * np.pi * (i / NUM_WP)), 0] wp_counters = np.array([0, int(NUM_WP / 2)]) #### Initialize the logger ################################# logger = Logger(logging_freq_hz=ARGS.simulation_freq_hz, num_drones=2, duration_sec=ARGS.duration_sec) #### Initialize the controllers ############################ ctrl = [DSLPIDControl(env) for i in range(2)] #### Run the simulation #################################### CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz)) action = {str(i): np.array([0, 0, 0, 0]) for i in range(2)} START = time.time() for i in range(ARGS.duration_sec * env.SIM_FREQ): #### Step the simulation ################################### obs, reward, done, info = env.step(action) #### Compute control at the desired frequency ############## if i % CTRL_EVERY_N_STEPS == 0: #### Compute control for the current way point ############# for j in range(2):
def __init__(self, drone_model: DroneModel=DroneModel.CF2X, initial_xyzs=None, initial_rpys=None, physics: Physics=Physics.PYB, freq: int=240, aggregate_phy_steps: int=1, gui=False, record=False, obs: ObservationType=ObservationType.KIN, act: ActionType=ActionType.RPM ): """Initialization of a generic single agent RL environment. Attribute `num_drones` is automatically set to 1; `vision_attributes` and `dynamics_attributes` are selected based on the choice of `obs` and `act`; `obstacles` is set to True and overridden with landmarks for vision applications; `user_debug_gui` is set to False for performance. Parameters ---------- drone_model : DroneModel, optional The desired drone type (detailed in an .urdf file in folder `assets`). initial_xyzs: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. initial_rpys: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). physics : Physics, optional The desired implementation of PyBullet physics/custom dynamics. freq : int, optional The frequency (Hz) at which the physics engine steps. aggregate_phy_steps : int, optional The number of physics steps within one call to `BaseAviary.step()`. gui : bool, optional Whether to use PyBullet's GUI. record : bool, optional Whether to save a video of the simulation in folder `files/videos/`. obs : ObservationType, optional The type of observation space (kinematic information or vision) act : ActionType, optional The type of action space (1 or 3D; RPMS, thurst and torques, waypoint or velocity with PID control; etc.) """ vision_attributes = True if obs == ObservationType.RGB else False dynamics_attributes = True if act in [ActionType.DYN, ActionType.ONE_D_DYN] else False self.OBS_TYPE = obs self.ACT_TYPE = act self.EPISODE_LEN_SEC = 5 #### Create integrated controllers ######################### if act in [ActionType.PID, ActionType.VEL, ActionType.TUN, ActionType.ONE_D_PID]: os.environ['KMP_DUPLICATE_LIB_OK']='True' if drone_model in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) if act == ActionType.TUN: self.TUNED_P_POS = np.array([.4, .4, 1.25]) self.TUNED_I_POS = np.array([.05, .05, .05]) self.TUNED_D_POS = np.array([.2, .2, .5]) self.TUNED_P_ATT = np.array([70000., 70000., 60000.]) self.TUNED_I_ATT = np.array([.0, .0, 500.]) self.TUNED_D_ATT = np.array([20000., 20000., 12000.]) elif drone_model in [DroneModel.HB, DroneModel.ARDRONE2]: self.ctrl = SimplePIDControl(drone_model=drone_model) if act == ActionType.TUN: self.TUNED_P_POS = np.array([.1, .1, .2]) self.TUNED_I_POS = np.array([.0001, .0001, .0001]) self.TUNED_D_POS = np.array([.3, .3, .4]) self.TUNED_P_ATT = np.array([.3, .3, .05]) self.TUNED_I_ATT = np.array([.0001, .0001, .0001]) self.TUNED_D_ATT = np.array([.3, .3, .5]) else: print("[ERROR] in BaseSingleAgentAviary.__init()__, no controller is available for the specified drone_model") super().__init__(drone_model=drone_model, num_drones=1, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles=True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui=False, # Remove of RPM sliders from all single agent learning aviaries vision_attributes=vision_attributes, dynamics_attributes=dynamics_attributes ) #### Set a limit on the maximum target speed ############### if act == ActionType.VEL: self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600) #### Try _trajectoryTrackingRPMs exists IFF ActionType.TUN # if act == ActionType.TUN and not (hasattr(self.__class__, '_trajectoryTrackingRPMs') and callable(getattr(self.__class__, '_trajectoryTrackingRPMs'))): print("[ERROR] in BaseSingleAgentAviary.__init__(), ActionType.TUN requires an implementation of _trajectoryTrackingRPMs in the instantiated subclass") exit()
def __init__(self, drone_model: DroneModel = DroneModel.CF2X, initial_xyzs=None, initial_rpys=None, physics: Physics = Physics.PYB, freq: int = 240, aggregate_phy_steps: int = 1, gui=False, record=False, obs: ObservationType = ObservationType.KIN, act: ActionType = ActionType.RPM): """Initialization of a generic single agent RL environment. Attribute `num_drones` is automatically set to 1; `vision_attributes` and `dynamics_attributes` are selected based on the choice of `obs` and `act`; `obstacles` is set to True and overridden with landmarks for vision applications; `user_debug_gui` is set to False for performance. Parameters ---------- drone_model : DroneModel, optional The desired drone type (detailed in an .urdf file in folder `assets`). initial_xyzs: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. initial_rpys: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). physics : Physics, optional The desired implementation of PyBullet physics/custom dynamics. freq : int, optional The frequency (Hz) at which the physics engine steps. aggregate_phy_steps : int, optional The number of physics steps within one call to `BaseAviary.step()`. gui : bool, optional Whether to use PyBullet's GUI. record : bool, optional Whether to save a video of the simulation in folder `files/videos/`. obs : ObservationType, optional The type of observation space (kinematic information or vision) act : ActionType, optional The type of action space (1 or 3D; RPMS, thurst and torques, or waypoint with PID control) """ vision_attributes = True if obs == ObservationType.RGB else False dynamics_attributes = True if act in [ ActionType.DYN, ActionType.ONE_D_DYN ] else False self.OBS_TYPE = obs self.ACT_TYPE = act self.EPISODE_LEN_SEC = 5 #### Create integrated controllers ######################### if act in [ActionType.PID, ActionType.ONE_D_PID]: os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' if self.DRONE_MODEL in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = [ DSLPIDControl(BaseAviary(drone_model=DroneModel.CF2X)) for i in range(self.NUM_DRONES) ] elif self.DRONE_MODEL == DroneModel.HB: self.ctrl = [ SimplePIDControl(BaseAviary(drone_model=DroneModel.HB)) for i in range(self.NUM_DRONES) ] super().__init__( drone_model=drone_model, num_drones=1, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles= True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui= False, # Remove of RPM sliders from all single agent learning aviaries vision_attributes=vision_attributes, dynamics_attributes=dynamics_attributes)
record=ARGS.record_video, obstacles=ARGS.obstacles, user_debug_gui=ARGS.user_debug_gui) #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() #### Initialize the logger ################################# logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz / AGGR_PHY_STEPS), num_drones=ARGS.num_drones) #### Initialize the controllers ############################ if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]: ctrl = [ DSLPIDControl(drone_model=ARGS.drone) for i in range(ARGS.num_drones) ] elif ARGS.drone in [DroneModel.HB]: ctrl = [ SimplePIDControl(drone_model=ARGS.drone) for i in range(ARGS.num_drones) ] #### Run the simulation #################################### CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz)) action = {str(i): np.array([0, 0, 0, 0]) for i in range(ARGS.num_drones)} START = time.time() for i in range(0, int(ARGS.duration_sec * env.SIM_FREQ), AGGR_PHY_STEPS): #### Make it rain rubber ducks #############################
) INITIAL_STATE = env.reset() action = {"0": np.zeros(4)} pos_err = 9999. #### TRACE_FILE starts at [0,0,0], sim starts at [0,0,INITIAL_STATE[2]] TRACE_CTRL_REFERENCE[:, 2] = INITIAL_STATE["0"]["state"][2] #### Initialize the logger ################################# logger = Logger(logging_freq_hz=SIMULATION_FREQ_HZ, num_drones=2, duration_sec=DURATION_SEC ) #### Initialize the controller ############################# ctrl = DSLPIDControl(env) #### Run the comparison #################################### START = time.time() for i in range(DURATION_SEC*env.SIM_FREQ): #### Step the simulation ################################### obs, reward, done, info = env.step(action) #### Compute next action using the set points from the trace action["0"], pos_err, yaw_err = ctrl.computeControlFromState(control_timestep=env.TIMESTEP, state=obs["0"]["state"], target_pos=TRACE_CTRL_REFERENCE[i, 0:3], target_vel=TRACE_CTRL_REFERENCE[i, 3:6] )
def __init__(self, drone_model: DroneModel = DroneModel.CF2X, num_drones: int = 1, neighbourhood_radius: float = np.inf, initial_xyzs=None, initial_rpys=None, physics: Physics = Physics.PYB, freq: int = 240, aggregate_phy_steps: int = 1, gui=False, record=False, obstacles=False, user_debug_gui=True): """Initialization of an aviary environment for or high-level planning. Parameters ---------- drone_model : DroneModel, optional The desired drone type (detailed in an .urdf file in folder `assets`). num_drones : int, optional The desired number of drones in the aviary. neighbourhood_radius : float, optional Radius used to compute the drones' adjacency matrix, in meters. initial_xyzs: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. initial_rpys: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). physics : Physics, optional The desired implementation of PyBullet physics/custom dynamics. freq : int, optional The frequency (Hz) at which the physics engine steps. aggregate_phy_steps : int, optional The number of physics steps within one call to `BaseAviary.step()`. gui : bool, optional Whether to use PyBullet's GUI. record : bool, optional Whether to save a video of the simulation in folder `files/videos/`. obstacles : bool, optional Whether to add obstacles to the simulation. user_debug_gui : bool, optional Whether to draw the drones' axes and the GUI RPMs sliders. """ #### Create integrated controllers ######################### os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' if drone_model in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = [ DSLPIDControl(drone_model=DroneModel.CF2X) for i in range(num_drones) ] elif drone_model == DroneModel.HB: self.ctrl = [ SimplePIDControl(drone_model=DroneModel.HB) for i in range(num_drones) ] super().__init__(drone_model=drone_model, num_drones=num_drones, neighbourhood_radius=neighbourhood_radius, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles=obstacles, user_debug_gui=user_debug_gui) #### Set a limit on the maximum target speed ############### self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000 / 3600)
def __init__(self): parser = argparse.ArgumentParser( description= 'Helix flight script using CtrlAviary or VisionAviary and DSLPIDControl' ) parser.add_argument('--drone', default="cf2x", type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) parser.add_argument('--num_drones', default=1, type=int, help='Number of drones (default: 3)', metavar='') parser.add_argument('--physics', default="pyb", type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) parser.add_argument( '--vision', default=False, type=str2bool, help='Whether to use VisionAviary (default: False)', metavar='') parser.add_argument('--gui', default=True, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') parser.add_argument( '--plot', default=False, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') parser.add_argument( '--user_debug_gui', default=False, type=str2bool, help= 'Whether to add debug lines and parameters to the GUI (default: False)', metavar='') parser.add_argument( '--aggregate', default=False, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') parser.add_argument( '--obstacles', default=False, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') parser.add_argument('--simulation_freq_hz', default=240, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') parser.add_argument('--control_freq_hz', default=48, type=int, help='Control frequency in Hz (default: 48)', metavar='') parser.add_argument( '--duration_sec', default=1000, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') self.ARGS = parser.parse_args() 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(self.ARGS.num_drones)]) INIT_RPYS = np.array([[0, 0, i * (np.pi / 2) / self.ARGS.num_drones] for i in range(self.ARGS.num_drones)]) AGGR_PHY_STEPS = int( self.ARGS.simulation_freq_hz / self.ARGS.control_freq_hz) if self.ARGS.aggregate else 1 if self.ARGS.vision: self.env = VisionAviary(drone_model=self.ARGS.drone, num_drones=self.ARGS.num_drones, initial_xyzs=INIT_XYZS, initial_rpys=INIT_RPYS, physics=self.ARGS.physics, neighbourhood_radius=10, freq=self.ARGS.simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, gui=self.ARGS.gui, record=self.ARGS.record_video, obstacles=self.ARGS.obstacles) else: self.env = CtrlAviary(drone_model=self.ARGS.drone, num_drones=self.ARGS.num_drones, initial_xyzs=INIT_XYZS, initial_rpys=INIT_RPYS, physics=self.ARGS.physics, neighbourhood_radius=10, freq=self.ARGS.simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, gui=self.ARGS.gui, record=self.ARGS.record_video, obstacles=self.ARGS.obstacles, user_debug_gui=self.ARGS.user_debug_gui) PYB_CLIENT = self.env.getPyBulletClient() if self.ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = [ DSLPIDControl(self.env) for i in range(self.ARGS.num_drones) ] elif self.ARGS.drone in [DroneModel.HB]: self.ctrl = [ SimplePIDControl(self.env) for i in range(self.ARGS.num_drones) ] self.CTRL_EVERY_N_STEPS = int( np.floor(self.env.SIM_FREQ / self.ARGS.control_freq_hz)) self.action = { str(i): np.array([0, 0, 0, 0]) for i in range(self.ARGS.num_drones) } self.START = time.time()
freq=simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, gui=False, record=False, obstacles=False, user_debug_gui=False) #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() #### Initialize the logger ################################# logger = Logger(logging_freq_hz=int(simulation_freq_hz / AGGR_PHY_STEPS), num_drones=1) #### Initialize the controllers ############################ PID = DSLPIDControl(env) flip = Flip() # params = flip.get_initial_parameters() # params = np.abs(params) # params = np.array([18.774, 0.1, 0.11096576, 18.774, 0.1]) params = np.array([ 18.78215108032221, 0.08218741361206124, 0.12091343074644069, 17.951940703885207, 0.05507561729533186 ]) # U1, T1, T3, U5 = params[0:4] # disturb = np.linspace(-5, 5, 10) pbounds = { 'U1': (12, 20),