Add B500 model to so_rpy_rotor_drag model

Author: ratheron

drone_models/so_rpy_rotor_drag/model.py

@@ -37,14 +37,14 @@ def dynamics(
     J_inv: Array,
     KF: Array,
     KM: Array,
-    rotor_coef: Array,
+    thrust_time_coef: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
     rpy_coef: Array,
     rpy_rates_coef: Array,
     cmd_rpy_coef: Array,
     drag_linear_coef: Array,
-    drag_abs_linear_coef: Array,
+    drag_square_coef: Array,
 ) -> tuple[Array, Array, Array, Array, Array | None]:
     """The fitted double integrator (DI) model with optional motor delay (D).
 
@@ -66,14 +66,14 @@ def dynamics(
         J_inv: Inverse inertia matrix (1/kg m^2).
         KF: Motor force constant (N/rad^2).
         KM: Motor torque constant (Nm/rad^2).
-        rotor_coef: Coefficient for the rotor dynamics (1/s).
+        thrust_time_coef: Coefficient for the rotor dynamics (1/s).
         acc_coef: Coefficient for the acceleration (1/s^2).
         cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
         rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
         rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
         cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).
         drag_linear_coef: Coefficient for the linear drag (1/s).
-        drag_abs_linear_coef: Coefficient for the absolute linear drag (1/s).
+        drag_square_coef: Coefficient for the square drag (1/s).
 
     Returns:
         The derivatives of all state variables.
@@ -84,25 +84,31 @@ def dynamics(
     mass, gravity_vec, KF, KM, J, J_inv = to_xp(
         mass, gravity_vec, KF, KM, J, J_inv, xp=xp, device=device
     )
-    rotor_coef, acc_coef, cmd_f_coef = to_xp(rotor_coef, acc_coef, cmd_f_coef, xp=xp, device=device)
+    thrust_time_coef, acc_coef, cmd_f_coef = to_xp(
+        thrust_time_coef, acc_coef, cmd_f_coef, xp=xp, device=device
+    )
     rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
         rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device
     )
-    drag_linear_coef, drag_abs_linear_coef = to_xp(
-        drag_linear_coef, drag_abs_linear_coef, xp=xp, device=device
+    drag_linear_coef, drag_square_coef = to_xp(
+        drag_linear_coef, drag_square_coef, xp=xp, device=device
     )
     cmd_f = cmd[..., -1]
-    cmd_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
+    # cmd_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
+    cmd_rotor_vel = cmd_f  # this is just a hack for testing TODO remove
     cmd_rpy = cmd[..., 0:3]
     rot = R.from_quat(quat)
     euler_angles = rot.as_euler("xyz")
 
     if rotor_vel is None:
         rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
     else:
-        rotor_vel_dot = 1 / rotor_coef * (cmd_rotor_vel[..., None] - rotor_vel) - KM * rotor_vel**2
+        rotor_vel_dot = (
+            1 / thrust_time_coef * (cmd_rotor_vel[..., None] - rotor_vel)
+        )  # - KM * rotor_vel**2
 
     forces_motor = KF * xp.sum(rotor_vel**2, axis=-1)
+    forces_motor = rotor_vel[..., 0]
     thrust = acc_coef + cmd_f_coef * forces_motor
 
     drone_z_axis = rot.inv().as_matrix()[..., -1, :]
@@ -112,7 +118,7 @@ def dynamics(
         1 / mass * thrust[..., None] * drone_z_axis
         + gravity_vec
         + 1 / mass * drag_linear_coef * vel
-        + 1 / mass * drag_abs_linear_coef * vel * xp.abs(vel)
+        + 1 / mass * drag_square_coef * vel * xp.abs(vel)
     )
     if dist_f is not None:
         vel_dot = vel_dot + dist_f / mass
@@ -149,14 +155,14 @@ def symbolic_dynamics(
     J_inv: Array,
     KF: Array,
     KM: Array,
-    rotor_coef: Array,
+    thrust_time_coef: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
     rpy_coef: Array,
     rpy_rates_coef: Array,
     cmd_rpy_coef: Array,
     drag_linear_coef: Array,
-    drag_abs_linear_coef: Array,
+    drag_square_coef: Array,
 ) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
     """The fitted double integrator (DI) model with optional motor delay (D).
 
@@ -176,11 +182,15 @@ def symbolic_dynamics(
     # Defining the dynamics function
     if model_rotor_vel:
         # motor_force2rotor_vel
-        cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)
+        # cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)
+        cmd_rotor_vel = symbols.cmd_thrust  # this is just a hack for testing TODO remove
         rotor_vel_dot = (
-            1 / rotor_coef * (cmd_rotor_vel - symbols.rotor_vel) - KM * symbols.rotor_vel**2
+            1
+            / thrust_time_coef
+            * (cmd_rotor_vel - symbols.rotor_vel)  # - KM * symbols.rotor_vel**2
         )
-        forces_motor = KF * cs.sum1(symbols.rotor_vel**2)
+        # forces_motor = KF * cs.sum1(symbols.rotor_vel**2)
+        forces_motor = symbols.rotor_vel[0]
     else:
         forces_motor = symbols.cmd_thrust
     # Creating force vector
@@ -192,7 +202,7 @@ def symbolic_dynamics(
         symbols.rot @ forces_motor_vec / mass
         + gravity_vec
         + 1 / mass * drag_linear_coef * symbols.vel
-        + 1 / mass * drag_abs_linear_coef * symbols.vel * cs.fabs(symbols.vel)
+        + 1 / mass * drag_square_coef * symbols.vel * cs.fabs(symbols.vel)
     )
     if model_dist_f:
         # Adding force disturbances to the state

drone_models/so_rpy_rotor_drag/params.py

@@ -21,14 +21,14 @@ class SoRpyRotorDragParams(NamedTuple):
     J_inv: Array
     KF: Array
     KM: Array
-    rotor_coef: Array
+    thrust_time_coef: Array
     acc_coef: Array
     cmd_f_coef: Array
     rpy_coef: Array
     rpy_rates_coef: Array
     cmd_rpy_coef: Array
     drag_linear_coef: Array
-    drag_abs_linear_coef: Array
+    drag_square_coef: Array
 
     @staticmethod
     def load(drone_model: str) -> SoRpyRotorDragParams:

drone_models/so_rpy_rotor_drag/params.toml

@@ -1,58 +1,62 @@
 [cf2x_L250]
 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]
-]
+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
+thrust_time_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
+drag_square_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]
-]
+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
+thrust_time_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
+drag_square_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]
-]
+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
+thrust_time_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
+drag_square_coef = 0.0010
+
+
+[cf2x_B500]
+mass = 0.0433777
+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
+thrust_time_coef = 0.08238289
+acc_coef = 0.0
+cmd_f_coef = 0.99509171
+rpy_coef = [-160.5, -160.5, -109.3]
+rpy_rates_coef = [-10.74, -10.74, -13.69]
+cmd_rpy_coef = [115.2, 115.2, 154.6]
+drag_linear_coef = -0.02063486
+drag_square_coef = -0.02272605