From d186f631c54f2aff93f9c9cff8a4f5da0aab65f8 Mon Sep 17 00:00:00 2001 From: Spencer Teetaert Date: Mon, 9 Oct 2023 15:24:58 -0400 Subject: [PATCH] Improved documentation. Removed unused code --- gym_pybullet_drones/envs/CFAviary.py | 93 ++++++++++------------------ gym_pybullet_drones/examples/cf.py | 45 +++----------- 2 files changed, 42 insertions(+), 96 deletions(-) diff --git a/gym_pybullet_drones/envs/CFAviary.py b/gym_pybullet_drones/envs/CFAviary.py index e3895597c..2182a1d5b 100644 --- a/gym_pybullet_drones/envs/CFAviary.py +++ b/gym_pybullet_drones/envs/CFAviary.py @@ -31,9 +31,6 @@ 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, @@ -41,12 +38,13 @@ def __init__(self, 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,7 +77,11 @@ 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, @@ -87,7 +89,7 @@ def __init__(self, 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,7 +623,6 @@ 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 @@ -658,7 +630,6 @@ def _limitThrust(self, val): 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 \ No newline at end of file + return [qx, qy, qz, qw] \ No newline at end of file diff --git a/gym_pybullet_drones/examples/cf.py b/gym_pybullet_drones/examples/cf.py index b4defad79..227bbf24f 100644 --- a/gym_pybullet_drones/examples/cf.py +++ b/gym_pybullet_drones/examples/cf.py @@ -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()