Skip to content

Commit

Permalink
Improved documentation. Removed unused code
Browse files Browse the repository at this point in the history
spencerteetaert committed Oct 9, 2023
1 parent 40ce90d commit d186f63
Showing 2 changed files with 42 additions and 96 deletions.
93 changes: 33 additions & 60 deletions gym_pybullet_drones/envs/CFAviary.py
Original file line number Diff line number Diff line change
@@ -31,22 +31,20 @@ class CFAviary(BaseAviary):
################################################################################

def __init__(self,
firmware_freq: int = 500,
ctrl_freq: int = 25,
verbose=False,
drone_model: DroneModel=DroneModel.CF2X,
num_drones: int=1,
neighbourhood_radius: float=np.inf,
initial_xyzs=None,
initial_rpys=None,
physics: Physics=Physics.PYB,
pyb_freq: int = 500,
ctrl_freq: int = 25,
gui=False,
record=False,
obstacles=False,
user_debug_gui=True,
output_folder='results',
udp_ip="127.0.0.1"
verbose=False,
):
"""Initialization of an aviary environment for use of BetaFlight controller.
@@ -79,15 +77,19 @@ def __init__(self,
udp_ip : base ip for betaflight controller emulator
"""
assert (pyb_freq % firmware_freq == 0), f"pyb_freq must be a multiple of firmware_freq for CFAviary {pyb_freq} {firmware_freq}"
firmware_freq = 500 if self.CONTROLLER == "mellinger" else 1000
assert (pyb_freq % firmware_freq == 0), f"pyb_freq ({pyb_freq}) must be a multiple of firmware_freq ({firmware_freq}) for CFAviary."
if num_drones != 1:
raise NotImplementedError("Multi-agent support for CF Aviary is not yet implemented.")

super().__init__(drone_model=drone_model,
num_drones=num_drones,
neighbourhood_radius=neighbourhood_radius,
initial_xyzs=initial_xyzs,
initial_rpys=initial_rpys,
physics=physics,
pyb_freq=pyb_freq,
ctrl_freq=pyb_freq,
ctrl_freq=firmware_freq, # ctrl_freq in this variable (self.CTRL_FREQ) corresponds to ctrl rate from aviary, in this case, firmware_freq
gui=gui,
record=record,
obstacles=obstacles,
@@ -107,7 +109,6 @@ def __init__(self,

self._initalize_cffirmware()


def _initalize_cffirmware(self):
"""Resets the firmware_wrapper object.
@@ -163,7 +164,6 @@ def _initalize_cffirmware(self):

# Reset environment
init_obs, init_info = super().reset()
print(init_obs, init_obs.shape)
init_pos=np.array([init_obs[0][0], init_obs[0][1], init_obs[0][2]]) # global coord, m
init_vel=np.array([init_obs[0][10], init_obs[0][11], init_obs[0][12]]) # global coord, m/s
init_rpy = np.array([init_obs[0][7], init_obs[0][8], init_obs[0][9]]) # body coord, rad
@@ -181,21 +181,9 @@ def _initalize_cffirmware(self):

# Initialize visualization tools
self.first_motor_killed_print = True
print(init_info)
# self.pyb_clinet = init_info['pyb_client']
self.last_visualized_setpoint = None

self.results_dict = { 'obs': [],
'reward': [],
'done': [],
'info': [],
'action': [],
}

return init_obs, init_info

################################################################################

def step(self, i):
'''Step the firmware_wrapper class and its environment.
This function should be called once at the rate of ctrl_freq. Step processes and high level commands,
@@ -232,7 +220,7 @@ def step(self, i):
# Update state
state_timestamp = int(self.tick / self.firmware_freq * 1e3)
if self.STATE_DELAY:
raise NotImplementedError("State delay is not implemented. Leave at 0.")
raise NotImplementedError("State delay is not yet implemented. Leave at 0.")
self._update_state(state_timestamp, *self.state_history[0])
self.state_history = self.state_history[1:] + [[cur_pos, cur_vel, cur_acc, cur_rpy * self.RAD_TO_DEG]]
else:
@@ -267,7 +255,6 @@ def step(self, i):
if self.first_motor_killed_print:
print("Drone firmware error. Motors are killed.")
self.first_motor_killed_print = False
done = True

self.action = action

@@ -278,19 +265,10 @@ def _update_initial_state(self, obs):
self.prev_rpy = np.array([obs[7], obs[8], obs[9]])


def close_results_dict(self):
"""Cleanup the rtesults dict and munchify it.
##################################
########## Sensor Data ###########
##################################

"""
self.results_dict['obs'] = np.vstack(self.results_dict['obs'])
self.results_dict['reward'] = np.vstack(self.results_dict['reward'])
self.results_dict['done'] = np.vstack(self.results_dict['done'])
self.results_dict['info'] = np.vstack(self.results_dict['info'])
self.results_dict['action'] = np.vstack(self.results_dict['action'])

self.results_dict = munchify(self.results_dict)

#region Sensor update
def _update_sensorData(self, timestamp, acc_vals, gyro_vals, baro_vals=[1013.25, 25]):
'''
Axis3f acc; // Gs
@@ -312,19 +290,16 @@ def _update_sensorData(self, timestamp, acc_vals, gyro_vals, baro_vals=[1013.25,
self.sensorData.interruptTimestamp = timestamp
self.sensorData_set = True


def _update_gyro(self, x, y, z):
self.sensorData.gyro.x = firm.lpf2pApply(self.gyrolpf[0], x)
self.sensorData.gyro.y = firm.lpf2pApply(self.gyrolpf[1], y)
self.sensorData.gyro.z = firm.lpf2pApply(self.gyrolpf[2], z)


def _update_acc(self, x, y, z):
self.sensorData.acc.x = firm.lpf2pApply(self.acclpf[0], x)
self.sensorData.acc.y = firm.lpf2pApply(self.acclpf[1], y)
self.sensorData.acc.z = firm.lpf2pApply(self.acclpf[2], z)


def _update_baro(self, baro, pressure, temperature):
'''
pressure: hPa
@@ -334,9 +309,12 @@ def _update_baro(self, baro, pressure, temperature):
baro.pressure = pressure #* 0.01 Best guess is this is because the sensor encodes raw reading two decimal places and stores as int
baro.temperature = temperature
baro.asl = (((1015.7 / baro.pressure)**0.1902630958 - 1) * (25 + 273.15)) / 0.0065
#endregion


##################################
######### State Update ###########
##################################

#region State update
def _update_state(self, timestamp, pos, vel, acc, rpy, quat=None):
'''
attitude_t attitude; // deg (legacy CF2 body coordinate system, where pitch is inverted)
@@ -354,14 +332,12 @@ def _update_state(self, timestamp, pos, vel, acc, rpy, quat=None):
self._update_3D_vec(self.state.acc, timestamp, *acc)
self.state_set = True


def _update_3D_vec(self, point, timestamp, x, y, z):
point.x = x
point.y = y
point.z = z
point.timestamp = timestamp


def _update_attitudeQuaternion(self, quaternion_t, timestamp, qx, qy, qz, qw=None):
'''Updates attitude quaternion.
@@ -378,16 +354,17 @@ def _update_attitudeQuaternion(self, quaternion_t, timestamp, qx, qy, qz, qw=Non
quaternion_t.z = qz
quaternion_t.w = qw


def _update_attitude_t(self, attitude_t, timestamp, roll, pitch, yaw):
attitude_t.timestamp = timestamp
attitude_t.roll = roll
attitude_t.pitch = -pitch # Legacy representation in CF firmware
attitude_t.yaw = yaw
#endregion

################################################################################
#region Controller

##################################
########### Controller ###########
##################################

def _step_controller(self):
if not (self.sensorData_set):
print("WARNING: sensorData has not been updated since last controller call.")
@@ -471,7 +448,6 @@ def sendFullStateCmd(self, pos, vel, acc, yaw, rpy_rate, timestep):
"""
self.command_queue += [['_sendFullStateCmd', [pos, vel, acc, yaw, rpy_rate, timestep]]]


def _sendFullStateCmd(self, pos, vel, acc, yaw, rpy_rate, timestep):
# print(f"INFO_{self.tick}: Full state command sent.")
self.setpoint.position.x = pos[0]
@@ -525,7 +501,6 @@ def _sendTakeoffCmd(self, height, duration):
firm.crtpCommanderHighLevelTakeoff(height, duration)
self.full_state_cmd_override = False


def sendTakeoffYawCmd(self, height, duration, yaw):
"""Adds a takeoffyaw command to command processing queue.
@@ -540,7 +515,6 @@ def _sendTakeoffYawCmd(self, height, duration, yaw):
firm.crtpCommanderHighLevelTakeoffYaw(height, duration, yaw)
self.full_state_cmd_override = False


def sendTakeoffVelCmd(self, height, vel, relative):
"""Adds a takeoffvel command to command processing queue.
@@ -555,7 +529,6 @@ def _sendTakeoffVelCmd(self, height, vel, relative):
firm.crtpCommanderHighLevelTakeoffWithVelocity(height, vel, relative)
self.full_state_cmd_override = False


def sendLandCmd(self, height, duration):
"""Adds a land command to command processing queue.
@@ -569,7 +542,6 @@ def _sendLandCmd(self, height, duration):
firm.crtpCommanderHighLevelLand(height, duration)
self.full_state_cmd_override = False


def sendLandYawCmd(self, height, duration, yaw):
"""Adds a landyaw command to command processing queue.
@@ -584,7 +556,6 @@ def _sendLandYawCmd(self, height, duration, yaw):
firm.crtpCommanderHighLevelLandYaw(height, duration, yaw)
self.full_state_cmd_override = False


def sendLandVelCmd(self, height, vel, relative):
"""Adds a landvel command to command processing queue.
@@ -599,7 +570,6 @@ def _sendLandVelCmd(self, height, vel, relative):
firm.crtpCommanderHighLevelLandWithVelocity(height, vel, relative)
self.full_state_cmd_override = False


def sendStopCmd(self):
"""Adds a stop command to command processing queue.
"""
@@ -609,7 +579,6 @@ def _sendStopCmd(self):
firm.crtpCommanderHighLevelStop()
self.full_state_cmd_override = False


def sendGotoCmd(self, pos, yaw, duration_s, relative):
"""Adds a goto command to command processing queue.
@@ -637,8 +606,12 @@ def _notifySetpointStop(self):
self.full_state_cmd_override = False


##################################
###### Hardware Functions ########
##################################

BRUSHED = True
SUPPLY_VOLTAGE = 3 # QUESTION: Is change of battery life worth simulating?
SUPPLY_VOLTAGE = 3
def _motorsGetPWM(self, thrust):
if (self.BRUSHED):
thrust = thrust / 65536 * 60
@@ -650,15 +623,13 @@ def _motorsGetPWM(self, thrust):
else:
raise NotImplementedError("Emulator does not support the brushless motor configuration at this time.")


def _limitThrust(self, val):
if val > self.MAX_PWM:
return self.MAX_PWM
elif val < 0:
return 0
return val


def _powerDistribution(self, control_t):
motor_pwms = []
if self.QUAD_FORMATION_X:
@@ -679,7 +650,11 @@ def _powerDistribution(self, control_t):
self.pwms = motor_pwms
else:
self.pwms = np.clip(motor_pwms, self.MIN_PWM).tolist()
#endregion


##################################
##### Base Aviary Overrides ######
##################################

def _actionSpace(self):
"""Returns the action space of the environment.
@@ -810,7 +785,6 @@ def _computeInfo(self):
"""
return {"answer": 42} #### Calculated by the Deep Thought supercomputer in 7.5M years

#region Utils
def _get_quaternion_from_euler(roll, pitch, yaw):
"""Convert an Euler angle to a quaternion.
@@ -827,5 +801,4 @@ def _get_quaternion_from_euler(roll, pitch, yaw):
qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)

return [qx, qy, qz, qw]
#endregion
return [qx, qy, qz, qw]
45 changes: 9 additions & 36 deletions gym_pybullet_drones/examples/cf.py
Original file line number Diff line number Diff line change
@@ -1,34 +1,14 @@
"""Control + Betaflight.
"""CrazyFlie software-in-the-loop control example.
Setup
-----
Step 1: Clone and open betaflight's source:
$ git clone https://github.com/betaflight/betaflight
$ cd betaflight/
$ code ./src/main/main.c
Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz`
(https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52)
from Betaflight's `SIMULATOR_BUILD` and compile:
$ cd betaflight/
$ make arm_sdk_install
$ make TARGET=SITL
Step 3: Copy over the configured `eeprom.bin` file from folder
`gym-pybullet-drones/gym_pybullet_drones/assets/` to BF's main folder `betaflight/`
$ cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/
Step 1: Clone pycffirmware from https://github.com/utiasDSL/pycffirmware
Step 2: Follow the install instructions for pycffirmware in its README
Example
-------
In one terminal run the SITL Betaflight:
$ cd betaflight/
$ ./obj/main/betaflight_SITL.elf
In a separate terminal, run:
$ cd gym-pybullet-drones/gym_pybullet_drones/examples/
$ python beta.py
In terminal, run:
python gym_pybullet_drones/examples/cf.py
"""
import time
@@ -52,7 +32,6 @@
DEFAULT_USER_DEBUG_GUI = False
DEFAULT_SIMULATION_FREQ_HZ = 500
DEFAULT_CONTROL_FREQ_HZ = 25
DEFAULT_DURATION_SEC = 20
DEFAULT_OUTPUT_FOLDER = 'results'
NUM_DRONES = 1
INIT_XYZ = np.array([[.5*i, .5*i, .1] for i in range(NUM_DRONES)])
@@ -66,12 +45,10 @@ def run(
user_debug_gui=DEFAULT_USER_DEBUG_GUI,
simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ,
control_freq_hz=DEFAULT_CONTROL_FREQ_HZ,
duration_sec=DEFAULT_DURATION_SEC,
output_folder=DEFAULT_OUTPUT_FOLDER,
):
#### Create the environment with or without video capture ##
env = CFAviary(firmware_freq=500,
drone_model=drone,
env = CFAviary(drone_model=drone,
num_drones=NUM_DRONES,
initial_xyzs=INIT_XYZ,
initial_rpys=INIT_RPY,
@@ -94,7 +71,7 @@ def run(
)

#### Run the simulation ####################################
delta = 75 # 3s
delta = 75 # 3s @ 25hz control loop
trajectory = [[0, 0, 0] for i in range(delta)] + \
[[0, 0, i/delta] for i in range(delta)] + \
[[i/delta, 0, 1] for i in range(delta)] + \
@@ -103,8 +80,6 @@ def run(
[[0, 1-i/delta, 1] for i in range(delta)] + \
[[0, 0, 1-i/delta] for i in range(delta)]

action = np.zeros((NUM_DRONES,4))
ARM_TIME = 1.
START = time.time()
for i in range(0, len(trajectory)):
t = i/env.ctrl_freq
@@ -122,7 +97,6 @@ def run(
env.sendFullStateCmd(pos, vel, acc, yaw, rpy_rate, t)
except:
break


#### Log the simulation ####################################
for j in range(NUM_DRONES):
@@ -158,9 +132,8 @@ def run(
parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='')
parser.add_argument('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='')
parser.add_argument('--user_debug_gui', default=DEFAULT_USER_DEBUG_GUI, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='')
parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 240)', metavar='')
parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 48)', metavar='')
parser.add_argument('--duration_sec', default=DEFAULT_DURATION_SEC, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='')
parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 500)', metavar='')
parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 25)', metavar='')
parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='')
ARGS = parser.parse_args()

0 comments on commit d186f63

Please sign in to comment.