Fix and remove outdated tests

Author: amacati

benchmarks/main.py

@@ -25,12 +25,12 @@ def main(
         reset_fn, step_fn = time_multi_drone_reset, time_multi_drone_step
     if reset:
         timings = reset_fn(n_tests=n_tests, number=number)
-        print_benchmark_results(name="Sim reset", timings=timings / number)
+        print_benchmark_results(name="Racing env reset", timings=timings / number)
     if step:
         timings = step_fn(n_tests=n_tests, number=number)
-        print_benchmark_results(name="Sim steps", timings=timings / number)
+        print_benchmark_results(name="Racing env steps", timings=timings / number)
         timings = step_fn(n_tests=n_tests, number=number, physics_mode="sys_id")
-        print_benchmark_results(name="Sim steps (sys_id backend)", timings=timings / number)
+        print_benchmark_results(name="Racing env steps (sys_id backend)", timings=timings / number)
         # timings = step_fn(n_tests=n_tests, number=number, physics_mode="mujoco")
         # print_benchmark_results(name="Sim steps (mujoco backend)", timings=timings / number)
 

config/level0.toml

@@ -27,22 +27,13 @@ freq = 500                                        # Simulation frequency, in Hz
 attitude_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 "DroneRacingAttitude-v0". If using "DroneRacingAttitude-v0", the drone will use the attitude controller instead of the position controller.
-random_resets = false         # Whether to re-seed the random number generator between episodes
-seed = 1337           # Random seed
-freq = 50             # 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.
+id = "DroneRacing-v0"   # "DroneRacing-v0" for single races, "MultiDroneRacing-v0" for multi-drone races. Each environment also implements a vector-environment for faster training
+random_resets = false   # Whether to re-seed the random number generator between episodes
+seed = 1337             # Random seed
+freq = 50               # Frequency of the environment's step function, in Hz
+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.
+action_space = "state"  # Action space of the environment. Can be either "state" or "attitude"
 
 [env.track]
 # Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the
@@ -75,3 +66,13 @@ pos = [1.0, 1.0, 0.05]
 rpy = [0, 0, 0]
 vel = [0, 0, 0]
 ang_vel = [0, 0, 0]
+
+[env.disturbances.action]
+fn = "normal"
+scale = 0.001
+
+[env.disturbances.dynamics]
+fn = "uniform"
+[env.disturbances.dynamics.kwargs]
+minval = [-0.1, -0.1, -0.1]
+maxval = [0.1, 0.1, 0.1]

config/level1.toml

@@ -24,12 +24,12 @@ attitude_freq = 500                               # Controller frequency, in Hz.
 gui = false                                       # Enable/disable PyBullet's GUI
 
 [env]
-id = "DroneRacing-v0" # Either "DroneRacing-v0" or "DroneRacingAttitude-v0". If using "DroneRacingAttitude-v0", the drone will use the attitude controller instead of the position controller.
-random_resets = false         # Whether to re-seed the random number generator between episodes
-seed = 1337           # Random seed
-freq = 50             # 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.
+id = "DroneRacing-v0"   # "DroneRacing-v0" for single races, "MultiDroneRacing-v0" for multi-drone races. Each environment also implements a vector-environment for faster training
+random_resets = false   # Whether to re-seed the random number generator between episodes
+seed = 1337             # Random seed
+freq = 50               # Frequency of the environment's step function, in Hz
+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.
+action_space = "state"  # Action space of the environment. Can be either "state" or "attitude"
 
 [env.track]
 # Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the

config/level2.toml

@@ -24,12 +24,12 @@ attitude_freq = 500                               # Controller frequency, in Hz.
 gui = false                                       # Enable/disable PyBullet's GUI
 
 [env]
-id = "DroneRacing-v0" # Either "DroneRacing-v0" or "DroneRacingAttitude-v0". If using "DroneRacingAttitude-v0", the drone will use the attitude controller instead of the position controller.
-random_resets = false         # Whether to re-seed the random number generator between episodes
-seed = 1337           # Random seed
-freq = 50             # 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.
+id = "DroneRacing-v0"   # "DroneRacing-v0" for single races, "MultiDroneRacing-v0" for multi-drone races. Each environment also implements a vector-environment for faster training
+random_resets = false   # Whether to re-seed the random number generator between episodes
+seed = 1337             # Random seed
+freq = 50               # Frequency of the environment's step function, in Hz
+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.
+action_space = "state"  # Action space of the environment. Can be either "state" or "attitude"
 
 [env.track]
 # Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the

config/level3.toml

@@ -24,12 +24,12 @@ attitude_freq = 500                               # Controller frequency, in Hz.
 gui = false                                       # Enable/disable PyBullet's GUI
 
 [env]
-id = "DroneRacing-v0" # Either "DroneRacing-v0" or "DroneRacingAttitude-v0". If using "DroneRacingAttitude-v0", the drone will use the attitude controller instead of the position controller.
-random_resets = true        # Whether to re-seed the random number generator between episodes
-seed = 1337           # Random seed
-freq = 50             # 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.
+id = "DroneRacing-v0"   # "DroneRacing-v0" for single races, "MultiDroneRacing-v0" for multi-drone races. Each environment also implements a vector-environment for faster training
+random_resets = true    # Whether to re-seed the random number generator between episodes
+seed = 1337             # Random seed
+freq = 50               # Frequency of the environment's step function, in Hz
+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.
+action_space = "state"  # Action space of the environment. Can be either "state" or "attitude"
 
 [env.track]
 # Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the

config/multi_level0.toml

@@ -28,13 +28,13 @@ attitude_freq = 500                               # Controller frequency, in Hz.
 gui = false                                       # Enable/disable PyBullet's GUI
 
 [env]
-id = "MultiDroneRacing-v0" # Either "MultiDroneRacingEnv-v0" or "MultiDroneRacingAttitude-v0". If using "MultiDroneRacingAttitude-v0", the drone will use the attitude controller instead of the position controller.
-n_drones = 2          # Number of drones has to match the track configuration for drones
-random_resets = false # Whether to re-seed the random number generator between episodes
-seed = 1337           # Random seed
-freq = 50             # 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.
+id = "MultiDroneRacing-v0"
+n_drones = 2            # Number of drones has to match the track configuration for drones
+random_resets = false   # Whether to re-seed the random number generator between episodes
+seed = 1337             # Random seed
+freq = 50               # Frequency of the environment's step function, in Hz
+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.
+action_space = "state"  # Action space of the environment. Can be either "state" or "attitude"
 
 [env.track]
 # Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the

config/multi_level3.toml

@@ -28,13 +28,13 @@ attitude_freq = 500                               # Controller frequency, in Hz.
 gui = false                                       # Enable/disable PyBullet's GUI
 
 [env]
-id = "MultiDroneRacing-v0" # Either "MultiDroneRacingEnv-v0" or "MultiDroneRacingAttitude-v0". If using "MultiDroneRacingAttitude-v0", the drone will use the attitude controller instead of the position controller.
-n_drones = 2          # Number of drones has to match the track configuration for drones
-random_resets = true  # Whether to re-seed the random number generator between episodes
-seed = 1337           # Random seed
-freq = 50             # 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.
+id = "MultiDroneRacing-v0"
+n_drones = 2            # Number of drones has to match the track configuration for drones
+random_resets = true    # Whether to re-seed the random number generator between episodes
+seed = 1337             # Random seed
+freq = 50               # Frequency of the environment's step function, in Hz
+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.
+action_space = "state"  # Action space of the environment. Can be either "state" or "attitude"
 
 [env.track]
 # Tall gates: 1.0m height. Short gates: 0.525m height. Height is measured from the ground to the

lsy_drone_racing/control/thrust_controller.py lsy_drone_racing/control/attitude_controller.py

@@ -14,7 +14,7 @@
 from typing import TYPE_CHECKING
 
 import numpy as np
-import pybullet as p
+from crazyflow.constants import MASS
 from scipy.interpolate import CubicSpline
 from scipy.spatial.transform import Rotation as R
 
@@ -24,23 +24,23 @@
     from numpy.typing import NDArray
 
 
-class ThrustController(BaseController):
+class AttitudeController(BaseController):
     """Example of a controller using the collective thrust and attitude interface.
 
     Modified from https://github.com/utiasDSL/crazyswarm-import/blob/ad2f7ea987f458a504248a1754b124ba39fc2f21/ros_ws/src/crazyswarm/scripts/position_ctl_m.py
     """
 
-    def __init__(self, initial_obs: dict[str, NDArray[np.floating]], initial_info: dict):
+    def __init__(self, obs: dict[str, NDArray[np.floating]], info: dict, config: dict):
         """Initialization of the controller.
 
         Args:
-            initial_obs: The initial observation of the environment's state. See the environment's
+            obs: The initial observation of the environment's state. See the environment's
                 observation space for details.
-            initial_info: Additional environment information from the reset.
+            info: Additional environment information from the reset.
         """
-        super().__init__(initial_obs, initial_info)
-        self.low_level_ctrl_freq = initial_info["low_level_ctrl_freq"]
-        self.drone_mass = initial_info["drone_mass"]
+        super().__init__(obs, info, config)
+        self.freq = config.env.freq
+        self.drone_mass = MASS
         self.kp = np.array([0.4, 0.4, 1.25])
         self.ki = np.array([0.05, 0.05, 0.05])
         self.kd = np.array([0.2, 0.2, 0.4])
@@ -58,8 +58,8 @@ def __init__(self, initial_obs: dict[str, NDArray[np.floating]], initial_info: d
                 [0.2, -1.8, 0.65],
                 [1.1, -1.35, 1.1],
                 [0.2, 0.0, 0.65],
-                [0.0, 0.75, 0.525],
-                [0.0, 0.75, 1.1],
+                [0.0, 0.7, 0.525],
+                [0.0, 0.7, 1.1],
                 [-0.5, -0.5, 1.1],
                 [-0.5, -1.0, 1.1],
             ]
@@ -71,28 +71,12 @@ def __init__(self, initial_obs: dict[str, NDArray[np.floating]], initial_info: d
         cs_z = CubicSpline(ts, waypoints[:, 2])
 
         des_completion_time = 15
-        ts = np.linspace(0, 1, int(initial_info["env_freq"] * des_completion_time))
+        ts = np.linspace(0, 1, int(self.freq * des_completion_time))
 
         self.x_des = cs_x(ts)
         self.y_des = cs_y(ts)
         self.z_des = cs_z(ts)
 
-        try:
-            # Draw interpolated Trajectory. Limit segments to avoid excessive drawings
-            stride = max(1, len(self.x_des) // 100)
-            trajectory = np.vstack([self.x_des, self.y_des, self.z_des])[..., ::stride].T
-            for i in range(len(trajectory) - 1):
-                p.addUserDebugLine(
-                    trajectory[i],
-                    trajectory[i + 1],
-                    lineColorRGB=[1, 0, 0],
-                    lineWidth=2,
-                    lifeTime=0,
-                    physicsClientId=0,
-                )
-        except p.error:
-            ...  # Ignore pybullet errors if not running in the pybullet GUI
-
     def compute_control(
         self, obs: dict[str, NDArray[np.floating]], info: dict | None = None
     ) -> NDArray[np.floating]:
@@ -116,7 +100,7 @@ def compute_control(
         vel_error = des_vel - obs["vel"]
 
         # Update integral error
-        self.i_error += pos_error * (1 / self.low_level_ctrl_freq)
+        self.i_error += pos_error * (1 / self.freq)
         self.i_error = np.clip(self.i_error, -self.ki_range, self.ki_range)
 
         # Compute target thrust

lsy_drone_racing/envs/drone_race.py

@@ -8,7 +8,7 @@
 from gymnasium.vector import VectorEnv
 from gymnasium.vector.utils import batch_space
 
-from lsy_drone_racing.envs.race_core import RaceCoreEnv, action_space, observation_space
+from lsy_drone_racing.envs.race_core import RaceCoreEnv, build_action_space, build_observation_space
 
 if TYPE_CHECKING:
     import numpy as np
@@ -22,6 +22,7 @@ def __init__(
         freq: int,
         sim_config: ConfigDict,
         sensor_range: float,
+        action_space: Literal["state", "attitude"] = "state",
         track: ConfigDict | None = None,
         disturbances: ConfigDict | None = None,
         randomizations: ConfigDict | None = None,
@@ -36,6 +37,7 @@ def __init__(
             freq=freq,
             sim_config=sim_config,
             sensor_range=sensor_range,
+            action_space=action_space,
             track=track,
             disturbances=disturbances,
             randomizations=randomizations,
@@ -44,9 +46,9 @@ def __init__(
             max_episode_steps=max_episode_steps,
             device=device,
         )
-        self.action_space = action_space("state")
+        self.action_space = build_action_space(action_space)
         n_gates, n_obstacles = len(track.gates), len(track.obstacles)
-        self.observation_space = observation_space(n_gates, n_obstacles)
+        self.observation_space = build_observation_space(n_gates, n_obstacles)
         self.autoreset = False
 
     def reset(self, seed: int | None = None, options: dict | None = None) -> tuple[dict, dict]:
@@ -69,6 +71,7 @@ def __init__(
         freq: int,
         sim_config: ConfigDict,
         sensor_range: float,
+        action_space: Literal["state", "attitude"] = "state",
         track: ConfigDict | None = None,
         disturbances: ConfigDict | None = None,
         randomizations: ConfigDict | None = None,
@@ -83,6 +86,7 @@ def __init__(
             freq=freq,
             sim_config=sim_config,
             sensor_range=sensor_range,
+            action_space=action_space,
             track=track,
             disturbances=disturbances,
             randomizations=randomizations,
@@ -91,10 +95,10 @@ def __init__(
             max_episode_steps=max_episode_steps,
             device=device,
         )
-        self.single_action_space = action_space("state")
+        self.single_action_space = build_action_space(action_space)
         self.action_space = batch_space(self.single_action_space, num_envs)
         n_gates, n_obstacles = len(track.gates), len(track.obstacles)
-        self.single_observation_space = observation_space(n_gates, n_obstacles)
+        self.single_observation_space = build_observation_space(n_gates, n_obstacles)
         self.observation_space = batch_space(self.single_observation_space, num_envs)
 
     def reset(self, seed: int | None = None, options: dict | None = None) -> tuple[dict, dict]: