From 645896c9c810e5b5b6cecc97a5ba3d1b731e15a3 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Sun, 8 Oct 2023 17:49:36 +0400 Subject: [PATCH] Use eeprom.bin instead of BF configurator --- README.md | 3 +- gym_pybullet_drones/assets/beta.txt | 82 ------------ gym_pybullet_drones/assets/eeprom.bin | Bin 0 -> 32768 bytes gym_pybullet_drones/examples/beta.py | 167 +++--------------------- gym_pybullet_drones/examples/beta2.py | 176 -------------------------- 5 files changed, 22 insertions(+), 406 deletions(-) delete mode 100644 gym_pybullet_drones/assets/beta.txt create mode 100644 gym_pybullet_drones/assets/eeprom.bin delete mode 100644 gym_pybullet_drones/examples/beta2.py diff --git a/README.md b/README.md index 9af67940a..305726787 100644 --- a/README.md +++ b/README.md @@ -40,13 +40,12 @@ python3 learn.py ### Betaflight SITL example (Ubuntu only) -First, check the steps in the docstrings of [`beta.py`](https://github.com/utiasDSL/gym-pybullet-drones/blob/main/gym_pybullet_drones/examples/beta.py), then, in one terminal, run the Betaflight SITL binary - ```sh git clone https://github.com/betaflight/betaflight cd betaflight/ make arm_sdk_install # if needed, `apt install curl`` make TARGET=SITL # comment out this line: https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52 +cp cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ betaflight/obj/main/betaflight_SITL.elf ``` diff --git a/gym_pybullet_drones/assets/beta.txt b/gym_pybullet_drones/assets/beta.txt deleted file mode 100644 index bf44da53f..000000000 --- a/gym_pybullet_drones/assets/beta.txt +++ /dev/null @@ -1,82 +0,0 @@ -defaults nosave - - -# version -# Betaflight / SITL (SITL) 4.5.0 Jul 7 2023 / 10:53:28 (b54ae921f) MSP API: 1.46 - -# start the command batch -batch start - -# reset configuration to default settings -defaults nosave - -mcu_id 000000000000000100000002 -signature - -# feature -feature -GPS -feature -TELEMETRY - -# aux -aux 0 0 0 1300 1700 0 0 - -# master -set motor_pwm_protocol = PWM -set pid_process_denom = 16 - -profile 0 - -# profile 0 -set pidsum_limit = 1000 -set pidsum_limit_yaw = 1000 -set p_pitch = 70 -set i_pitch = 75 -set d_pitch = 43 -set f_pitch = 224 -set p_roll = 58 -set i_roll = 62 -set d_roll = 42 -set f_roll = 187 -set p_yaw = 58 -set i_yaw = 62 -set f_yaw = 187 -set d_min_roll = 42 -set d_min_pitch = 43 -set simplified_master_multiplier = 130 -set simplified_i_gain = 60 -set simplified_d_gain = 110 -set simplified_dmax_gain = 0 -set simplified_feedforward_gain = 120 -set simplified_pitch_d_gain = 90 -set simplified_pitch_pi_gain = 115 - -profile 1 - -profile 2 - -profile 3 - -# restore original profile selection -profile 0 - -rateprofile 0 - -# rateprofile 0 -set rates_type = BETAFLIGHT -set roll_rc_rate = 180 -set pitch_rc_rate = 180 -set yaw_rc_rate = 180 -set roll_srate = 0 -set pitch_srate = 0 -set yaw_srate = 0 - -rateprofile 1 - -rateprofile 2 - -rateprofile 3 - -# restore original rateprofile selection -rateprofile 0 - -# save configuration \ No newline at end of file diff --git a/gym_pybullet_drones/assets/eeprom.bin b/gym_pybullet_drones/assets/eeprom.bin new file mode 100644 index 0000000000000000000000000000000000000000..a3e16b7d7130f2dba63d6e3c2ce81f4010498789 GIT binary patch literal 32768 zcmeI!zfTik7zgn0bJw>A2eu7f8e2&v5U`YA<|ac3L7Oy@IuON$f$5!$VKEvO7#wtE zVqkD~(xsb;lLM3S4{%^?oEg*wpSx?}Km;Wk2fy#7*XR1)z3+3M_ulvV=ADnPg1A&M z11Aa3+!rya^=NBR}Q^}Oh~Ua#Yi);46k`!p1W zjyh!YTLMLlsAB2FHG`Zu!z|f->E<@IRK+@GmZ!0SDNSJgL~i@CB^S@FGJH;MHYs8) zb4!is#oQj^l0J^{18Fik@=1|Y?4)8h6=zbhNyXVz>~&&`dlQ1KL9B8m$db*8QgWY+5Fa|5gCgIuH)_j-MF!BaR!7?wB*3 ziT;4R@^YGZpl#}?jp$o(APS;U7IXj3@?8xb&9^(&dI#F*mVSES=zh(Y9LADM;;IJ5 zlY5SPyXdYTH={wd5o8YvQJ0?qmDe0zJB;EBO7YAjGIH~7P6F4|bVrTI% zWIzE5P=Epypa2CZKmiI+fC3bt00k&O0SZun0u-PC1t>rP3Q&Lo6rcbFC_n)UP=Epy zpa2CZKmiI+fC3bt00k&O0SZun0u-PC1t>rP3Q&Lo6rcbFC_n)UP=Epypa2CZKmiI+ zfC3bt00k&O0SZun0u-PC1t>rP3Q&Lo6rcbFC_n)UP=Epypa2CZKmiI+fC3bt00k&O M0SZun0)JQFJL>qrlK=n! literal 0 HcmV?d00001 diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index db96c991c..b092ad423 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -2,9 +2,10 @@ Setup ----- -Step 1: Clone betaflight: +Step 1: Clone and open betaflight's source: $ git clone https://github.com/betaflight/betaflight - $ cd 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) @@ -13,19 +14,9 @@ $ make arm_sdk_install $ make TARGET=SITL -Step 3: Install betaflight configurator (https://github.com/betaflight/betaflight-configurator/releases): - $ wget https://github.com/betaflight/betaflight-configurator/releases/download/10.9.0/betaflight-configurator_10.9.0_amd64.deb - $ sudo dpkg -i betaflight-configurator_10.9.0_amd64.deb - If needed, also run: - $ apt install libgconf-2-4 - $ apt --fix-broken install - -Step 4: Load the configuration file onto the target using the BF configurator: - First, start the SITL controller: - $ ./obj/main/betaflight_SITL.elf - Type address `tcp://localhost:5761` (top right) and click `Connect` - Find the `Presets` tab (on the left) -> `Load backup` -> select file `../assets/beta.txt` - Restart `betaflight_SITL.elf` +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/ Example ------- @@ -40,25 +31,17 @@ $ python beta.py """ -import os import time import argparse -from datetime import datetime -import socket -import struct -import pdb -import math -import random import numpy as np -import pybullet as p -import matplotlib.pyplot as plt import csv from transforms3d.quaternions import rotate_vector, qconjugate, mat2quat, qmult from transforms3d.utils import normalized_vector from gym_pybullet_drones.utils.enums import DroneModel, Physics -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.BetaAviary import BetaAviary +from gym_pybullet_drones.control.CTBRControl import CTBRControl from gym_pybullet_drones.utils.Logger import Logger from gym_pybullet_drones.utils.utils import sync, str2bool @@ -72,22 +55,6 @@ DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' -UDP_IP = "127.0.0.1" - -sock = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP - -sock_pwm = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP -sock_pwm.bind((UDP_IP, 9002)) -sock_pwm.settimeout(0.0) - -# sock_raw = socket.socket(socket.AF_INET, # Internet -# socket.SOCK_DGRAM) # UDP -# sock_raw.bind((UDP_IP, 9001)) -# sock_raw.settimeout(0.0) - - def run( drone=DEFAULT_DRONES, physics=DEFAULT_PHYSICS, @@ -100,7 +67,7 @@ def run( output_folder=DEFAULT_OUTPUT_FOLDER, ): #### Create the environment with or without video capture ## - env = CtrlAviary(drone_model=drone, + env = BetaAviary(drone_model=drone, num_drones=1, initial_xyzs=np.array([[.0, .0, .1]]), initial_rpys=np.array([[.0, .0, .0]]), @@ -111,6 +78,8 @@ def run( user_debug_gui=user_debug_gui ) + ctrl = CTBRControl(drone_model=drone) + #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() @@ -140,80 +109,21 @@ def run( TRAJ_TIME = 1.5 START = time.time() for i in range(0, int(duration_sec*env.CTRL_FREQ)): - - #### Step the simulation ################################### - obs, reward, terminated, truncated, info = env.step(action) - - #### State message to Betaflight ########################### - o = obs[0] # p, q, euler, v, w, rpm (all in world frame) - p = o[:3] - q = np.array([o[6], o[3], o[4], o[5]]) # w, x, y, z - v = o[10:13] - w = o[13:16] # world frame - w_body = rotate_vector(w, qconjugate(q)) # local frame t = i/env.CTRL_FREQ - fdm_packet = struct.pack( - '@dddddddddddddddddd', # t, w, a, q, v, p, pressure - t, # double timestamp in seconds - # minus signs due to ENU to NED conversion - w_body[0], -w_body[1], -w_body[2], # double imu_angular_velocity_rpy[3] - 0, 0, 0, # double imu_linear_acceleration_xyz[3] - 1., .0, .0, 0., # double imu_orientation_quat[4] w, x, y, z - 0, 0, 0, # double velocity_xyz[3] - 0, 0, 0, # double position_xyz[3] - 1.0 # double pressure; - ) - sock.sendto(fdm_packet, (UDP_IP, 9003)) - - #### RC defaults to Betaflight ############################# - thro = 1000 # Positive up - yaw = 1500 # Positive CCW - pitch = 1500 # Positive forward in x - roll = 1500 # Positive right/forward in y - if t > TRAJ_TIME: + #### Step the simulation ################################### + obs, reward, terminated, truncated, info = env.step(action, i) + + if t > env.TRAJ_TIME: try: target = next(trajectory) + action[0,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, + state=obs[0], + target_pos=target["pos"], + target_vel=target["vel"] + ) except: break - - thro, roll, pitch, yaw = ctbr_controller( - target["pos"], - target["vel"], - # np.array([.0, .0, z]), - # np.array([.0, .0, vz]), - p, v, q - ) - thro, roll, pitch, yaw = ctbr2beta(thro, roll, pitch, yaw) - - #### RC message to Betaflight ############################## - aux1 = 1000 if t < ARM_TIME else 1500 # Arm - rc_packet = struct.pack( - '@dHHHHHHHHHHHHHHHH', - t, # datetime.now().timestamp(), # double timestamp; // in seconds - round(roll), round(pitch), round(thro), round(yaw), # roll, pitch, throttle, yaw - aux1, 1000, 1000, 1000, # aux 1, .. - 1000, 1000, 1000, 1000, - 1000, 1000, 1000, 1000 - ) - # print("rc", struct.unpack('@dHHHHHHHHHHHHHHHH', rc_packet)) - sock.sendto(rc_packet, (UDP_IP, 9004)) - - #### PWMs message from Betaflight ########################## - try: - data, addr = sock_pwm.recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) - except socket.error as msg: - # print(msg) - pass - else: - # print("received message: ", data) - betaflight_pwms = np.array(struct.unpack('@ffff', data)) - remapped_input = np.array([ - betaflight_pwms[2], # 0 - betaflight_pwms[1], # 1 - betaflight_pwms[3], # 2 - betaflight_pwms[0] # 3 - ]) # Betaflight SITL motor mapping - action = np.array([np.sqrt(env.MAX_THRUST / 4 / env.KF * remapped_input)]) + #### Log the simulation #################################### logger.log(drone=0, @@ -240,41 +150,6 @@ def run( if plot: logger.plot() -def ctbr_controller(tar_pos, tar_vel, cur_pos, cur_vel, cur_att): - G = np.array([.0, .0, -9.8]) - K_P = np.array([3., 3., 8.]) - K_D = np.array([2.5, 2.5, 5.]) - K_RATES = np.array([5., 5., 1.]) - P = tar_pos - cur_pos - D = tar_vel - cur_vel - tar_acc = K_P * P + K_D * D - G - norm_thrust = np.dot(tar_acc, rotate_vector([.0, .0, 1.], cur_att)) - # Calculate target attitude - z_body = normalized_vector(tar_acc) - x_body = normalized_vector(np.cross(np.array([.0, 1., .0]), z_body)) - y_body = normalized_vector(np.cross(z_body, x_body)) - tar_att = mat2quat(np.vstack([x_body, y_body, z_body]).T) - # Calculate body rates - q_error = qmult(qconjugate(cur_att), tar_att) - body_rates = 2 * K_RATES * q_error[1:] - if q_error[0] < 0: - body_rates = -body_rates - return norm_thrust, *body_rates - -def ctbr2beta(thrust, roll, pitch, yaw): - MIN_CHANNEL = 1000 - MAX_CHANNEL = 2000 - MAX_RATE = 360 - MAX_THRUST = 40.9 - mid = (MAX_CHANNEL + MIN_CHANNEL) / 2 - d = (MAX_CHANNEL - MIN_CHANNEL) / 2 - thrust = thrust / MAX_THRUST * d * 2 + MIN_CHANNEL - rates = np.array([roll, pitch, -yaw]) - rates = rates / np.pi * 180 / MAX_RATE * d + mid - thrust = np.clip(thrust, MIN_CHANNEL, MAX_CHANNEL) - rates = np.clip(rates, MIN_CHANNEL, MAX_CHANNEL) - return thrust, *rates - if __name__ == "__main__": #### Define and parse (optional) arguments for the script ## parser = argparse.ArgumentParser(description='Test flight script using SITL Betaflight') diff --git a/gym_pybullet_drones/examples/beta2.py b/gym_pybullet_drones/examples/beta2.py deleted file mode 100644 index 07384ba02..000000000 --- a/gym_pybullet_drones/examples/beta2.py +++ /dev/null @@ -1,176 +0,0 @@ -"""Control + Betaflight. - -Setup ------ -Step 1: Clone betaflight: - $ git clone https://github.com/betaflight/betaflight - $ cd betaflight/ - -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: Install betaflight configurator (https://github.com/betaflight/betaflight-configurator/releases): - $ wget https://github.com/betaflight/betaflight-configurator/releases/download/10.9.0/betaflight-configurator_10.9.0_amd64.deb - $ sudo dpkg -i betaflight-configurator_10.9.0_amd64.deb - If needed, also run: - $ apt install libgconf-2-4 - $ apt --fix-broken install - -Step 4: Load the configuration file onto the target using the BF configurator: - First, start the SITL controller: - $ ./obj/main/betaflight_SITL.elf - Type address `tcp://localhost:5761` (top right) and click `Connect` - Find the `Presets` tab (on the left) -> `Load backup` -> select file `../assets/beta.txt` - Restart `betaflight_SITL.elf` - -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 - -""" -import time -import argparse -import numpy as np -import csv - -from transforms3d.quaternions import rotate_vector, qconjugate, mat2quat, qmult -from transforms3d.utils import normalized_vector - -from gym_pybullet_drones.utils.enums import DroneModel, Physics -from gym_pybullet_drones.envs.BetaAviary import BetaAviary -from gym_pybullet_drones.control.CTBRControl import CTBRControl -from gym_pybullet_drones.utils.Logger import Logger -from gym_pybullet_drones.utils.utils import sync, str2bool - -DEFAULT_DRONES = DroneModel("racer") -DEFAULT_PHYSICS = Physics("pyb") -DEFAULT_GUI = True -DEFAULT_PLOT = True -DEFAULT_USER_DEBUG_GUI = False -DEFAULT_SIMULATION_FREQ_HZ = 500 -DEFAULT_CONTROL_FREQ_HZ = 500 -DEFAULT_DURATION_SEC = 20 -DEFAULT_OUTPUT_FOLDER = 'results' - -def run( - drone=DEFAULT_DRONES, - physics=DEFAULT_PHYSICS, - gui=DEFAULT_GUI, - plot=DEFAULT_PLOT, - 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 = BetaAviary(drone_model=drone, - num_drones=1, - initial_xyzs=np.array([[.0, .0, .1]]), - initial_rpys=np.array([[.0, .0, .0]]), - physics=physics, - pyb_freq=simulation_freq_hz, - ctrl_freq=control_freq_hz, - gui=gui, - user_debug_gui=user_debug_gui - ) - - ctrl = CTBRControl(drone_model=drone) - - #### Obtain the PyBullet Client ID from the environment #### - PYB_CLIENT = env.getPyBulletClient() - - #### Initialize the logger ################################# - logger = Logger(logging_freq_hz=control_freq_hz, - num_drones=1, - output_folder=output_folder, - ) - - #### Run the simulation #################################### - with open("../assets/beta.csv", mode='r') as csv_file: - csv_reader = csv.DictReader(csv_file) - trajectory = iter([{ - "pos": np.array([ - float(row["p_x"]), - float(row["p_y"]), - float(row["p_z"]), - ]), - "vel": np.array([ - float(row["v_x"]), - float(row["v_y"]), - float(row["v_z"]), - ]), - } for row in csv_reader]) - action = np.zeros((1,4)) - ARM_TIME = 1. - TRAJ_TIME = 1.5 - START = time.time() - for i in range(0, int(duration_sec*env.CTRL_FREQ)): - t = i/env.CTRL_FREQ - #### Step the simulation ################################### - obs, reward, terminated, truncated, info = env.step(action, i) - - if t > env.TRAJ_TIME: - try: - target = next(trajectory) - action[0,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, - state=obs[0], - target_pos=target["pos"], - target_vel=target["vel"] - ) - except: - break - - - #### Log the simulation #################################### - logger.log(drone=0, - timestamp=i/env.CTRL_FREQ, - state=obs[0] - ) - - #### Printout ############################################## - env.render() - - #### Sync the simulation ################################### - if gui: - pass - sync(i, START, env.CTRL_TIMESTEP) - - #### Close the environment ################################# - env.close() - - #### Save the simulation results ########################### - logger.save() - logger.save_as_csv("beta") # Optional CSV save - - #### Plot the simulation results ########################### - if plot: - logger.plot() - -if __name__ == "__main__": - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Test flight script using SITL Betaflight') - parser.add_argument('--drone', default=DEFAULT_DRONES, type=DroneModel, help='Drone model (default: BETA)', metavar='', choices=DroneModel) - parser.add_argument('--physics', default=DEFAULT_PHYSICS, type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) - 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('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') - ARGS = parser.parse_args() - - run(**vars(ARGS))