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,
drone_model: DroneModel = DroneModel.CF2X,
num_drones: int = 2,
neighbourhood_radius: float = np.inf,
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):
if num_drones < 2:
print(
"[ERROR] in BaseMultiagentAviary.__init__(), num_drones should be >= 2"
)
exit()
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 self.DRONE_MODEL in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = [
DSLPIDControl(BaseAviary(drone_model=DroneModel.CF2X))
for i in range(self.NUM_DRONES)
]
elif self.DRONE_MODEL == DroneModel.HB:
self.ctrl = [
SimplePIDControl(BaseAviary(drone_model=DroneModel.HB))
for i in range(self.NUM_DRONES)
]
super().__init__(
drone_model=drone_model,
num_drones=num_drones,
neighbourhood_radius=neighbourhood_radius,
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,
drone_model: DroneModel=DroneModel.CF2X,
num_drones: int=2,
neighbourhood_radius: float=np.inf,
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 multi-agent RL environment.
Attributes `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`).
num_drones : int, optional
The desired number of drones in the aviary.
neighbourhood_radius : float, optional
Radius used to compute the drones' adjacency matrix, in meters.
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, waypoint or velocity with PID control; etc.)
"""
if num_drones < 2:
print("[ERROR] in BaseMultiagentAviary.__init__(), num_drones should be >= 2")
exit()
if act == ActionType.TUN:
print("[ERROR] in BaseMultiagentAviary.__init__(), ActionType.TUN can only used with BaseSingleAgentAviary")
exit()
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.VEL, ActionType.ONE_D_PID]:
os.environ['KMP_DUPLICATE_LIB_OK']='True'
if drone_model in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = [DSLPIDControl(drone_model=DroneModel.CF2X) for i in range(num_drones)]
elif drone_model == DroneModel.HB:
self.ctrl = [SimplePIDControl(drone_model=DroneModel.HB) for i in range(num_drones)]
else:
print("[ERROR] in BaseMultiagentAviary.__init()__, no controller is available for the specified drone_model")
super().__init__(drone_model=drone_model,
num_drones=num_drones,
neighbourhood_radius=neighbourhood_radius,
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
)
#### Set a limit on the maximum target speed ###############
if act == ActionType.VEL:
self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600)
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, waypoint or velocity with PID control; etc.)
"""
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.VEL, ActionType.TUN, ActionType.ONE_D_PID]:
os.environ['KMP_DUPLICATE_LIB_OK']='True'
if drone_model in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X)
if act == ActionType.TUN:
self.TUNED_P_POS = np.array([.4, .4, 1.25])
self.TUNED_I_POS = np.array([.05, .05, .05])
self.TUNED_D_POS = np.array([.2, .2, .5])
self.TUNED_P_ATT = np.array([70000., 70000., 60000.])
self.TUNED_I_ATT = np.array([.0, .0, 500.])
self.TUNED_D_ATT = np.array([20000., 20000., 12000.])
elif drone_model in [DroneModel.HB, DroneModel.ARDRONE2]:
self.ctrl = SimplePIDControl(drone_model=drone_model)
if act == ActionType.TUN:
self.TUNED_P_POS = np.array([.1, .1, .2])
self.TUNED_I_POS = np.array([.0001, .0001, .0001])
self.TUNED_D_POS = np.array([.3, .3, .4])
self.TUNED_P_ATT = np.array([.3, .3, .05])
self.TUNED_I_ATT = np.array([.0001, .0001, .0001])
self.TUNED_D_ATT = np.array([.3, .3, .5])
else:
print("[ERROR] in BaseSingleAgentAviary.__init()__, no controller is available for the specified drone_model")
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
)
#### Set a limit on the maximum target speed ###############
if act == ActionType.VEL:
self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600)
#### Try _trajectoryTrackingRPMs exists IFF ActionType.TUN #
if act == ActionType.TUN and not (hasattr(self.__class__, '_trajectoryTrackingRPMs') and callable(getattr(self.__class__, '_trajectoryTrackingRPMs'))):
print("[ERROR] in BaseSingleAgentAviary.__init__(), ActionType.TUN requires an implementation of _trajectoryTrackingRPMs in the instantiated subclass")
exit()
class BaseSingleAgentAviary(BaseAviary):
"""Base single drone environment class for reinforcement learning."""
################################################################################
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, waypoint or velocity with PID control; etc.)
"""
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.VEL, ActionType.TUN, ActionType.ONE_D_PID]:
os.environ['KMP_DUPLICATE_LIB_OK']='True'
if drone_model in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X)
if act == ActionType.TUN:
self.TUNED_P_POS = np.array([.4, .4, 1.25])
self.TUNED_I_POS = np.array([.05, .05, .05])
self.TUNED_D_POS = np.array([.2, .2, .5])
self.TUNED_P_ATT = np.array([70000., 70000., 60000.])
self.TUNED_I_ATT = np.array([.0, .0, 500.])
self.TUNED_D_ATT = np.array([20000., 20000., 12000.])
elif drone_model in [DroneModel.HB, DroneModel.ARDRONE2]:
self.ctrl = SimplePIDControl(drone_model=drone_model)
if act == ActionType.TUN:
self.TUNED_P_POS = np.array([.1, .1, .2])
self.TUNED_I_POS = np.array([.0001, .0001, .0001])
self.TUNED_D_POS = np.array([.3, .3, .4])
self.TUNED_P_ATT = np.array([.3, .3, .05])
self.TUNED_I_ATT = np.array([.0001, .0001, .0001])
self.TUNED_D_ATT = np.array([.3, .3, .5])
else:
print("[ERROR] in BaseSingleAgentAviary.__init()__, no controller is available for the specified drone_model")
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
)
#### Set a limit on the maximum target speed ###############
if act == ActionType.VEL:
self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600)
#### Try _trajectoryTrackingRPMs exists IFF ActionType.TUN #
if act == ActionType.TUN and not (hasattr(self.__class__, '_trajectoryTrackingRPMs') and callable(getattr(self.__class__, '_trajectoryTrackingRPMs'))):
print("[ERROR] in BaseSingleAgentAviary.__init__(), ActionType.TUN requires an implementation of _trajectoryTrackingRPMs in the instantiated subclass")
exit()
################################################################################
def _addObstacles(self):
"""Add obstacles to the environment.
Only if the observation is of type RGB, 4 landmarks are added.
Overrides BaseAviary's method.
"""
if self.OBS_TYPE == ObservationType.RGB:
p.loadURDF("block.urdf",
[1, 0, .1],
p.getQuaternionFromEuler([0, 0, 0]),
physicsClientId=self.CLIENT
)
p.loadURDF("cube_small.urdf",
[0, 1, .1],
p.getQuaternionFromEuler([0, 0, 0]),
physicsClientId=self.CLIENT
)
p.loadURDF("duck_vhacd.urdf",
[-1, 0, .1],
p.getQuaternionFromEuler([0, 0, 0]),
physicsClientId=self.CLIENT
)
p.loadURDF("teddy_vhacd.urdf",
[0, -1, .1],
p.getQuaternionFromEuler([0, 0, 0]),
physicsClientId=self.CLIENT
)
else:
pass
################################################################################
def _actionSpace(self):
"""Returns the action space of the environment.
Returns
-------
ndarray
A Box() of size 1, 3, 4, or 6 depending on the action type.
"""
if self.ACT_TYPE == ActionType.TUN:
size = 6
elif self.ACT_TYPE in [ActionType.RPM, ActionType.DYN, ActionType.VEL]:
size = 4
elif self.ACT_TYPE == ActionType.PID:
size = 3
elif self.ACT_TYPE in [ActionType.ONE_D_RPM, ActionType.ONE_D_DYN, ActionType.ONE_D_PID]:
size = 1
else:
print("[ERROR] in BaseSingleAgentAviary._actionSpace()")
exit()
return spaces.Box(low=-1*np.ones(size),
high=np.ones(size),
dtype=np.float32
)
################################################################################
def _preprocessAction(self,
action
):
"""Pre-processes the action passed to `.step()` into motors' RPMs.
Parameter `action` is processed differenly for each of the different
action types: `action` can be of length 1, 3, 4, or 6 and represent
RPMs, desired thrust and torques, the next target position to reach
using PID control, a desired velocity vector, new PID coefficients, etc.
Parameters
----------
action : ndarray
The input action for each drone, to be translated into RPMs.
Returns
-------
ndarray
(4,)-shaped array of ints containing to clipped RPMs
commanded to the 4 motors of each drone.
"""
if self.ACT_TYPE == ActionType.TUN:
self.ctrl.setPIDCoefficients(p_coeff_pos=(action[0]+1)*self.TUNED_P_POS,
i_coeff_pos=(action[1]+1)*self.TUNED_I_POS,
d_coeff_pos=(action[2]+1)*self.TUNED_D_POS,
p_coeff_att=(action[3]+1)*self.TUNED_P_ATT,
i_coeff_att=(action[4]+1)*self.TUNED_I_ATT,
d_coeff_att=(action[5]+1)*self.TUNED_D_ATT
)
return self._trajectoryTrackingRPMs()
elif self.ACT_TYPE == ActionType.RPM:
return np.array(self.HOVER_RPM * (1+0.05*action))
elif self.ACT_TYPE == ActionType.DYN:
return nnlsRPM(thrust=(self.GRAVITY*(action[0]+1)),
x_torque=(0.05*self.MAX_XY_TORQUE*action[1]),
y_torque=(0.05*self.MAX_XY_TORQUE*action[2]),
z_torque=(0.05*self.MAX_Z_TORQUE*action[3]),
counter=self.step_counter,
max_thrust=self.MAX_THRUST,
max_xy_torque=self.MAX_XY_TORQUE,
max_z_torque=self.MAX_Z_TORQUE,
a=self.A,
inv_a=self.INV_A,
b_coeff=self.B_COEFF,
gui=self.GUI
)
elif self.ACT_TYPE == ActionType.PID:
state = self._getDroneStateVector(0)
rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP,
cur_pos=state[0:3],
cur_quat=state[3:7],
cur_vel=state[10:13],
cur_ang_vel=state[13:16],
target_pos=state[0:3]+0.1*action
)
return rpm
elif self.ACT_TYPE == ActionType.VEL:
state = self._getDroneStateVector(0)
if np.linalg.norm(action[0:3]) != 0:
v_unit_vector = action[0:3] / np.linalg.norm(action[0:3])
else:
v_unit_vector = np.zeros(3)
rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP,
cur_pos=state[0:3],
cur_quat=state[3:7],
cur_vel=state[10:13],
cur_ang_vel=state[13:16],
target_pos=state[0:3], # same as the current position
target_rpy=np.array([0,0,state[9]]), # keep current yaw
target_vel=self.SPEED_LIMIT * np.abs(action[3]) * v_unit_vector # target the desired velocity vector
)
return rpm
elif self.ACT_TYPE == ActionType.ONE_D_RPM:
return np.repeat(self.HOVER_RPM * (1+0.05*action), 4)
elif self.ACT_TYPE == ActionType.ONE_D_DYN:
return nnlsRPM(thrust=(self.GRAVITY*(1+0.05*action[0])),
x_torque=0,
y_torque=0,
z_torque=0,
counter=self.step_counter,
max_thrust=self.MAX_THRUST,
max_xy_torque=self.MAX_XY_TORQUE,
max_z_torque=self.MAX_Z_TORQUE,
a=self.A,
inv_a=self.INV_A,
b_coeff=self.B_COEFF,
gui=self.GUI
)
elif self.ACT_TYPE == ActionType.ONE_D_PID:
state = self._getDroneStateVector(0)
rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP,
cur_pos=state[0:3],
cur_quat=state[3:7],
cur_vel=state[10:13],
cur_ang_vel=state[13:16],
target_pos=state[0:3]+0.1*np.array([0,0,action[0]])
)
return rpm
else:
print("[ERROR] in BaseSingleAgentAviary._preprocessAction()")
################################################################################
def _observationSpace(self):
"""Returns the observation space of the environment.
Returns
-------
ndarray
A Box() of shape (H,W,4) or (12,) depending on the observation type.
"""
if self.OBS_TYPE == ObservationType.RGB:
return spaces.Box(low=0,
high=255,
shape=(self.IMG_RES[1], self.IMG_RES[0], 4),
dtype=np.uint8
)
elif self.OBS_TYPE == ObservationType.KIN:
############################################################
#### OBS OF SIZE 20 (WITH QUATERNION AND RPMS)
#### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WX WY WZ P0 P1 P2 P3
# obs_lower_bound = np.array([-1, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1])
# obs_upper_bound = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
# return spaces.Box( low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32 )
############################################################
#### OBS SPACE OF SIZE 12
return spaces.Box(low=np.array([-1,-1,0, -1,-1,-1, -1,-1,-1, -1,-1,-1]),
high=np.array([1,1,1, 1,1,1, 1,1,1, 1,1,1]),
dtype=np.float32
)
############################################################
else:
print("[ERROR] in BaseSingleAgentAviary._observationSpace()")
################################################################################
def _computeObs(self):
"""Returns the current observation of the environment.
Returns
-------
ndarray
A Box() of shape (H,W,4) or (12,) depending on the observation type.
"""
if self.OBS_TYPE == ObservationType.RGB:
if self.step_counter%self.IMG_CAPTURE_FREQ == 0:
self.rgb[0], self.dep[0], self.seg[0] = self._getDroneImages(0,
segmentation=False
)
#### Printing observation to PNG frames example ############
if self.RECORD:
self._exportImage(img_type=ImageType.RGB,
img_input=self.rgb[0],
path=self.ONBOARD_IMG_PATH,
frame_num=int(self.step_counter/self.IMG_CAPTURE_FREQ)
)
return self.rgb[0]
elif self.OBS_TYPE == ObservationType.KIN:
obs = self._clipAndNormalizeState(self._getDroneStateVector(0))
############################################################
#### OBS OF SIZE 20 (WITH QUATERNION AND RPMS)
# return obs
############################################################
#### OBS SPACE OF SIZE 12
return np.hstack([obs[0:3], obs[7:10], obs[10:13], obs[13:16]]).reshape(12,)
############################################################
else:
print("[ERROR] in BaseSingleAgentAviary._computeObs()")
################################################################################
def _clipAndNormalizeState(self,
state
):
"""Normalizes a drone's state to the [-1,1] range.
Must be implemented in a subclass.
Parameters
----------
state : ndarray
Array containing the non-normalized state of a single drone.
"""
raise NotImplementedError
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 self.DRONE_MODEL in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = [
DSLPIDControl(BaseAviary(drone_model=DroneModel.CF2X))
for i in range(self.NUM_DRONES)
]
elif self.DRONE_MODEL == DroneModel.HB:
self.ctrl = [
SimplePIDControl(BaseAviary(drone_model=DroneModel.HB))
for i in range(self.NUM_DRONES)
]
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)
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 the controllers ############################
if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]:
ctrl = [DSLPIDControl(env) for i in range(ARGS.num_drones)]
elif ARGS.drone in [DroneModel.HB]:
ctrl = [SimplePIDControl(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)}
START = time.time()
for i in range(0, int(ARGS.duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS):
#### 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 #############
for j in range(ARGS.num_drones):
action[str(j)], _, _ = ctrl[j].computeControlFromState(
state=obs[str(j)]["state"],
target_pos=np.array([0,0,i*0.001]),
target_rpy=np.array([0,0,1.57])
class BaseSingleAgentAviary(BaseAviary):
####################################################################################################
#### Initialize the environment ####################################################################
####################################################################################################
#### Arguments #####################################################################################
#### - drone_model (DroneModel) desired drone type (associated to an .urdf file) ###########
#### - num_drones (int) desired number of drones in the aviary #####################
#### - neighbourhood_radius (float) used to compute the drones' adjacency matrix, in meters ####
#### - initial_xyzs ((3,1) array) initial XYZ position of the drones #########################
#### - initial_rpys ((3,1) array) initial orientations of the drones (radians) ###############
#### - physics (Physics) desired implementation of physics/dynamics #################
#### - freq (int) the frequency (Hz) at which the physics engine advances ####
#### - aggregate_phy_steps (int) number of physics updates within one call of .step() #######
#### - gui (bool) whether to use PyBullet's GUI ##############################
#### - record (bool) whether to save a video of the simulation ##################
#### - obstacles (bool) whether to add obstacles to the simulation #################
#### - user_debug_gui (bool) whether to draw the drones' axes and the GUI sliders #######
#### -
####
####################################################################################################
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)
####################################################################################################
#### Add obstacles to the environment from .urdf files #############################################
####################################################################################################
def _addObstacles(self):
if self.OBS_TYPE==ObservationType.RGB:
p.loadURDF("block.urdf", [1,0,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
p.loadURDF("cube_small.urdf", [0,1,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
p.loadURDF("duck_vhacd.urdf", [-1,0,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
p.loadURDF("teddy_vhacd.urdf", [0,-1,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
else: pass
####################################################################################################
#### Return the action space of the environment, a Box(4,) #########################################
####################################################################################################
def _actionSpace(self):
#### Action vector ######## P0 P1 P2 P3
act_lower_bound = np.array([-1, -1, -1, -1])
act_upper_bound = np.array([1, 1, 1, 1])
if self.ACT_TYPE==ActionType.RPM or self.ACT_TYPE==ActionType.DYN: return spaces.Box( low=act_lower_bound, high=act_upper_bound, dtype=np.float32 )
elif self.ACT_TYPE==ActionType.PID: return spaces.Box( low=-1*np.ones(3), high=np.ones(3), dtype=np.float32 )
elif self.ACT_TYPE in [ActionType.ONE_D_RPM, ActionType.ONE_D_DYN, ActionType.ONE_D_PID]: return spaces.Box( low=np.array([-1]), high=np.array([1]), dtype=np.float32 )
else: print("[ERROR] in BaseSingleAgentAviary._actionSpace()")
####################################################################################################
#### Preprocess the action passed to step() ########################################################
####################################################################################################
#### Arguments #####################################################################################
#### - action ((4,1) array) unclipped RPMs commanded to the 4 motors of the drone ######
####################################################################################################
#### Returns #######################################################################################
#### - clipped_action ((4,1) array) clipped RPMs commanded to the 4 motors of the drone ########
####################################################################################################
def _preprocessAction(self, action):
if self.ACT_TYPE==ActionType.RPM: return np.array(self.HOVER_RPM * (1+0.05*action))
elif self.ACT_TYPE==ActionType.DYN: return self._nnlsRPM(thrust=(self.GRAVITY*(action[0]+1)), x_torque=(0.05*self.MAX_XY_TORQUE*action[1]), y_torque=(0.05*self.MAX_XY_TORQUE*action[2]), z_torque=(0.05*self.MAX_Z_TORQUE*action[3]))
elif self.ACT_TYPE==ActionType.PID:
state = self._getDroneStateVector(0)
rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP,
cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16],
target_pos=state[0:3]+1*action )
return rpm
elif self.ACT_TYPE==ActionType.ONE_D_RPM: return np.repeat(self.HOVER_RPM * (1+0.05*action), 4)
elif self.ACT_TYPE==ActionType.ONE_D_DYN: return self._nnlsRPM(thrust=(self.GRAVITY*(1+0.05*action[0])), x_torque=0, y_torque=0, z_torque=0)
elif self.ACT_TYPE==ActionType.ONE_D_PID:
state = self._getDroneStateVector(0)
rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP,
cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16],
target_pos=state[0:3]+1*np.array([0,0,action[0]]) )
return rpm
else: print("[ERROR] in BaseSingleAgentAviary._preprocessAction()")
####################################################################################################
#### Return the observation space of the environment, a Box(20,) ###################################
####################################################################################################
def _observationSpace(self):
if self.OBS_TYPE==ObservationType.RGB: return spaces.Box(low=0, high=255, shape=(self.IMG_RES[1], self.IMG_RES[0], 4), dtype=np.uint8)
elif self.OBS_TYPE==ObservationType.KIN:
###################################
#### OBS OF SIZE 20 (WITH QUATERNION AND RPMS)
#### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WR WP WY P0 P1 P2 P3
# obs_lower_bound = np.array([-1, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1])
# obs_upper_bound = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
# return spaces.Box( low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32 )
###################################
#### OBS SPACE OF SIZE 12
return spaces.Box( low=np.array([-1,-1,0, -1,-1,-1, -1,-1,-1, -1,-1,-1]), high=np.array([1,1,1, 1,1,1, 1,1,1, 1,1,1]), dtype=np.float32 )
###################################
else: print("[ERROR] in BaseSingleAgentAviary._observationSpace()")
####################################################################################################
#### Return the current observation of the environment #############################################
####################################################################################################
#### Returns #######################################################################################
#### - obs (20,) array for its content see _observationSpace() ####################
####################################################################################################
def _computeObs(self):
if self.OBS_TYPE==ObservationType.RGB:
if self.step_counter%self.IMG_CAPTURE_FREQ==0:
self.rgb[0], self.dep[0], self.seg[0] = self._getDroneImages(0, segmentation=False)
#### Printing observation to PNG frames example ####################################################
if self.RECORD: self._exportImage(img_type=ImageType.RGB, img_input=self.rgb[0], path=self.ONBOARD_IMG_PATH, frame_num=int(self.step_counter/self.IMG_CAPTURE_FREQ))
return self.rgb[0]
elif self.OBS_TYPE==ObservationType.KIN:
obs = self._clipAndNormalizeState(self._getDroneStateVector(0))
###################################
#### OBS OF SIZE 20 (WITH QUATERNION AND RPMS)
# return obs
###################################
#### OBS SPACE OF SIZE 12
return np.hstack([ obs[0:3], obs[7:10], obs[10:13], obs[13:16] ]).reshape(12,)
###################################
else: print("[ERROR] in BaseSingleAgentAviary._observationSpace()")
####################################################################################################
#### Normalize the 20 values in the simulation state to the [-1,1] range ###########################
####################################################################################################
#### Arguments #####################################################################################
#### - state ((20,1) array) raw simulation state #######################################
####################################################################################################
#### Returns #######################################################################################
#### - ...
####################################################################################################
def _clipAndNormalizeState(self, state):
pass
####################################################################################################
#### Non-negative Least Squares (NNLS) RPM from desired thrust and torques ########################
####################################################################################################
#### Arguments #####################################################################################
#### - thrust (float) desired thrust along the local z-axis ######################
#### - x_torque (float) desired x-axis torque ######################################
#### - y_torque (float) desired y-axis torque ######################################
#### - z_torque (float) desired z-axis torque ######################################
####################################################################################################
#### Returns #######################################################################################
#### - rpm ((4,1) array) RPM values to apply to the 4 motors ########################
####################################################################################################
def _nnlsRPM(self, thrust, x_torque, y_torque, z_torque):
#### Check the feasibility of thrust and torques ###################################################
if self.GUI and (thrust<0 or thrust>self.MAX_THRUST): print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible thrust {:.2f} outside range [0, {:.2f}]".format(thrust, self.MAX_THRUST))
if self.GUI and np.abs(x_torque)>self.MAX_XY_TORQUE: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible roll torque {:.2f} outside range [{:.2f}, {:.2f}]".format(x_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE))
if self.GUI and np.abs(y_torque)>self.MAX_XY_TORQUE: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible pitch torque {:.2f} outside range [{:.2f}, {:.2f}]".format(y_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE))
if self.GUI and np.abs(z_torque)>self.MAX_Z_TORQUE: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible yaw torque {:.2f} outside range [{:.2f}, {:.2f}]".format(z_torque, -self.MAX_Z_TORQUE, self.MAX_Z_TORQUE))
B = np.multiply(np.array([thrust, x_torque, y_torque, z_torque]), self.B_COEFF)
sq_rpm = np.dot(self.INV_A, B)
#### Use NNLS if any of the desired angular velocities is negative #################################
if np.min(sq_rpm)<0:
sol, res = nnls(self.A, B, maxiter=3*self.A.shape[1])
if self.GUI:
print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible squared rotor speeds, using NNLS")
print("Negative sq. rotor speeds:\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0], sq_rpm[1], sq_rpm[2], sq_rpm[3]),
"\t\tNormalized: [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0]/np.linalg.norm(sq_rpm), sq_rpm[1]/np.linalg.norm(sq_rpm), sq_rpm[2]/np.linalg.norm(sq_rpm), sq_rpm[3]/np.linalg.norm(sq_rpm)))
print("NNLS:\t\t\t\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0], sol[1], sol[2], sol[3]),
"\t\t\tNormalized: [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0]/np.linalg.norm(sol), sol[1]/np.linalg.norm(sol), sol[2]/np.linalg.norm(sol), sol[3]/np.linalg.norm(sol)),
"\t\tResidual: {:.2f}".format(res) )
sq_rpm = sol
return np.sqrt(sq_rpm)
#### 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=num_drones)
#### Initialize the controllers ############################
if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]:
ctrl = [
DSLPIDControl(drone_model=ARGS.drone) for i in range(num_drones)
]
elif ARGS.drone in [DroneModel.HB]:
ctrl = [
SimplePIDControl(drone_model=ARGS.drone) for i in range(num_drones)
]
#### Run the simulation ####################################
CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ / ARGS.control_freq_hz))
# Action of [0, 0, 0, 0] puts the drone into a hover position
action = {
0: np.array([0, 0, 0, 0, 0]),
1: np.array([0, 0, 0, 0]),
}
done = {"__all__": False}
START = time.time()
obs = env.reset()
state = {
env.shooter_id: shooter_policy.get_initial_state(),
env.defender_id: shooter_policy.get_initial_state(),
PYB_CLIENT = env.getPyBulletClient()
#### Initialize the logger #################################
logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz /
AGGR_PHY_STEPS),
num_drones=ARGS.num_drones)
#### Initialize the controllers ############################
if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]:
ctrl = [
DSLPIDControl(drone_model=ARGS.drone)
for i in range(ARGS.num_drones)
]
elif ARGS.drone in [DroneModel.HB]:
ctrl = [
SimplePIDControl(drone_model=ARGS.drone)
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)}
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)
def __init__(self,
drone_model: DroneModel = DroneModel.CF2X,
num_drones: int = 1,
neighbourhood_radius: float = np.inf,
initial_xyzs=None,
initial_rpys=None,
physics: Physics = Physics.PYB,
freq: int = 240,
aggregate_phy_steps: int = 1,
gui=False,
record=False,
obstacles=False,
user_debug_gui=True):
"""Initialization of an aviary environment for or high-level planning.
Parameters
----------
drone_model : DroneModel, optional
The desired drone type (detailed in an .urdf file in folder `assets`).
num_drones : int, optional
The desired number of drones in the aviary.
neighbourhood_radius : float, optional
Radius used to compute the drones' adjacency matrix, in meters.
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/`.
obstacles : bool, optional
Whether to add obstacles to the simulation.
user_debug_gui : bool, optional
Whether to draw the drones' axes and the GUI RPMs sliders.
"""
#### Create integrated controllers #########################
os.environ['KMP_DUPLICATE_LIB_OK'] = 'True'
if drone_model in [DroneModel.CF2X, DroneModel.CF2P]:
self.ctrl = [
DSLPIDControl(drone_model=DroneModel.CF2X)
for i in range(num_drones)
]
elif drone_model == DroneModel.HB:
self.ctrl = [
SimplePIDControl(drone_model=DroneModel.HB)
for i in range(num_drones)
]
super().__init__(drone_model=drone_model,
num_drones=num_drones,
neighbourhood_radius=neighbourhood_radius,
initial_xyzs=initial_xyzs,
initial_rpys=initial_rpys,
physics=physics,
freq=freq,
aggregate_phy_steps=aggregate_phy_steps,
gui=gui,
record=record,
obstacles=obstacles,
user_debug_gui=user_debug_gui)
#### Set a limit on the maximum target speed ###############
self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000 / 3600)
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()
