Add thrust deploy interface. Fix kaggle deploy script

Author: amacati

lsy_drone_racing/envs/drone_racing_deploy_env.py

@@ -222,17 +222,14 @@ def step(
     ) -> tuple[dict[str, NDArray[np.floating]], float, bool, bool, dict]:
         """Take a step in the environment.
 
-        Todo:
-            Implement the hardware interface for collective thrust and attitude control.
-
         Note:
             Sleeps for the remaining time if the step took less than the control period. This
             ensures that the environment is running at the correct frequency during deployment.
         """
-        raise NotImplementedError("Hardware interface for thrust control not yet implemented.")
         tstart = time.perf_counter()
-        collective_thrust, attitude = action
-        self.cf.cmdAttitudeThrust(collective_thrust, attitude)
+        collective_thrust, rpy = action
+        rpy_deg = np.rad2deg(rpy)
+        self.cf.cmdVel(*rpy_deg, collective_thrust)
         if (dt := time.perf_counter() - tstart) < 1 / self.config.env.freq:
             rospy.sleep(1 / self.config.env.freq - dt)
         return self.obs, -1.0, False, False, self.info

scripts/kaggle.py

@@ -4,19 +4,25 @@
     Please do not alter this script or ask the course supervisors first!
 """
 
+
 import logging
+from pathlib import Path
 
 import pandas as pd
 from sim import simulate
 
+from lsy_drone_racing.utils import load_config
+
 logger = logging.getLogger(__name__)
 
 
 def main():
     """Run the simulation N times and save the results as 'submission.csv'."""
     n_runs = 10
-    ctrl_path = "lsy_drone_racing/control/" + "trajectory_controller.py"  # TODO: Load dynamically
-    ep_times = simulate(config="config/level3.toml", controller=ctrl_path, n_runs=n_runs, gui=False)
+    config = load_config(Path(__file__).parents[1] / "config/level3.toml")
+    ep_times = simulate(
+        config="level3.toml", controller=config.controller.file, n_runs=n_runs, gui=False
+    )
 
     # Log the number of failed runs if any
     if failed := [x for x in ep_times if x is None]: