diff --git a/.gitignore b/.gitignore index d2316598d..98af37ec3 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,9 @@ tmp/ # Learning results experiments/learning/results/save-* +# Betaflight +betaflight_sitl/ + # macOS users .DS_Store diff --git a/README.md b/README.md index 2c512fbc8..c3829e2a9 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,7 @@ In another terminal, run the example ```sh conda activate drones cd gym_pybullet_drones/examples/ -python3 beta.py # also check the steps in the file's docstrings +python3 beta.py --num_drones 1 # also check the steps in the file's docstrings to use multiple drones ``` ## Troubleshooting diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index 2e393e794..454c80c5c 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -3,63 +3,72 @@ # Clone, edit, build, configure, multiple Betaflight SITL executables # Check for the correct number of command-line arguments -if [ "$#" -ne 2 ]; then - echo "Usage: $0 " +if [ "$#" -ne 1 ]; then + echo "Usage: $0 " exit 1 fi # Extract command-line arguments -gpd_base_path="$1" -num_iterations="$2" +desired_max_num_drones="$1" -# Create directory along gym-pybullet-donres -cd $gpd_base_path -cd .. -mkdir betaflights/ -cd betaflights/ +# Create gitignored directory in gym-pybullet-donres +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +cd $SCRIPT_DIR +cd ../../ +mkdir betaflight_sitl/ +cd betaflight_sitl/ -pattern0="delayMicroseconds_real(50); // max rate 20kHz" -pattern1="#define PORT_PWM_RAW 9001 // Out" -pattern2="#define PORT_PWM 9002 // Out" -pattern3="#define PORT_STATE 9003 // In" -pattern4="#define PORT_RC 9004 // In" -# pattern5="#define BASE_PORT 5760" -pattern6="ret = udpInit(&stateLink, NULL, 9003, true);" +# Step 1: Clone and open betaflight's source: +git clone https://github.com/betaflight/betaflight temp/ -replacement0="// delayMicroseconds_real(50); // max rate 20kHz" -replacement6="ret = udpInit(&stateLink, NULL, PORT_STATE, true);" -for ((i = 1; i <= num_iterations; i++)); do +# 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` +cd temp/ +sed -i "s/delayMicroseconds_real(50);/\/\/delayMicroseconds_real(50);/g" ./src/main/main.c +sed -i "s/ret = udpInit(\&stateLink, NULL, 9003, true);/\/\/ret = udpInit(\&stateLink, NULL, PORT_STATE, true);/g" ./src/main/target/SITL/sitl.c +sed -i "s/printf(\"start UDP server.../\/\/printf(\"start UDP server.../g" ./src/main/target/SITL/sitl.c - # Clone - git clone https://github.com/betaflight/betaflight "bf${i}" - cd "bf${i}/" +# Prepare +make arm_sdk_install - # Edit - sed -i "s/$pattern0/$replacement0/g" ./src/main/target/SITL/sitl.c +cd .. - replacement1="#define PORT_PWM_RAW 90${i}1 // Out" - sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c +pattern1="PORT_PWM_RAW 9001" +pattern2="PORT_PWM 9002" +pattern3="PORT_STATE 9003" +pattern4="PORT_RC 9004" +# pattern5="BASE_PORT 5760" - replacement2="#define PORT_PWM 90${i}2 // Out" - sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c +for ((i = 0; i < desired_max_num_drones; i++)); do - replacement3="#define PORT_STATE 90${i}3 // In" - sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c + # Copy + cp -r temp/ "bf${i}/" + cd "bf${i}/" - replacement4="#define PORT_PWPORT_RCM_RAW 90${i}4 // In" + # Step 3: Change the UDP ports used by each Betaflight SITL instancet + replacement1="PORT_PWM_RAW 90${i}1" + sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c + replacement2="PORT_PWM 90${i}2" + sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c + replacement3="PORT_STATE 90${i}3" + sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c + replacement4="PORT_RC 90${i}4" sed -i "s/$pattern4/$replacement4/g" ./src/main/target/SITL/sitl.c - - # replacement5="#define BASE_PORT 57${i}0" + # replacement5="BASE_PORT 57${i}0" # sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c - sed -i "s/$pattern6/$replacement6/g" ./src/main/target/SITL/sitl.c - # Build - make arm_sdk_install - make TARGET=SITL + make TARGET=SITL + + cd .. + +done + +for ((i = 0; i < desired_max_num_drones; i++)); do - # Copy configured memory - cp "${gpd_base_path}/gym_pybullet_drones/assets/eeprom.bin" . + # Step 4: Copy over the configured `eeprom.bin` file from folder + cp "${SCRIPT_DIR}/eeprom.bin" "bf${i}/" done diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index 7500371e8..8d822262d 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -2,29 +2,23 @@ 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/ +Use script `gym_pybullet_drones/assets/clone_bfs.sh` to create +executables for as many drones as needed (e.g. 2): + $ ./gym_pybullet_drones/assets/clone_bfs.sh 2 Example ------- -In one terminal run the SITL Betaflight: +Run as many SITL Betaflight as drones in the simulation +in separate terminals (navigate the each `bf0`, `bf1`, etc. folder first): - $ cd betaflight/ + $ cd betaflights/bf0/ $ ./obj/main/betaflight_SITL.elf + $ cd betaflights/bf1/ + $ ./obj/main/betaflight_SITL.elf + + $ .. + In a separate terminal, run: $ cd gym-pybullet-drones/gym_pybullet_drones/examples/ @@ -54,12 +48,11 @@ DEFAULT_CONTROL_FREQ_HZ = 500 DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' -NUM_DRONES = 2 -INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,NUM_DRONES+1)]) -INIT_RPY = np.array([[.0, .0, .0] for _ in range(NUM_DRONES)]) +DEFAULT_NUM_DRONES = 2 def run( drone=DEFAULT_DRONES, + num_drones=DEFAULT_NUM_DRONES, physics=DEFAULT_PHYSICS, gui=DEFAULT_GUI, plot=DEFAULT_PLOT, @@ -70,8 +63,10 @@ def run( output_folder=DEFAULT_OUTPUT_FOLDER, ): #### Create the environment with or without video capture ## + INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,num_drones+1)]) + INIT_RPY = np.array([[.0, .0, .0] for _ in range(num_drones)]) env = BetaAviary(drone_model=drone, - num_drones=NUM_DRONES, + num_drones=num_drones, initial_xyzs=INIT_XYZ, initial_rpys=INIT_RPY, physics=physics, @@ -88,7 +83,7 @@ def run( #### Initialize the logger ################################# logger = Logger(logging_freq_hz=control_freq_hz, - num_drones=NUM_DRONES, + num_drones=num_drones, output_folder=output_folder, ) @@ -121,7 +116,7 @@ def run( float(row["v_z"]), ]), } for row in csv_reader])) - action = np.zeros((NUM_DRONES,4)) + action = np.zeros((num_drones,4)) ARM_TIME = 1. TRAJ_TIME = 1.5 START = time.time() @@ -131,7 +126,7 @@ def run( obs, reward, terminated, truncated, info = env.step(action, i) if t > env.TRAJ_TIME: - for j in range(NUM_DRONES): + for j in range(num_drones): try: target = next(trajectory1) if j%2 == 0 else next(trajectory2) action[j,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, @@ -144,7 +139,7 @@ def run( #### Log the simulation #################################### - for j in range(NUM_DRONES): + for j in range(num_drones): logger.log(drone=j, timestamp=i/env.CTRL_FREQ, state=obs[j] @@ -173,6 +168,7 @@ def run( #### 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('--num_drones', default=DEFAULT_NUM_DRONES, type=int, help='Number of drones (default: 3)', metavar='') 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='')