Refactor the simulation. Add the attitude controller interface. Add d…

…ocker support. Add docs. Improve tests and tooling.

.dockerignore

@@ -0,0 +1,5 @@
+.git
+.github
+.vscode
+.pytest_cache
+**/*.pyc

.github/workflows/test_solution.yml

@@ -28,17 +28,11 @@ jobs:
           post-cleanup: 'all'
       - name: Install cffirmware
         run: |
-          git clone https://github.com/utiasDSL/pycffirmware.git
+          git clone -b drone_racing https://github.com/utiasDSL/pycffirmware.git
           cd pycffirmware
           git submodule update --init --recursive
           ./wrapper/build_linux.sh
-      - name: Install stable baselines
-        run: pip install stable-baselines3
-        shell: bash -el {0}
-      - name: Install stable baselines
-        run: pip install stable-baselines3
-        shell: bash -el {0}
-      - run: pip install .
+      - run: pip install .[rl, test]
         shell: bash -el {0}
       - name: Run simulation tests
         run: python scripts/kaggle.py

.readthedocs.yaml

@@ -15,4 +15,6 @@ sphinx:
 
 python:
   install:
-  - requirements: docs/requirements.txt
+  - requirements: docs/requirements.txt
+  - method: pip
+    path: .

README.md

@@ -28,7 +28,7 @@
 ## Installation
 
 To run the LSY Autonomous Drone Racing project, you will need 2 repositories:
-- [pycffirmware](https://github.com/utiasDSL/pycffirmware) - `main` branch: A simulator for the on-board controller response of the drones we are using to accurately model their behavior
+- [pycffirmware](https://github.com/utiasDSL/pycffirmware/tree/drone_racing) - `drone_racing` branch: A simulator for the on-board controller response of the drones we are using to accurately model their behavior.
 - [lsy_drone_racing](https://github.com/utiasDSL/lsy_drone_racing) - `main` branch: This repository contains the drone simulation, environments, and scripts to simulate and deploy the drones in the racing challenge
 
 ### Fork lsy_drone_racing
@@ -62,7 +62,7 @@ In addition, you also need to install the pycffirmware package from source with
 
 ```bash
 cd ~/repos
-git clone https://github.com/utiasDSL/pycffirmware.git
+git clone -b drone_racing https://github.com/utiasDSL/pycffirmware.git
 cd pycffirmware
 git submodule update --init --recursive
 sudo apt update
@@ -80,17 +80,20 @@ python scripts/sim.py
 
 If everything is installed correctly, this opens the simulator and simulates a drone flying through four gates.
 
+### Using Docker
+TODO: Add docker instructions
+
 ## Difficulty levels
-The complete problem is specified by a YAML file, e.g. [`getting_started.yaml`](config/getting_started.yaml)
+The complete problem is specified by a TOML file, e.g. [`level0.toml`](config/level0.toml)
 
 The config folder contains settings for progressively harder scenarios:
 
 |         Evaluation Scenario         | Constraints | Rand. Inertial Properties | Randomized Obstacles, Gates | Rand. Between Episodes |         Notes         |
 | :---------------------------------: | :---------: | :-----------------------: | :-------------------------: | :--------------------: | :-------------------: |
-| [`level0.yaml`](config/level0.yaml) |   **Yes**   |           *No*            |            *No*             |          *No*          |   Perfect knowledge   |
-| [`level1.yaml`](config/level1.yaml) |   **Yes**   |          **Yes**          |            *No*             |          *No*          |       Adaptive        |
-| [`level2.yaml`](config/level2.yaml) |   **Yes**   |          **Yes**          |           **Yes**           |          *No*          | Learning, re-planning |
-| [`level3.yaml`](config/level3.yaml) |   **Yes**   |          **Yes**          |           **Yes**           |        **Yes**         |      Robustness       |
+| [`level0.toml`](config/level0.toml) |   **Yes**   |           *No*            |            *No*             |          *No*          |   Perfect knowledge   |
+| [`level1.toml`](config/level1.toml) |   **Yes**   |          **Yes**          |            *No*             |          *No*          |       Adaptive        |
+| [`level2.toml`](config/level2.toml) |   **Yes**   |          **Yes**          |           **Yes**           |          *No*          | Learning, re-planning |
+| [`level3.toml`](config/level3.toml) |   **Yes**   |          **Yes**          |           **Yes**           |        **Yes**         |      Robustness       |
 |                                     |             |                           |                             |                        |                       |
 |              sim2real               |   **Yes**   |    Real-life hardware     |           **Yes**           |          *No*          |   Sim2real transfer   |
 
@@ -100,7 +103,7 @@ The config folder contains settings for progressively harder scenarios:
 You can choose which configuration to use by changing the `--config` command line option. To e.g. run the example controller on the hardest scenario, you can use the following command
 
 ```bash
-python scripts/sim.py --config config/level3.yaml
+python scripts/sim.py --config config/level3.toml
 ```
 
 ## The online competition
@@ -240,11 +243,19 @@ pip install cfclient
 conda deactivate
 ```
 
+### Vicon bridge
+TODO: Expand
+
 ### Common errors
 
 #### libNatNet
 If libNatNet is missing either during compiling crazyswarm or launching hover_swarm.launch, one option is to manually install it. Download the library from its [github repo](https://github.com/whoenig/NatNetSDKCrossplatform), follow the build instructions, and then add the library to your `LIBRARY_PATH` and `LD_LIBRARY_PATH` variables.
 
+#### LIBUSB_ERROR_ACCESS
+Change the USB access permissions with
+
+```sudo chmod -R 777 /dev/bus/usb/```
+
 ### Fly with the drones 
 
 #### Settings
@@ -274,7 +285,7 @@ roslaunch crazyswarm cf_sim2real.launch
 In a second terminal:
 
 ```bash
-python scripts/deploy.py --controller <path/to/your/controller.py> --config config/level3.yaml
+python scripts/deploy.py --controller <path/to/your/controller.py> --config config/level3.toml
 ```
 
 where `<path/to/your/controller.py>` implements a controller that inherits from `lsy_drone_racing.controller.BaseController`

benchmarks/config/test.toml

@@ -1,5 +1,5 @@
 [sim]
-physics = "pyb"                                   # pyb: PyBullet, dyn: Mathematical dynamics model, TODO: Complete the list
+physics = "pyb"
 camera_view = [5.0, -40.0, -40.0, 0.5, -1.0, 0.5]
 sim_freq = 500                                    # Simulation frequency, in Hz
 ctrl_freq = 500                                   # Controller frequency, in Hz. This frequency is used to simulate the onboard controller, NOT for the environment's step function

benchmarks/sim.py

@@ -7,7 +7,7 @@
 import numpy as np
 
 if TYPE_CHECKING:
-    import numpy.typing as npt
+    from numpy.typing import NDArray
 
 load_config_code = f"""
 from pathlib import Path
@@ -27,15 +27,15 @@
 """
 
 
-def time_sim_reset(n_tests: int = 10) -> npt.NDArray[np.floating]:
+def time_sim_reset(n_tests: int = 10) -> NDArray[np.floating]:
     setup = load_config_code + env_setup_code
     stmt = """env.reset()"""
     return np.array(timeit.repeat(stmt=stmt, setup=setup, number=1, repeat=n_tests))
 
 
 def time_sim_step(
     n_tests: int = 10, sim_steps: int = 100, physics_mode: str = "pyb"
-) -> npt.NDArray[np.floating]:
+) -> NDArray[np.floating]:
     modify_config_code = f"""config.sim.physics = '{physics_mode}'\n"""
     setup = load_config_code + modify_config_code + env_setup_code + "\nenv.reset()"
     stmt = f"""

config/level0.toml

@@ -0,0 +1,69 @@
+# Level 0
+
+# | Evaluation Scenario | Rand. Inertial Properties | Randomized Obstacles, Gates | Rand. Between Episodes | Notes             |
+# | :-----------------: | :-----------------------: | :-------------------------: | :--------------------: | :---------------: |
+# | `level0.toml`       | *No*                      | *No*                        | *No*                   | Perfect knowledge |
+
+[sim]
+# Physics options:
+# "pyb": PyBullet
+# "dyn": Mathematical dynamics model 
+# "pyb_gnd" PyBullet with ground effect
+# "pyb_drag": PyBullet with drag
+# "pyb_dw": PyBullet with downwash
+# "pyb_gnd_drag_dw": PyBullet with ground effect, drag, and downwash.
+physics = "pyb"
+
+camera_view = [5.0, -40.0, -40.0, 0.5, -1.0, 0.5]
+sim_freq = 500                                    # Simulation frequency, in Hz
+ctrl_freq = 500                                   # Controller frequency, in Hz. This frequency is used to simulate the onboard controller, NOT for the environment's step function
+gui = false                                       # Enable/disable PyBullet's GUI
+
+[sim.disturbances.action]
+type = "GaussianNoise"
+std = 0.001
+
+[sim.disturbances.dynamics]
+type = "UniformNoise"
+low = [-0.1, -0.1, -0.1]
+high = [0.1, 0.1, 0.1]
+
+[env]
+id = "DroneRacing-v0" # Either "DroneRacing-v0" or "DroneRacingThrust-v0". If using "DroneRacingThrust-v0", the drone will use the thrust controller instead of the position controller.
+reseed = true         # Whether to re-seed the random number generator between episodes
+seed = 1337           # Random seed
+freq = 30             # Frequency of the environment's step function, in Hz
+symbolic = false      # Whether to include symbolic expressions in the info dict. Note: This can interfere with multiprocessing! If you want to parallelize your training, set this to false.
+sensor_range = 0.45   # Range at which the exact location of gates and obstacles become visible to the drone. Objects that are not in the drone's sensor range report their nominal position.
+
+[env.track]
+# Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the
+# center of the gate.
+[[env.track.gates]]
+pos = [0.45, -1.0, 0.525]
+rpy = [0.0, 0.0, 2.35]
+[[env.track.gates]]
+pos = [1.0, -1.55, 1.0]
+rpy = [0.0, 0.0, -0.78]
+[[env.track.gates]]
+pos = [0.0, 0.5, 0.525]
+rpy = [0.0, 0.0, 0.0]
+[[env.track.gates]]
+pos = [-0.5, -0.5, 1.0]
+rpy = [0.0, 0.0, 3.14]
+
+# Obstacle height: 1.05m. Height is measured from the ground to the top of the obstacle.
+[[env.track.obstacles]]
+pos = [1.0, -0.5, 0.95]
+[[env.track.obstacles]]
+pos = [0.5, -1.5, 0.95]
+[[env.track.obstacles]]
+pos = [-0.5, 0.0, 0.95]
+[[env.track.obstacles]]
+pos = [0.0, 1.0, 0.95]
+
+[env.track.drone]
+pos = [1.0, 1.0, 0.05]
+rpy = [0, 0, 0]
+vel = [0, 0, 0]
+ang_vel = [0, 0, 0]