Update thrust dynamics model and add intertia/gyro torque

Author: ratheron

drone_models/data/params.toml

@@ -2,25 +2,29 @@
 
 [cf2x_L250]
 gravity_vec = [0.0, 0.0, -9.81]
-mass = 0.03454
+mass = 0.0319
 L = 0.03253
-J = [ # TODO
+J = [  # TODO
     [16.8e-6, 0.0, 0.0],
     [0.0, 16.8e-6, 0.0],
     [0.0, 0.0, 29.8e-6]
 ]
 rpm2thrust = [0.0, -5.382196214637237e-7, 2.4582929831265485e-10]
 rpm2torque = [0.0, 1.410454111996297e-9, 1.4592584373980652e-12] 
 thrust2torque = 0.007350862856566459
-# TODO: This is an arbitrary number that leads to a working simulation. It MUST be fitted and
-# updated as soon as possible.
-thrust_tau = 0.03 # TODO value?
-rotor_tau = 0.03 # TODO value?
+rotor_dyn_coef = [ 7.355623702172756, 0.0, 0.0, 0.00024443862952110715,] # [ 7.355623702172756, 0.0, 0.0, 0.00024443862952110715,]
+rotor_dyn_coef_simple = 6.886705423469015
+thrust_dyn_coef = 6.8932095506763345
 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]
 ]
+drag_matrix = [  # This term is from the so_rpy_rotor_drag model
+    [-0.01471782, 0.0,         0.0         ],
+    [0.0,         -0.01471782, 0.0         ],
+    [0.0,         0.0,         -0.01277641 ]
+]
 # The following parameters are for the platform, but are maybe not actually used by the models. However, 
 # we still keep them here in one place, since some other things (sim, estimator, firmware) might need them.
 pwm_min = 7000
@@ -36,23 +40,29 @@ prop_inertia = 34.52e-9 # TODO seems off
 
 [cf2x_P250]
 gravity_vec = [0.0, 0.0, -9.81]
-mass = 0.03454
+mass = 0.0318
 L = 0.03253
-J = [ # TODO
-    [14e-6, 0.0,    0.0],
-    [0.0, 14e-6, 0.0],
-    [0.0, 0.0, 21.7e-6]
+J = [  # TODO from L250
+    [16.8e-6, 0.0, 0.0],
+    [0.0, 16.8e-6, 0.0],
+    [0.0, 0.0, 29.8e-6]
 ]
 rpm2thrust = [0.0, -3.6200226530383495e-7, 1.6060924304100328e-10] # Index is order
 rpm2torque = [0.0, -2.2665265829562245e-9, 1.1149485566919186e-12] # Index is order
 thrust2torque = 0.0069928948992470565
-thrust_tau = 0.138 # TODO value?
-rotor_tau = 0.138 # TODO value?
+rotor_dyn_coef = [ 5.172596691828673, 8.14774234381285e-5, 0.0, 0.0002095253491455741,]
+rotor_dyn_coef_simple = 7.709730027690284
+thrust_dyn_coef = 7.9435775497736785
 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]
 ]
+drag_matrix = [  # This term is from the so_rpy_rotor_drag model
+    [-0.01351483, 0.0,         0.0         ],
+    [0.0,         -0.01351483, 0.0         ],
+    [0.0,         0.0,         -0.01677452 ]
+]
 # The following parameters are for the platform, but are maybe not actually used by the models. However, 
 # we still keep them here in one place, since some other things (sim, estimator, firmware) might need them.
 pwm_min = 7000
@@ -68,23 +78,29 @@ prop_inertia = 26.97e-9
 
 [cf2x_T350]
 gravity_vec = [0.0, 0.0, -9.81]
-mass = 0.037 # TODO
+mass = 0.0379
 L = 0.03253
-J = [ # TODO
-    [14e-6, 0.0,   0.0    ],
-    [0.0,   14e-6, 0.0    ],
-    [0.0,   0.0,   21.7e-6]
+J = [  # TODO from L250
+    [16.8e-6, 0.0, 0.0],
+    [0.0, 16.8e-6, 0.0],
+    [0.0, 0.0, 29.8e-6]
 ]
 rpm2thrust = [0.0, -7.167227176573658e-7, 2.9401303690194613e-10] # Index is order
 rpm2torque = [0.0, 5.815894847811497e-10, 1.331813874166509e-12] # Index is order
 thrust2torque = 0.005355990836477486
-thrust_tau = 0.09625268508006131
-rotor_tau = 0.0984319234122739
+rotor_dyn_coef = [ 11.374753209400291, 0.0, 0.0, 0.00037867688499079635,]
+rotor_dyn_coef_simple = 11.352970450445243
+thrust_dyn_coef = 11.12424272978587
 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]
 ]
+drag_matrix = [  # This term is from the so_rpy_rotor_drag model
+    [-0.01556697, 0.0,         0.0         ],
+    [0.0,         -0.01556697, 0.0         ],
+    [0.0,         0.0,         -0.02191672 ]
+]
 # The following parameters are for the platform, but are maybe not actually used by the models. However, 
 # we still keep them here in one place, since some other things (sim, estimator, firmware) might need them.
 pwm_min = 7000
@@ -110,13 +126,19 @@ J = [
 rpm2thrust = [0.0, -3.133427287299859e-7, 4.407354891648379e-10] # TODO , Index is order
 rpm2torque = [0.0, 1.65886356219615e-9, 2.4693477924534137e-12] # TODO , Index is order
 thrust2torque = 0.00593893393599368
-thrust_tau = 0.06286705869479182
-rotor_tau = 0.06082197967926632
+rotor_dyn_coef = [ 13.996001897562685, 0.00011093207920685363, 5.933168530682111, 0.00031951312393561264,]
+rotor_dyn_coef_simple = 15.416891997523813
+thrust_dyn_coef = 15.09965949800411
 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]
 ]
+drag_matrix = [  # This term is from the so_rpy_rotor_drag model
+    [-0.02149163, 0.0,          0.0         ],
+    [0.0,         -0.02149163,  0.0         ],
+    [0.0,         0.0,          -0.02359736 ]
+]
 # The following parameters are for the platform, but are maybe not actually used by the models. However, 
 # we still keep them here in one place, since some other things (sim, estimator, firmware) might need them.
 pwm_min = 7000

drone_models/first_principles/model.py

@@ -28,14 +28,16 @@ def dynamics(
     dist_t: Array | None = None,
     *,
     mass: float,
+    L: float,
+    prop_inertia: float,
     gravity_vec: Array,
     J: Array,
     J_inv: Array,
     rpm2thrust: Array,
     rpm2torque: Array,
-    L: float,
     mixing_matrix: Array,
-    rotor_tau: float,
+    rotor_dyn_coef: Array,
+    drag_matrix: Array,
 ) -> tuple[Array, Array, Array, Array, Array | None]:
     r"""First principles model for a quatrotor.
 
@@ -63,7 +65,7 @@ def dynamics(
         rpm2torque: Propeller torque constant (Nm min^2).
         L: Distance from the CoM to the motor (m).
         mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
-        rotor_tau: Thrust time constant (s).
+        rotor_dyn_coef: Rotor dynamics coefficients.
 
     .. math::
         \sum_{i=1}^{\\infty} x_{i} TODO
@@ -74,41 +76,57 @@ def dynamics(
         More information https://ahrs.readthedocs.io/en/latest/filters/angular.html
     """
     xp = array_namespace(pos)
-    mass, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, L, mixing_matrix, rotor_tau = to_xp(
-        mass,
-        gravity_vec,
-        J,
-        J_inv,
-        rpm2thrust,
-        rpm2torque,
-        L,
-        mixing_matrix,
-        rotor_tau,
-        xp=xp,
-        device=device(pos),
+    mass, L, prop_inertia, gravity_vec, rpm2thrust, rpm2torque = to_xp(
+        mass, L, prop_inertia, gravity_vec, rpm2thrust, rpm2torque, xp=xp, device=device(pos)
+    )
+    J, J_inv, mixing_matrix, rotor_dyn_coef, drag_matrix = to_xp(
+        J, J_inv, mixing_matrix, rotor_dyn_coef, drag_matrix, xp=xp, device=device(pos)
     )
-    rot = R.from_quat(quat)
+    rot = R.from_quat(quat)  # from body to world
+    rot_mat = rot.inv().as_matrix()  # from world to body
     # Rotor dynamics
     if rotor_vel is None:
         rotor_vel, rotor_vel_dot = cmd, None
     else:
-        rotor_vel_dot = 1 / rotor_tau * (cmd - rotor_vel)  # - 1 / KM * rotor_vel**2
+        rotor_vel_dot = xp.where(
+            cmd > rotor_vel,
+            rotor_dyn_coef[0] * (cmd - rotor_vel) + rotor_dyn_coef[1] * (cmd**2 - rotor_vel**2),
+            rotor_dyn_coef[2] * (cmd - rotor_vel) + rotor_dyn_coef[3] * (cmd**2 - rotor_vel**2),
+        )
     # Creating force and torque vector
     forces_motor = rpm2thrust[0] + rpm2thrust[1] * rotor_vel + rpm2thrust[2] * rotor_vel**2
-    torques_motor = rpm2torque[0] + rpm2torque[1] * rotor_vel + rpm2torque[2] * rotor_vel**2
     forces_motor_tot = xp.sum(forces_motor, axis=-1)
     zeros = xp.zeros_like(forces_motor_tot)
     forces_motor_vec = xp.stack((zeros, zeros, forces_motor_tot), axis=-1)
+    forces_motor_vec_world = rot.apply(forces_motor_vec)
+    force_gravity = gravity_vec * mass
+    force_drag = (rot_mat.mT @ (drag_matrix @ (rot_mat @ vel[..., None])))[..., 0]
 
-    torque_vec = (mixing_matrix @ (forces_motor)[..., None])[..., 0] * xp.stack(
+    torques_motor = rpm2torque[0] + rpm2torque[1] * rotor_vel + rpm2torque[2] * rotor_vel**2
+    torque_thrust = (mixing_matrix @ (forces_motor)[..., None])[..., 0] * xp.stack(
         [L, L, xp.asarray(0.0)]
-    ) + (mixing_matrix @ (torques_motor)[..., None])[..., 0] * xp.stack(
+    )
+    torque_drag = (mixing_matrix @ (torques_motor)[..., None])[..., 0] * xp.stack(
         [xp.asarray(0.0), xp.asarray(0.0), xp.asarray(1.0)]
-    )  # TODO add gyro etc
+    )
+    # convert rotor speed from RPM to rad/s for physical calculations
+    rpm_to_rad = 2 * xp.pi / 60
+    rotor_vel_rads = rotor_vel * rpm_to_rad
+    rotor_vel_dot_rads = (
+        rotor_vel_dot * rpm_to_rad if rotor_vel_dot is not None else xp.zeros_like(rotor_vel)
+    )
+    torque_inertia = prop_inertia * xp.stack(
+        [
+            -ang_vel[..., 1] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1),
+            -ang_vel[..., 0] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1),
+            xp.sum(mixing_matrix[..., -1, :] * rotor_vel_dot_rads, axis=-1),
+        ],
+        axis=-1,
+    )
+    torque_vec = torque_thrust + torque_drag + torque_inertia
 
     # Linear equation of motion
-    forces_motor_vec_world = rot.apply(forces_motor_vec)
-    forces_sum = forces_motor_vec_world + gravity_vec * mass  # TODO add drag etc
+    forces_sum = forces_motor_vec_world + force_gravity + force_drag
     if dist_f is not None:
         forces_sum = forces_sum + dist_f
 
@@ -130,14 +148,16 @@ def symbolic_dynamics(
     model_dist_t: bool = False,
     *,
     mass: float,
+    L: float,
+    prop_inertia: float,
     gravity_vec: Array,
     J: Array,
     J_inv: Array,
     rpm2thrust: Array,
     rpm2torque: Array,
-    L: float,
     mixing_matrix: Array,
-    rotor_tau: float,
+    rotor_dyn_coef: Array,
+    drag_matrix: Array,
 ) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
     """TODO take from numeric."""
     # States and Inputs
@@ -152,27 +172,43 @@ def symbolic_dynamics(
 
     # Defining the dynamics function
     if model_rotor_vel:
-        # Thrust dynamics
-        rotor_vel_dot = 1 / rotor_tau * (U - symbols.rotor_vel)  # - 1 / KM * symbols.rotor_vel**2
-        forces_motor = (
-            rpm2thrust[0] + rpm2thrust[1] * symbols.rotor_vel + rpm2thrust[2] * symbols.rotor_vel**2
-        )
-        torques_motor = (
-            rpm2torque[0] + rpm2torque[1] * symbols.rotor_vel + rpm2torque[2] * symbols.rotor_vel**2
+        # Rotor dynamics
+        rotor_vel_dot = cs.if_else(
+            U > symbols.rotor_vel,
+            rotor_dyn_coef[0] * (U - symbols.rotor_vel)
+            + rotor_dyn_coef[1] * (U**2 - symbols.rotor_vel**2),
+            rotor_dyn_coef[2] * (U - symbols.rotor_vel)
+            + rotor_dyn_coef[3] * (U**2 - symbols.rotor_vel**2),
         )
     else:
-        forces_motor = rpm2thrust[0] + rpm2thrust[1] * U + rpm2thrust[2] * U**2
-        torques_motor = rpm2torque[0] + rpm2torque[1] * U + rpm2torque[2] * U**2
-
+        symbols.rotor_vel = U
     # Creating force and torque vector
+    forces_motor = (
+        rpm2thrust[0] + rpm2thrust[1] * symbols.rotor_vel + rpm2thrust[2] * symbols.rotor_vel**2
+    )
     forces_motor_vec = cs.vertcat(0.0, 0.0, cs.sum1(forces_motor))
-    torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(
-        L, L, 0.0
-    ) + mixing_matrix @ torques_motor * cs.vertcat(0.0, 0.0, 1.0)
+    forces_motor_vec_world = symbols.rot @ forces_motor_vec
+    force_gravity = gravity_vec * mass
+    force_drag = symbols.rot @ (drag_matrix @ (symbols.rot.T @ symbols.vel))
+
+    torques_motor = (
+        rpm2torque[0] + rpm2torque[1] * symbols.rotor_vel + rpm2torque[2] * symbols.rotor_vel**2
+    )
+    torques_thrust = mixing_matrix @ forces_motor * cs.vertcat(L, L, 0.0)
+    torques_drag = mixing_matrix @ torques_motor * cs.vertcat(0.0, 0.0, 1.0)
+    # convert rotor speed from RPM to rad/s for physical calculations
+    rpm_to_rad = 2 * cs.pi / 60
+    rotor_vel_rads = symbols.rotor_vel * rpm_to_rad
+    rotor_vel_dot_rads = rotor_vel_dot * rpm_to_rad if model_rotor_vel else symbols.rotor_vel * 0.0
+    torque_inertia = prop_inertia * cs.vertcat(
+        -symbols.ang_vel[1] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads),
+        -symbols.ang_vel[0] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads),
+        cs.sum(mixing_matrix[-1, :] * rotor_vel_dot_rads),
+    )
+    torques_motor_vec = torques_thrust + torques_drag + torque_inertia
 
     # Linear equation of motion
-    forces_motor_vec_world = symbols.rot @ forces_motor_vec
-    forces_sum = forces_motor_vec_world + gravity_vec * mass
+    forces_sum = forces_motor_vec_world + force_gravity + force_drag
     if model_dist_f:
         forces_sum = forces_sum + symbols.dist_f