class AviaryWrapper(Node):

    #### Initialize the node ###################################
    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

    #### Step the env and publish drone0's state on topic 'obs'
    def step_callback(self):
        self.step_cb_count += 1
        obs, reward, done, info = self.env.step({"0": self.action})
        msg = Float32MultiArray()
        msg.data = obs["0"]["state"].tolist()
        self.publisher_.publish(msg)
        if self.step_cb_count % 10 == 0:
            self.get_logger().info('Publishing obs: "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f"' \
                                   % (msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4],
                                      msg.data[5], msg.data[6], msg.data[7], msg.data[8], msg.data[9],
                                      msg.data[10], msg.data[11], msg.data[12], msg.data[13], msg.data[14],
                                      msg.data[15], msg.data[16], msg.data[17], msg.data[18], msg.data[19]
                                      )
                                   )

    #### Read the action to apply to the env from topic 'action'
    def get_action_callback(self, msg):
        self.get_action_cb_count += 1
        self.action = np.array(
            [msg.data[0], msg.data[1], msg.data[2], msg.data[3]])
        if self.get_action_cb_count % 10 == 0:
            self.get_logger().info(
                'I received action: "%f" "%f" "%f" "%f"' %
                (msg.data[0], msg.data[1], msg.data[2], msg.data[3]))
Esempio n. 2
0
                    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:

            #### Compute control for the current way point #############
            action["0"], _, _ = ctrl.computeControlFromState(
                control_timestep=CTRL_EVERY_N_STEPS * env.TIMESTEP,
                state=obs["0"]["state"],
                target_pos=TARGET_POS[wp_counter, :],
            )

            #### Go to the next way point and loop #####################
            wp_counter = wp_counter + 1 if wp_counter < (NUM_WP - 1) else 0

        #### Log the simulation ####################################
Esempio n. 3
0
    #### Derive the target trajectory to obtain target velocities and accelerations
    TARGET_VELOCITY[1:, :] = (TARGET_POSITION[1:, :] -
                              TARGET_POSITION[0:-1, :]) / ENV.SIM_FREQ
    TARGET_ACCELERATION[1:, :] = (TARGET_VELOCITY[1:, :] -
                                  TARGET_VELOCITY[0:-1, :]) / ENV.SIM_FREQ

    #### Run the simulation ####################################
    START = time.time()
    for i in range(0, DURATION * ENV.SIM_FREQ):

        ### Secret control performance booster #####################
        # if i/ENV.SIM_FREQ>3 and i%30==0 and i/ENV.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [random.gauss(0, 0.3), 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, _, _, _ = ENV.step(ACTION)

        #### Compute control for drone 0 ###########################
        STATE = OBS["0"]["state"]
        ACTION["0"] = CTRL_0.compute_control(
            current_position=STATE[0:3],
            current_velocity=STATE[10:13],
            current_rpy=STATE[7:10],
            target_position=TARGET_POSITION[i, :],
            target_velocity=TARGET_VELOCITY[i, :],
            target_acceleration=TARGET_ACCELERATION[i, :])
        #### Log drone 0 ###########################################
        LOGGER.log(drone=0, timestamp=i / ENV.SIM_FREQ, state=STATE)

        #### Compute control for drone 1 ###########################
        STATE = OBS["1"]["state"]
Esempio n. 4
0
class start_drone():
    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()

    def move(self, iterator, target_pos, target_ori):
        obs, reward, done, info = self.env.step(self.action)
        #### Compute control at the desired frequency ##############
        if iterator % self.CTRL_EVERY_N_STEPS == 0:
            #### Compute control for the current way point #############
            for j in range(self.ARGS.num_drones):
                self.action[str(
                    j)], _, _ = self.ctrl[j].computeControlFromState(
                        state=obs[str(j)]["state"],
                        target_pos=np.array(target_pos),
                        target_rpy=np.array(target_ori))
        #### Sync the simulation ###################################
        if self.ARGS.gui:
            sync(iterator, self.START, self.env.TIMESTEP)