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
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)
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]))
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
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)
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()
$ 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],
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)
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))
'--duration_sec', default=12, 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]]) AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz / ARGS.control_freq_hz) if ARGS.aggregate else 1 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, aggregate_phy_steps=AGGR_PHY_STEPS, gui=ARGS.gui, record=ARGS.record_video, obstacles=True) #### 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 /
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 )
#### Define and parse (optional) arguments for the script ########################################## parser = argparse.ArgumentParser(description='Downwash example script using CtrlAviary and DSLPIDControl') parser.add_argument('--drone', default=DroneModel.CF2X, type=lambda model: DroneModel[model], help='Drone model (default: CF2X)', metavar='') parser.add_argument('--num_drones', default=2, type=int, help='Number of drones (default: 2)', metavar='') parser.add_argument('--physics', default=Physics.PYB_GND_DRAG_DW,type=lambda phy: Physics[phy], help='Physics updates (default: PYB_GND_DRAG_DW)', 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('--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=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=ARGS.num_drones, initial_xyzs=INIT_XYZS, physics=ARGS.physics, 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, num_drones=ARGS.num_drones, duration_sec=ARGS.duration_sec) #### Initialize the controllers #################################################################### ctrl = [DSLPIDControl(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) }
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()
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)])
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
from gym_pybullet_drones.utils.Logger import Logger DRONE = DroneModel.CF2X NUM_DRONES = 2 GUI = True PHYSICS = Physics.PYB_GND_DRAG_DW RECORD_VIDEO = False SIMULATION_FREQ_HZ = 240 CONTROL_FREQ_HZ = 48 DURATION_SEC = 10 if __name__ == "__main__": #### Initialize the simulation ##################################################################### INIT_XYZS = np.array([[.5, 0, 1], [-.5, 0, .5]]) 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 the trajectories ################################################################### PERIOD = 10 NUM_WP = 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=SIMULATION_FREQ_HZ, num_drones=NUM_DRONES, duration_sec=DURATION_SEC) #### Initialize the controllers ####################################################################
import random import numpy as np import pybullet as p 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 # from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl 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) PYB_CLIENT = ENV.getPyBulletClient() #### Initialize the LOGGER ################################# LOGGER = Logger(logging_freq_hz=ENV.SIM_FREQ) #### Initialize the controller ############################# CTRL = HW1Control(ENV) # TEMP_CTRL = DSLPIDControl(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],
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)
from gym_pybullet_drones.envs.BaseAviary import DroneModel from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary from gym_pybullet_drones.utils.utils import * if __name__ == "__main__": #### Define and parse (optional) arguments for the script ########################################## parser = argparse.ArgumentParser(description='Debugging script for PyBullet applyExternalForce() and applyExternalTorque() PyBullet') parser.add_argument('--duration_sec', default=30, 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, user_debug_gui=False) #### 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() for i in range(ARGS.duration_sec*env.SIM_FREQ):
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(
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 #####################################################################