Fix force_torque2rotor_vel conversion

Author: amacati

drone_controllers/mellinger/control.py

@@ -186,8 +186,8 @@ def attitude2force_torque(
         prev_ang_vel: Previous angular velocity in rad/s.
         prev_ang_vel_des: Previous angular velocity command in rad/s.
         L: Distance from the center of the quadrotor to the center of the rotor in m.
-        KM: Torque constant in Nm/rad/s^2.
-        KF: Force constant in N/rad/s^2.
+        KM: Torque constant (Nm/RPM).
+        KF: Force constant (N/RPM).
         mixing_matrix: Mixing matrix for the motor forces with shape (4, 3).
 
     Returns:
@@ -252,14 +252,14 @@ def attitude2force_torque(
     # controllers within drone_models, we still want to return SI forces and torques. We thus need
     # to convert the legacy output to SI units.
     # l. 310 ff
-    torque_des = motor_forces @ mixing_matrix * xp.stack([L, L, KM / KF])
+    torque_des = (mixing_matrix @ motor_forces[..., None])[..., 0] * xp.stack([L, L, KM / KF])
     force_des = xp.sum(motor_forces, axis=-1)[..., None]
     return force_des, torque_des, r_int_error
 
 
 def force_torque_pwms2pwms(force_pwm: Array, torque_pwm: Array, mixing_matrix: Array) -> Array:
     """Convert desired collective thrust and torques to rotor speeds using legacy behavior."""
-    return force_pwm[..., None] + (mixing_matrix @ torque_pwm[..., None])[..., 0]
+    return force_pwm[..., None] + (torque_pwm @ mixing_matrix)
 
 
 @register_controller_parameters(ForceTorqueParams)
@@ -293,8 +293,8 @@ def force_torque2rotor_vel(
         thrust_min: Minimum thrust in N.
         thrust_max: Maximum thrust in N.
         L: Distance from the center of the quadrotor to the center of the rotor in m.
-        KM: Torque constant in Nm/rad/s^2.
-        KF: Force constant in N/rad/s^2.
+        KM: Torque constant (Nm/RPM).
+        KF: Force constant (N/RPM).
         mixing_matrix: Mixing matrix for the motor forces with shape (4, 3).
 
     Returns:
@@ -303,7 +303,7 @@ def force_torque2rotor_vel(
     xp = array_namespace(torque)
     assert torque.shape[-1] == 3, f"Torque must have shape (..., 3), but has {torque.shape}"
     assert force.shape[-1] == 1, f"Force must have shape (..., 1), but has {force.shape}"
-    torque_forces = (mixing_matrix @ (torque * xp.asarray([L, L, KM / KF]))[..., None])[..., 0]
+    torque_forces = (torque * xp.asarray([1 / L, 1 / L, KF / KM])) @ mixing_matrix
     motor_forces = (torque_forces + force) / 4
     # Clip motor forces on the thrust instead of PWM level.
     motor_forces = xp.where(xp.all(force == 0), 0.0, xp.clip(motor_forces, thrust_min, thrust_max))

drone_controllers/mellinger/params.toml

@@ -7,13 +7,12 @@ pwm_min = 20000.0
 pwm_max = 65535.0
 torque_pwm_max = [32000.0, 32000.0, 32000.0]
 L = 0.03253
-KF = 8.7e-10
+KF = 3.16e-10  # TODO: Check what the firmware uses
 KM = 7.94e-12
 mixing_matrix = [
-    [-1.0, -1.0, -1.0],
-    [-1.0,  1.0,  1.0],
-    [ 1.0,  1.0, -1.0],
-    [ 1.0, -1.0,  1.0]
+    [-1.0, -1.0, 1.0, 1.0],
+    [-1.0,  1.0,  1.0, -1.0],
+    [-1.0, 1.0,  -1.0, 1.0]
 ]
 gravity_vec = [0.0, 0.0, -9.81]