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)
#### 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):
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):
#### Apply x-axis force to the base ################################################################
# p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[1e-4,0.,0,], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[1e-4,0.,0,], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
"""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)
#### Initialize the ACTION #################################
ACTION = {}
OBS = ENV.reset()
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)
