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)
 def __init__(self):
     super().__init__('aviary_wrapper')
     self.step_cb_count = 0
     self.get_action_cb_count = 0
     timer_freq_hz = 240
     timer_period_sec = 1 / timer_freq_hz
     #### Create the CtrlAviary environment wrapped by the node #
     self.env = CtrlAviary(drone_model=DroneModel.CF2X,
                           num_drones=1,
                           neighbourhood_radius=np.inf,
                           initial_xyzs=None,
                           initial_rpys=None,
                           physics=Physics.PYB,
                           freq=timer_freq_hz,
                           aggregate_phy_steps=1,
                           gui=True,
                           record=False,
                           obstacles=False,
                           user_debug_gui=True)
     #### Initialize an action with the RPMs at hover ###########
     self.action = np.ones(4) * self.env.HOVER_RPM
     #### Declare publishing on 'obs' and create a timer to call
     #### action_callback every timer_period_sec ################
     self.publisher_ = self.create_publisher(Float32MultiArray, 'obs', 1)
     self.timer = self.create_timer(timer_period_sec, self.step_callback)
     #### Subscribe to topic 'action' ###########################
     self.action_subscription = self.create_subscription(
         Float32MultiArray, 'action', self.get_action_callback, 1)
     self.action_subscription  # prevent unused variable warning
Beispiel #3
0
 def __init__(self):
     super().__init__('random_control')
     self.action_cb_count = 0; self.get_obs_cb_count = 0
     #### Set the frequency used to publish actions #####################################################
     timer_freq_hz = 50; timer_period_sec = 1/timer_freq_hz
     #### Dummy CtrlAviary to obtain the HOVER_RPM constant #############################################
     self.env = CtrlAviary()
     #### Declare publishing on 'action' and create a timer to call action_callback every timer_period_sec
     self.publisher_ = self.create_publisher(Float32MultiArray, 'action', 1)
     self.timer = self.create_timer(timer_period_sec, self.action_callback)
     #### Subscribe to topic 'obs' ######################################################################
     self.obs_subscription = self.create_subscription(Float32MultiArray, 'obs', self.get_obs_callback, 1); self.obs_subscription  # prevent unused variable warning
Beispiel #4
0
    NUM_WP = ARGS.control_freq_hz * PERIOD
    TARGET_POS = np.zeros((NUM_WP, 3))
    for i in range(NUM_WP):
        TARGET_POS[i, :] = INIT_XYZ[0, 0], INIT_XYZ[
            0, 1], INIT_XYZ[0, 2] + 0.15 * (np.sin(
                (i / NUM_WP) * (2 * np.pi)) + 1)
    wp_counter = 0

    #### Create the environment ################################
    env = CtrlAviary(
        drone_model=DroneModel.CF2X,
        num_drones=1,
        initial_xyzs=INIT_XYZ,
        physics=Physics.PYB_GND,
        # physics=Physics.PYB, # For comparison
        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 logger #################################
    logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz /
                                        AGGR_PHY_STEPS),
                    num_drones=1)
Beispiel #5
0
                        metavar='')
    parser.add_argument(
        '--duration_sec',
        default=10,
        type=int,
        help='Duration of the simulation in seconds (default: 10)',
        metavar='')
    ARGS = parser.parse_args()

    #### Initialize the simulation #############################
    INIT_XYZS = np.array([[.5, 0, 1], [-.5, 0, .5]])
    env = CtrlAviary(drone_model=ARGS.drone,
                     num_drones=2,
                     initial_xyzs=INIT_XYZS,
                     physics=Physics.PYB_DW,
                     neighbourhood_radius=10,
                     freq=ARGS.simulation_freq_hz,
                     gui=ARGS.gui,
                     record=ARGS.record_video,
                     obstacles=True)

    #### 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,
Beispiel #6
0
                             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,
                                                                           2]
        wp_counters = np.array(
            [int((i * NUM_WP / 6) % NUM_WP) for i in range(NUM_DRONES)])
Beispiel #7
0
from gym_pybullet_drones.envs.BaseAviary import DroneModel
from aer1216_fall2020_hw2_ctrl import HW2Control

DURATION = 30
"""int: The duration of the simulation in seconds."""
GUI = True
"""bool: Whether to use PyBullet graphical interface."""
RECORD = False
"""bool: Whether to save a video under /files/videos. Requires ffmpeg"""

if __name__ == "__main__":

    #### Create the ENVironment ################################
    ENV = CtrlAviary(num_drones=3,
                     drone_model=DroneModel.CF2P,
                     initial_xyzs=np.array([[.0, .0, .15], [-.3, .0, .15],
                                            [.3, .0, .15]]),
                     gui=GUI,
                     record=RECORD)
    PYB_CLIENT = ENV.getPyBulletClient()

    #### Initialize the LOGGER #################################
    LOGGER = Logger(
        logging_freq_hz=ENV.SIM_FREQ,
        num_drones=3,
    )

    #### Initialize the CONTROLLERS ############################
    CTRL_0 = HW2Control(env=ENV, control_type=0)
    CTRL_1 = HW2Control(env=ENV, control_type=1)
    CTRL_2 = HW2Control(env=ENV, control_type=2)
Beispiel #8
0
        type=int,
        help='Duration of the simulation in seconds (default: 30)',
        metavar='')
    parser.add_argument(
        '--num_resets',
        default=1,
        type=int,
        help=
        'Number of times the simulation is reset to its initial conditions (default: 2)',
        metavar='')
    ARGS = parser.parse_args()

    #### Initialize the simulation #####################################################################
    env = CtrlAviary(drone_model=DroneModel.CF2X,
                     initial_xyzs=np.array([-.7, -.5, .3]).reshape(1, 3),
                     initial_rpys=np.array([0, -30 * (np.pi / 180),
                                            0]).reshape(1, 3),
                     gui=True,
                     obstacles=True)

    #### Get PyBullet's and drone's ids ################################################################
    PYB_CLIENT = env.getPyBulletClient()
    DRONE_IDS = env.getDroneIds()

    #### Make the drone weightless #####################################################################
    p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)

    #### Draw base frame ###############################################################################
    env._showDroneLocalAxes(0)

    #### Run the simulation ############################################################################
    START = time.time()
    H = 1
    INIT_XYZS = np.array([[0, 0, H]])
    simulation_freq_hz = 240
    control_freq_hz = 40
    drone = DroneModel("cf2p")
    duration_sec = 6
    AGGR_PHY_STEPS = 1
    physics = Physics("pyb")

    #### Create the environment with or without video capture ##
    env = CtrlAviary(drone_model=drone,
                     num_drones=1,
                     initial_xyzs=INIT_XYZS,
                     physics=physics,
                     neighbourhood_radius=10,
                     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)
Beispiel #10
0
from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary
from gym_pybullet_drones.utils.Logger import Logger
from gym_pybullet_drones.utils.utils import sync
from aer1216_fall2020_hw1_ctrl import HW1Control

DURATION = 10
"""int: The duration of the simulation in seconds."""
GUI = True
"""bool: Whether to use PyBullet graphical interface."""
RECORD = False
"""bool: Whether to save a video under /files/videos. Requires ffmpeg"""

if __name__ == "__main__":

    #### Create the ENVironment ################################
    ENV = CtrlAviary(gui=GUI, record=RECORD)
    PYB_CLIENT = ENV.getPyBulletClient()

    #### Initialize the LOGGER #################################
    LOGGER = Logger(logging_freq_hz=ENV.SIM_FREQ)

    #### Initialize the controller #############################
    CTRL = HW1Control(ENV)

    #### Initialize the ACTION #################################
    ACTION = {}
    OBS = ENV.reset()
    STATE = OBS["0"]["state"]
    ACTION["0"] = CTRL.compute_control(current_position=STATE[0:3],
                                       current_velocity=STATE[10:13],
                                       target_position=STATE[0:3],
Beispiel #11
0
    ARGS = parser.parse_args()

    #### Load a trace and control reference from a .pkl file ###
    with open(os.path.dirname(os.path.abspath(__file__))+"/../files/"+ARGS.trace_file, 'rb') as in_file:
        TRACE_TIMESTAMPS, TRACE_DATA, TRACE_CTRL_REFERENCE, _, _, _ = pickle.load(in_file)

    #### Compute trace's parameters ############################
    DURATION_SEC = int(TRACE_TIMESTAMPS[-1])
    SIMULATION_FREQ_HZ = int(len(TRACE_TIMESTAMPS)/TRACE_TIMESTAMPS[-1])

    #### Initialize the simulation #############################
    env = CtrlAviary(drone_model=DroneModel.CF2X,
                     num_drones=1,
                     initial_xyzs=np.array([0, 0, .1]).reshape(1, 3),
                     physics=ARGS.physics,
                     freq=SIMULATION_FREQ_HZ,
                     gui=ARGS.gui,
                     record=ARGS.record_video,
                     obstacles=False
                     )
    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
    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 drone_model in [DroneModel.CF2X, DroneModel.CF2P]:
                self.ctrl = [DSLPIDControl(CtrlAviary(drone_model=DroneModel.CF2X))]
            elif drone_model == DroneModel.HB:
                self.ctrl = [SimplePIDControl(CtrlAviary(drone_model=DroneModel.HB))]
        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
                         )
    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()
Beispiel #14
0
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)]

    #### Run the simulation ############################################################################
    CTRL_EVERY_N_STEPS= int(np.floor(env.SIM_FREQ/CONTROL_FREQ_HZ))
Beispiel #15
0
        $ python hw1_simulation.py
"""
import time
import numpy as np
from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary
from gym_pybullet_drones.utils.Logger import Logger
from gym_pybullet_drones.utils.utils import sync
from hw1_control import HW1Control

DURATION = 10  # int: the duration of the simulation in seconds
GUI = True  # bool: whether to use PyBullet graphical interface

if __name__ == "__main__":

    #### Create the ENVironment ################################
    ENV = CtrlAviary(gui=GUI)

    #### Initialize the LOGGER #################################
    LOGGER = Logger(logging_freq_hz=ENV.SIM_FREQ)

    #### Initialize the controller #############################
    CTRL = HW1Control(ENV)

    #### Initialize the ACTION #################################
    ACTION = {}
    OBS = ENV.reset()
    STATE = OBS["0"]["state"]
    ACTION["0"] = CTRL.compute_control(current_position=STATE[0:3],
                                       current_quaternion=STATE[3:7],
                                       current_velocity=STATE[10:13],
                                       current_angular_velocity=STATE[13:16],
Beispiel #16
0
                               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,
                         user_debug_gui=ARGS.user_debug_gui)

    #### Obtain the PyBullet Client ID from the environment ############################################
    PYB_CLIENT = env.getPyBulletClient()

    #### Initialize a circular trajectory ##############################################################
    PERIOD = 10
    NUM_WP = ARGS.control_freq_hz * PERIOD
    TARGET_POS = np.zeros((NUM_WP, 3))
    for i in range(NUM_WP):
        TARGET_POS[i, :] = R * np.cos(
Beispiel #17
0
if __name__ == "__main__":

    #### Load a trace and control reference from a .pkl file ###########################################
    with open(
            os.path.dirname(os.path.abspath(__file__)) + "/../files/" +
            TRACE_FILE, 'rb') as in_file:
        TRACE_TIMESTAMPS, TRACE_DATA, TRACE_CTRL_REFERENCE, _, _, _ = pickle.load(
            in_file)

    #### Compute trace's parameters ####################################################################
    DURATION_SEC = int(TRACE_TIMESTAMPS[-1])
    SIMULATION_FREQ_HZ = int(len(TRACE_TIMESTAMPS) / TRACE_TIMESTAMPS[-1])

    #### Initialize the simulation #####################################################################
    env = CtrlAviary(drone_model=DroneModel.CF2X, num_drones=1, initial_xyzs=np.array([0,0,.1]).reshape(1,3), \
                    physics=PHYSICS, freq=SIMULATION_FREQ_HZ, gui=GUI, record=RECORD_VIDEO, obstacles=False)
    INITIAL_STATE = env.reset()
    action = {
        "0": np.zeros(4)
    }
    pos_err = 9999.

    #### Assuming TRACE_FILE starts at position [0,0,0] and the 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 #####################################################################