Beispiel #1
0
 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)
Beispiel #2
0
 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)
Beispiel #3
0
    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)
Beispiel #4
0
    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()
Beispiel #5
0
    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)
Beispiel #6
0
                      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,
                      user_debug_gui=ARGS.user_debug_gui
                      )
 #### Obtain the PyBullet Client ID from the environment ####
 PYB_CLIENT = env.getPyBulletClient()
 #### Initialize the controllers ############################
 if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]:
     ctrl = [DSLPIDControl(env) for i in range(ARGS.num_drones)]
 elif ARGS.drone in [DroneModel.HB]:
     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):
     #### 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(ARGS.num_drones):
             action[str(j)], _, _ = ctrl[j].computeControlFromState(
                                                                    state=obs[str(j)]["state"],
                                                                    target_pos=np.array([0,0,i*0.001]),
                                                                    target_rpy=np.array([0,0,1.57])
    #### 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=num_drones)

    #### Initialize the controllers ############################
    if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]:
        ctrl = [
            DSLPIDControl(drone_model=ARGS.drone) for i in range(num_drones)
        ]
    elif ARGS.drone in [DroneModel.HB]:
        ctrl = [
            SimplePIDControl(drone_model=ARGS.drone) for i in range(num_drones)
        ]

    #### Run the simulation ####################################
    CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz))
    # Action of [0, 0, 0, 0] puts the drone into a hover position
    action = {
        0: np.array([0, 0, 0, 0, 0]),
        1: np.array([0, 0, 0, 0]),
    }
    done = {"__all__": False}
    START = time.time()
    obs = env.reset()
    state = {
        env.shooter_id: shooter_policy.get_initial_state(),
        env.defender_id: shooter_policy.get_initial_state(),
    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 #############################
        # 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)
Beispiel #9
0
    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()