Add model parameters for P250 and T350

amacati · amacati · commit fbf4cc8ae3ba · 2025-08-24T14:13:33.000+02:00
diff --git a/drone_models/core.py b/drone_models/core.py
@@ -89,7 +89,9 @@ def parametrize(
         if xp is not None:  # Convert to any array API framework
             params = named_tuple2xp(params, xp=xp, device=device)
     except KeyError as e:
-        raise KeyError(f"Model `{model_id}` does not exist in the parameter registry") from e
+        raise KeyError(
+            f"Model `{model_id}` does not exist in the parameter registry for drone `{drone_model}`"
+        ) from e
     except ValueError as e:
         raise ValueError(f"Drone model `{drone_model}` not supported for `{model_id}`") from e
     return partial(fn, **params._asdict())
diff --git a/drone_models/drones.py b/drone_models/drones.py
@@ -3,4 +3,4 @@
 TODO: Add description.
 """
 
-available_drones: tuple[str] = ("cf2x_L250",)
+available_drones: tuple[str] = ("cf2x_L250", "cf2x_P250", "cf2x_T350")
diff --git a/drone_models/first_principles/params.py b/drone_models/first_principles/params.py
@@ -15,15 +15,15 @@
 class FirstPrinciplesParams(NamedTuple):
     """Parameters for the FirstPrinciples model."""
 
-    thrust_tau: float
+    mass: float
+    gravity_vec: Array
+    J: Array
+    J_inv: Array
     KF: float
     KM: float
     L: float
     mixing_matrix: Array
-    gravity_vec: Array
-    mass: float
-    J: Array
-    J_inv: Array
+    thrust_tau: float
 
     @staticmethod
     def load(drone_model: str) -> FirstPrinciplesParams:
diff --git a/drone_models/first_principles/params.toml b/drone_models/first_principles/params.toml
@@ -1,5 +1,11 @@
 [cf2x_L250]
-thrust_tau = 6.9235846332510365
+mass = 0.033
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.68e-5, 0.0, 0.0],
+    [0.0, 1.68e-5, 0.0],
+    [0.0, 0.0, 2.98e-5]
+]
 KF = 8.701227710666256e-10
 KM = 7.94e-12
 L = 0.03253
@@ -9,10 +15,43 @@ mixing_matrix = [
     [ 1.0,  1.0, -1.0],
     [ 1.0, -1.0,  1.0]
 ]
+thrust_tau = 6.9235846332510365
+
+[cf2x_P250]
+mass = 0.03454
 gravity_vec = [0.0, 0.0, -9.81]
-mass = 0.033
+# J from IROS 2022
 J = [
-    [1.68e-5, 0.0, 0.0],
-    [0.0, 1.68e-5, 0.0],
-    [0.0, 0.0, 2.98e-5]
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+KF = 5.792654323957372e-10
+KM = 7.94e-12
+L = 0.03253
+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]
+]
+thrust_tau = 7.2333515424212695
+
+[cf2x_T350]
+mass = 0.03454
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+KF = 1.068340683985525e-09
+KM = 7.94e-12
+L = 0.03253
+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]
 ]
+thrust_tau = 8.719306881898989
diff --git a/drone_models/so_rpy/params.py b/drone_models/so_rpy/params.py
@@ -17,13 +17,13 @@ class SoRpyParams(NamedTuple):
 
     mass: float
     gravity_vec: Array
+    J: Array
+    J_inv: Array
     acc_coef: Array
     cmd_f_coef: Array
     rpy_coef: Array
     rpy_rates_coef: Array
     cmd_rpy_coef: Array
-    J: Array
-    J_inv: Array
 
     @staticmethod
     def load(drone_model: str) -> SoRpyParams:
diff --git a/drone_models/so_rpy/params.toml b/drone_models/so_rpy/params.toml
@@ -11,3 +11,33 @@ J = [
     [0.0, 1.68e-5, 0.0],
     [0.0, 0.0, 2.98e-5]
 ]
+
+# TODO: Check if parameters are correct
+[cf2x_P250]
+mass = 0.03454
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+acc_coef = 20.907574256269616  
+cmd_f_coef = 3.653687545690674
+rpy_coef = [-99.94, -130.3, 0.0]
+rpy_rates_coef = [-13.3, -16.33, 0.0]
+cmd_rpy_coef = [84.73, 119.3, 0.0]
+
+# TODO: Check if parameters are correct
+[cf2x_T350]
+mass = 0.03454
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+acc_coef = 20.907574256269616
+cmd_f_coef = 3.653687545690674
+rpy_coef = [-99.94, -130.3, 0.0]
+rpy_rates_coef = [-13.3, -16.33, 0.0]
+cmd_rpy_coef = [84.73, 119.3, 0.0]
diff --git a/drone_models/so_rpy_rotor/params.py b/drone_models/so_rpy_rotor/params.py
@@ -17,10 +17,10 @@ class SoRpyRotorParams(NamedTuple):
 
     mass: float
     gravity_vec: Array
-    KF: Array
-    KM: Array
     J: Array
     J_inv: Array
+    KF: Array
+    KM: Array
     rotor_coef: Array
     acc_coef: Array
     cmd_f_coef: Array
diff --git a/drone_models/so_rpy_rotor/params.toml b/drone_models/so_rpy_rotor/params.toml
@@ -1,16 +1,52 @@
 [cf2x_L250]
 mass = 0.033
 gravity_vec = [0.0, 0.0, -9.81]
-KF = 8.701227710666256e-10
-KM = 7.94e-12
 J = [
     [1.68e-5, 0.0, 0.0],
     [0.0, 1.68e-5, 0.0],
     [0.0, 0.0, 2.98e-5]
 ]
+KF = 8.701227710666256e-10
+KM = 7.94e-12
 rotor_coef = 0.1108
 acc_coef = 0.0
 cmd_f_coef = 1.0591
 rpy_coef = [-257.2, -257.2, -225.3]
 rpy_rates_coef = [-20.2, -20.2, -28.61]
-cmd_rpy_coef = [207.1, 207.1, 392.7]
+cmd_rpy_coef = [207.1, 207.1, 392.7]
+
+# TODO: Check if parameters are correct
+[cf2x_P250]
+mass = 0.03454
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+KF = 5.792654323957372e-10
+KM = 7.94e-12
+rotor_coef = 0.0654
+acc_coef = 0.1670
+cmd_f_coef = 0.5615
+rpy_coef = [-286.2, -286.2, -192.9]
+rpy_rates_coef = [-23.03, -23.03, -22.22]
+cmd_rpy_coef = [225.6, 225.6, 323.5]
+
+# TODO: Check if parameters are correct
+[cf2x_T350]
+mass = 0.03253
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+KF = 1.068340683985525e-09
+KM = 7.94e-12
+rotor_coef = 0.0654
+acc_coef = 0.1670
+cmd_f_coef = 0.5615
+rpy_coef = [-286.2, -286.2, -192.9]
+rpy_rates_coef = [-23.03, -23.03, -22.22]
+cmd_rpy_coef = [225.6, 225.6, 323.5]
diff --git a/drone_models/so_rpy_rotor_drag/params.py b/drone_models/so_rpy_rotor_drag/params.py
@@ -17,10 +17,10 @@ class SoRpyRotorDragParams(NamedTuple):
 
     mass: float
     gravity_vec: Array
-    KF: Array
-    KM: Array
     J: Array
     J_inv: Array
+    KF: Array
+    KM: Array
     rotor_coef: Array
     acc_coef: Array
     cmd_f_coef: Array
diff --git a/drone_models/so_rpy_rotor_drag/params.toml b/drone_models/so_rpy_rotor_drag/params.toml
@@ -1,13 +1,13 @@
 [cf2x_L250]
 mass = 0.033
 gravity_vec = [0.0, 0.0, -9.81]
-KF = 8.701227710666256e-10
-KM = 7.94e-12
 J = [
     [1.68e-5, 0.0, 0.0],
     [0.0, 1.68e-5, 0.0],
     [0.0, 0.0, 2.98e-5]
 ]
+KF = 8.701227710666256e-10
+KM = 7.94e-12
 rotor_coef = 0.1891
 acc_coef = 0.0
 cmd_f_coef = 1.0627
@@ -16,3 +16,43 @@ rpy_rates_coef = [-20.2, -20.2, -28.61]
 cmd_rpy_coef = [207.1, 207.1, 392.7]
 drag_linear_coef = -0.0177
 drag_abs_linear_coef = 0.0010
+
+# TODO: Check if parameters are correct
+[cf2x_P250]
+mass = 0.03454
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+KF = 5.792654323957372e-10
+KM = 7.94e-12
+rotor_coef = 0.1891
+acc_coef = 0.0
+cmd_f_coef = 1.0627
+rpy_coef = [-257.2, -257.2, -225.3]
+rpy_rates_coef = [-20.2, -20.2, -28.61]
+cmd_rpy_coef = [207.1, 207.1, 392.7]
+drag_linear_coef = -0.0177
+drag_abs_linear_coef = 0.0010
+
+# TODO: Check if parameters are correct
+[cf2x_T350]
+mass = 0.03253
+gravity_vec = [0.0, 0.0, -9.81]
+J = [
+    [1.4e-5, 0.0,    0.0],
+    [0.0, 1.4e-5, 0.0],
+    [0.0, 0.0, 2.17e-5]
+]
+KF = 1.068340683985525e-09
+KM = 7.94e-12
+rotor_coef = 0.1891
+acc_coef = 0.0
+cmd_f_coef = 1.0627
+rpy_coef = [-257.2, -257.2, -225.3]
+rpy_rates_coef = [-20.2, -20.2, -28.61]
+cmd_rpy_coef = [207.1, 207.1, 392.7]
+drag_linear_coef = -0.0177
+drag_abs_linear_coef = 0.0010
