added hover script to test force estimator

config/level3.toml

@@ -9,11 +9,11 @@ file = "trajectory_controller.py" # Put your controller file name here. Specifyi
 [deploy]
 ### Settings only relevant for deployment
 # Whether to check if gate and obstacle positions observed by vicon are within the limits defined down below.
-check_race_track = true
+check_race_track = false
 # Whether to check if the drone start position is within the limits specified down below.
 check_drone_start_pos = true
 # Lets you practice your controller without putting up gates & obstacles, assumes nominal positions given below.
-practice_without_track_objects = false
+practice_without_track_objects = true
 
 [sim]
 # Physics options:

lsy_drone_racing/control/thrust_controller_hover.py

@@ -0,0 +1,160 @@
+"""This module implements a ThrustController for quadrotor control.
+
+It utilizes the collective thrust interface for drone control to compute control commands based on
+current state observations and desired waypoints. The attitude control is handled by computing a
+PID control law for position tracking, incorporating gravity compensation in thrust calculations.
+
+The waypoints are generated using cubic spline interpolation from a set of predefined waypoints.
+Note that the trajectory uses pre-defined waypoints instead of dynamically generating a good path.
+"""
+
+from __future__ import annotations  # Python 3.10 type hints
+
+import math
+from typing import TYPE_CHECKING
+
+import numpy as np
+import pybullet as p
+from scipy.interpolate import CubicSpline
+from scipy.spatial.transform import Rotation as R
+
+from lsy_drone_racing.control import BaseController
+
+if TYPE_CHECKING:
+    from numpy.typing import NDArray
+
+
+class ThrustController(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):
+        """Initialization of the controller.
+
+        Args:
+            initial_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.
+        """
+        super().__init__(initial_obs, initial_info)
+        self.low_level_ctrl_freq = initial_info["low_level_ctrl_freq"]
+        self.drone_mass = initial_info["drone_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])
+        self.ki_range = np.array([2.0, 2.0, 0.4])
+        self.i_error = np.zeros(3)
+        self.g = 9.81
+        self._tick = 0
+
+        # Same waypoints as in the trajectory controller. Determined by trial and error.
+        waypoints = np.array(
+            [
+                [1.0, 1.0, 0.05],
+                [1.0, 1.0, 1.0],
+                [1.0, 1.0, 1.0],
+                [1.0, 1.0, 1.0],
+                [1.0, 1.0, 1.0],
+            ]
+        )
+        # Scale trajectory between 0 and 1
+        ts = np.linspace(0, 1, np.shape(waypoints)[0])
+        cs_x = CubicSpline(ts, waypoints[:, 0])
+        cs_y = CubicSpline(ts, waypoints[:, 1])
+        cs_z = CubicSpline(ts, waypoints[:, 2])
+
+        des_completion_time = 30
+        ts = np.linspace(0, 1, int(initial_info["env_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]:
+        """Compute the next desired collective thrust and roll/pitch/yaw of the drone.
+
+        Args:
+            obs: The current observation of the environment. See the environment's observation space
+                for details.
+            info: Optional additional information as a dictionary.
+
+        Returns:
+            The collective thrust and orientation [t_des, r_des, p_des, y_des] as a numpy array.
+        """
+        i = min(self._tick, len(self.x_des) - 1)
+        des_pos = np.array([self.x_des[i], self.y_des[i], self.z_des[i]])
+        des_vel = np.zeros(3)
+        des_yaw = 0.0
+
+        # Calculate the deviations from the desired trajectory
+        pos_error = des_pos - obs["pos"]
+        vel_error = des_vel - obs["vel"]
+
+        # Update integral error
+        self.i_error += pos_error * (1 / self.low_level_ctrl_freq)
+        self.i_error = np.clip(self.i_error, -self.ki_range, self.ki_range)
+
+        # Compute target thrust
+        target_thrust = np.zeros(3)
+        target_thrust += self.kp * pos_error
+        target_thrust += self.ki * self.i_error
+        target_thrust += self.kd * vel_error
+        # target_thrust += params.quad.m * desired_acc
+        target_thrust[2] += self.drone_mass * self.g
+
+        # Update z_axis to the current orientation of the drone
+        z_axis = R.from_euler("xyz", obs["rpy"]).as_matrix()[:, 2]
+
+        # update current thrust
+        thrust_desired = target_thrust.dot(z_axis)
+        thrust_desired = max(thrust_desired, 0.3 * self.drone_mass * self.g)
+        thrust_desired = min(thrust_desired, 1.8 * self.drone_mass * self.g)
+
+        # update z_axis_desired
+        z_axis_desired = target_thrust / np.linalg.norm(target_thrust)
+        x_c_des = np.array([math.cos(des_yaw), math.sin(des_yaw), 0.0])
+        y_axis_desired = np.cross(z_axis_desired, x_c_des)
+        y_axis_desired /= np.linalg.norm(y_axis_desired)
+        x_axis_desired = np.cross(y_axis_desired, z_axis_desired)
+
+        R_desired = np.vstack([x_axis_desired, y_axis_desired, z_axis_desired]).T
+        euler_desired = R.from_matrix(R_desired).as_euler("xyz", degrees=False)
+        thrust_desired, euler_desired
+        return np.concatenate([[thrust_desired], euler_desired])
+
+    def step_callback(
+        self,
+        action: NDArray[np.floating],
+        obs: dict[str, NDArray[np.floating]],
+        reward: float,
+        terminated: bool,
+        truncated: bool,
+        info: dict,
+    ):
+        """Increment the tick counter."""
+        self._tick += 1
+
+    def episode_callback(self):
+        """Reset the integral error."""
+        self.i_error[:] = 0
+        self._tick = 0

lsy_drone_racing/utils/disturbance_observer.py

@@ -201,33 +201,52 @@ def __init__(self, dt: np.floating, model_name: str = "analytical_mel_att", init
         self._state = self._UKF.x
 
         # Set process noise covariance (tunable). Uncertainty in the dynamics. High Q -> less trust in model
-        # Q_x = Q_discrete_white_noise(dim=2, dt=dt, var=1e-9, block_size=6, order_by_dim=False)
-        # Q_f = np.eye(3)*1e-6 # Q_discrete_white_noise(dim=3, dt=dt, var=1e-6, block_size=1, order_by_dim=False)
-        # Q_t = np.eye(3)*1e-6 # Q_discrete_white_noise(dim=3, dt=dt, var=1e-6, block_size=1, order_by_dim=False)
-        # self._UKF.Q = block_diag(Q_x, Q_f, Q_t) # np.eye(18)*1e-6 # 
-        # self._UKF.Q[12:15, 12:15] = self._UKF.Q[12:15, 12:15] * 1e1 # Force
-        # self._UKF.Q[15:18, 15:18] = self._UKF.Q[15:18, 15:18] * 1e1 # Torque
-
-        self._varQ = 1e-2
-        self._varR = 1e-3
-
-        # self._UKF.Q = Q_discrete_white_noise(dim=3, dt=self._dt, var=self._varQ, block_size=6, order_by_dim=False) # TODO manually setup matrix
-        # self._UKF.Q[12:15, 12:15] = self._UKF.Q[12:15, 12:15] * 5e0 # Force
-        # self._UKF.Q[15:18, 15:18] = self._UKF.Q[15:18, 15:18] * 5e0 # Torque
-        # self._UKF.Q = np.eye(18)*1e-9
-        # This way, pos, vel, and force (or rpy, angular vel, and torque) influence each other
-        # print(self._UKF.Q.tolist()) 
-
-        Q_p = np.eye(3)*self._varQ*1e-0
-        Q_a = np.eye(3)*self._varQ*1e-0
-        Q_v = np.eye(3)*self._varQ*1e-0
-        Q_w = np.eye(3)*self._varQ*1e-0
-        Q_f = np.eye(3)*self._varQ*1e-0
-        Q_t = np.eye(3)*self._varQ*1e-0
-        self._UKF.Q = block_diag(Q_p, Q_a, Q_v, Q_w, Q_f, Q_t)
+        self._UKF.Q = np.eye(dim_x)
+        # self._varQ = 1e-10
+        # Q_x = Q_discrete_white_noise(dim=2, dt=dt, var=self._varQ, block_size=6, order_by_dim=False)
+        # Q_f = np.eye(3)*1e0 # Q_discrete_white_noise(dim=3, dt=dt, var=1e-6, block_size=1, order_by_dim=False)
+        # Q_t = np.eye(3)*1e0 # Q_discrete_white_noise(dim=3, dt=dt, var=1e-6, block_size=1, order_by_dim=False)
+        # self._KF.Q = block_diag(Q_x, Q_f, Q_t) # np.eye(18)*1e-6 # 
+        # self._KF.Q[12:15, 12:15] = self._KF.Q[12:15, 12:15] * 1e1 # Force
+        # self._KF.Q[15:18, 15:18] = self._KF.Q[15:18, 15:18] * 1e1 # Torque
+
+        # self._varQ = 1e-1
+        # self._KF.Q = Q_discrete_white_noise(dim=3, dt=self._dt, var=self._varQ, block_size=dim_x//3, order_by_dim=False) # TODO manually setup matrix
+
+        # See sketch for more info on how the matrix is set up
+        Q_xyz = Q_discrete_white_noise(dim=2, dt=self._dt, var=1e-1, block_size=3, order_by_dim=False) # pos & vel
+        # plt.matshow(Q_xyz)
+        # plt.show()
+        self._UKF.Q[0:3,0:3] = Q_xyz[0:3,0:3] # pos
+        self._UKF.Q[6:9,6:9] = Q_xyz[3:6,3:6] # vel
+        self._UKF.Q[0:3,6:9] = Q_xyz[0:3,3:6] # pos <-> vel
+        self._UKF.Q[6:9,0:3] = Q_xyz[3:6,0:3] # pos <-> vel
+
+        Q_rpy = Q_discrete_white_noise(dim=2, dt=self._dt, var=1e-0, block_size=3, order_by_dim=False) # rpy & rpy rates
+        # plt.matshow(Q_rpy)
+        # plt.show()
+        self._UKF.Q[3:6,3:6] = Q_rpy[0:3,0:3] # rpy
+        self._UKF.Q[9:12,9:12] = Q_rpy[3:6,3:6] # rpy rates
+        self._UKF.Q[3:6,9:12] = Q_rpy[0:3,3:6] # rpy <-> rpy rates
+        self._UKF.Q[9:12,3:6] = Q_rpy[3:6,0:3] # rpy <-> rpy rates
+
+
+
+        # self._KF.Q[0:3] = self._KF.Q[0:3] * 1e-1 # pos
+        # self._KF.Q[3:6] = self._KF.Q[3:6] * 1e-1 # rpy
+        # self._KF.Q[6:9] = self._KF.Q[6:9] * 1e-1 # vel
+        # self._KF.Q[9:12] = self._KF.Q[9:12] * 1e-1 # rpy rate
+
+        if dim_x > 12:
+            self._UKF.Q[12:15, 12:15] *= 1e-5 # Force
+            self._UKF.Q[15:18, 15:18] *= 1e-9 # Torque
 
         # Set measurement noise covariance (tunable). Uncertaints in the measurements. High R -> less trust in measurements
-        self._UKF.R = np.eye(self._obs_dim) * self._varR
+        self._UKF.R = np.eye(self._obs_dim)
+        # very low noise on the position ("mm precision" => even less noise)
+        self._UKF.R[:3, :3] = self._UKF.R[:3, :3] * 1e-8
+        # "high" measurements noise on the angles, estimate: 0.01 constains all values => std=3e-3
+        self._UKF.R[3:, 3:] = self._UKF.R[3:, 3:] * 3e-3 # 1e-5 works quite well, but bias!
 
         # Initialize state and covariance
         if initial_obs is not None:

lsy_drone_racing/vicon.py

@@ -18,7 +18,7 @@
 import numpy as np
 import rospy
 import yaml
-from crazyswarm.msg import StateVector
+from vicon_bridge.msg import StateVector, Command
 from rosgraph import Master
 from scipy.spatial.transform import Rotation as R
 from tf2_msgs.msg import TFMessage
@@ -71,6 +71,11 @@ def __init__(
                 "/estimated_state", StateVector, self.estimator_callback
             )
 
+        self.control_pub = rospy.Publisher("/controller_command",
+                                            Command,
+                                            queue_size=1,
+                                        )
+
         if timeout:
             tstart = time.time()
             while not self.active and time.time() - tstart < timeout: