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 #####################################################################
