Update so_rpy_rotor models

Author: ratheron

drone_models/data/params.toml

@@ -1,29 +1,30 @@
 # This file contains all the **physical** parameters for all models.
 
 [cf2x_L250]
-mass = 0.033
 gravity_vec = [0.0, 0.0, -9.81]
+mass = 0.033
+L = 0.03253
 J = [
     [1.68e-5, 0.0, 0.0],
     [0.0, 1.68e-5, 0.0],
     [0.0, 0.0, 2.98e-5]
 ]
 rpm2thrust = [0.0, 0.0, 3.16e-10] # TODO
 rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
-L = 0.03253
+# 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? TODO rename to rotor_tau
 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]
 ]
-# 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
 
 
 [cf2x_P250]
-mass = 0.03454
 gravity_vec = [0.0, 0.0, -9.81]
+mass = 0.03454
+L = 0.03253
 # J from IROS 2022
 J = [
     [1.4e-5, 0.0,    0.0],
@@ -32,48 +33,48 @@ J = [
 ]
 rpm2thrust = [0.0, 0.0, 5.792654323957372e-10] # TODO
 rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
-L = 0.03253
+thrust_tau = 7.2333515424212695 # TODO value? TODO rename to rotor_tau
 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]
+mass = 0.03454
+L = 0.03253
 J = [
     [1.4e-5, 0.0,    0.0],
     [0.0, 1.4e-5, 0.0],
     [0.0, 0.0, 2.17e-5]
 ]
 rpm2thrust = [0.0, 0.0, 1.068340683985525e-09] # TODO
 rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
-L = 0.03253
+thrust_tau = 8.719306881898989 # TODO value? TODO rename to rotor_tau
 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
 
 
 [cf21B_500]
-mass = 0.04338
 gravity_vec = [0.0, 0.0, -9.81]
+mass = 0.04338
+L = 0.035355
 J = [
     [2.5e-5, 0.0,    0.0],
     [0.0,    2.8e-5, 0.0],
     [0.0,    0.0,    4.9e-5]
 ]
 rpm2thrust = [0.0, -8.357406697944084e-07, 4.716909960828207e-10]
 rpm2torque = [0.0, 2.435151264350409e-09, 2.424629050055297e-12]
-L = 0.035355
+thrust2torque = 0.00593893393599368
+thrust_tau = 0.08 # TODO value? TODO rename to rotor_tau
 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 = 0.03 # TODO
+]

drone_models/so_rpy/params.toml

@@ -1,19 +1,13 @@
 # TODO: Double-check yaw parameters. Currently induces weird behavior in sim
 [cf2x_L250]
-mass = 0.03454
-gravity_vec = [0.0, 0.0, -9.81]
 acc_coef = 0.0
 cmd_f_coef = 1.0633
 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]
-J = [[1.68e-5, 0.0, 0.0], [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 = 0.0
 cmd_f_coef = 1.0633
 rpy_coef = [-99.94, -130.3, 0.0]
@@ -22,9 +16,6 @@ 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 = 0.0
 cmd_f_coef = 1.0633
 rpy_coef = [-99.94, -130.3, 0.0]
@@ -33,9 +24,6 @@ cmd_rpy_coef = [84.73, 119.3, 0.0]
 
 # TODO: Check if parameters are correct
 [cf2x_B500]
-mass = 0.043
-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 = 0.0
 cmd_f_coef = 1.0633
 rpy_coef = [-99.94, -130.3, 0.0]

drone_models/so_rpy_rotor/model.py

@@ -11,7 +11,6 @@
 
 import drone_models.symbols as symbols
 from drone_models.core import supports
-from drone_models.transform import motor_force2rotor_vel
 from drone_models.utils import rotation, to_xp
 
 if TYPE_CHECKING:
@@ -33,9 +32,7 @@ def dynamics(
     gravity_vec: Array,
     J: Array,
     J_inv: Array,
-    KF: Array,
-    KM: Array,
-    rotor_coef: Array,
+    thrust_time_coef: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
     rpy_coef: Array,
@@ -60,9 +57,7 @@ def dynamics(
             [0, 0, -9.81].
         J: Inertia matrix (kg m^2).
         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).
@@ -75,26 +70,26 @@ def dynamics(
     xp = array_namespace(pos)
     # Convert constants to the correct framework and device
     device = xp_device(pos)
-    mass, gravity_vec, KF, KM, J, J_inv = to_xp(
-        mass, gravity_vec, KF, KM, J, J_inv, xp=xp, device=device
+    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, 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
     )
-    rotor_coef, acc_coef, cmd_f_coef = to_xp(rotor_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
     )
 
     cmd_f = cmd[..., -1]
-    cmd_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
     cmd_rpy = cmd[..., 0:3]
     rot = R.from_quat(quat)
     euler_angles = rot.as_euler("xyz")
 
+    # Note that we are abusing the rotor_vel state as the thrust
     if rotor_vel is None:
-        rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
+        rotor_vel, rotor_vel_dot = cmd_f[..., 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_f[..., None] - rotor_vel)
 
-    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.as_matrix()[..., -1]
@@ -135,9 +130,7 @@ def symbolic_dynamics(
     gravity_vec: Array,
     J: Array,
     J_inv: Array,
-    KF: Array,
-    KM: Array,
-    rotor_coef: Array,
+    thrust_time_coef: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
     rpy_coef: Array,
@@ -160,13 +153,18 @@ def symbolic_dynamics(
     cmd_rpy = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
 
     # Defining the dynamics function
+    # Note that we are abusing the rotor_vel state as the thrust
     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

drone_models/so_rpy_rotor/params.toml

@@ -1,14 +1,5 @@
 [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]
-]
-KF = 8.701227710666256e-10
-KM = 7.94e-12
-rotor_coef = 0.1108
+thrust_time_coef = 0.1108
 acc_coef = 0.0
 cmd_f_coef = 1.0591
 rpy_coef = [-257.2, -257.2, -225.3]
@@ -17,16 +8,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
+thrust_time_coef = 0.0654
 acc_coef = 0.1670
 cmd_f_coef = 0.5615
 rpy_coef = [-286.2, -286.2, -192.9]
@@ -35,16 +17,7 @@ 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
+thrust_time_coef = 0.0654
 acc_coef = 0.1670
 cmd_f_coef = 0.5615
 rpy_coef = [-286.2, -286.2, -192.9]

drone_models/so_rpy_rotor_drag/model.py

@@ -32,8 +32,6 @@ def dynamics(
     gravity_vec: Array,
     J: Array,
     J_inv: Array,
-    KF: Array,
-    KM: Array,
     thrust_time_coef: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
@@ -61,8 +59,6 @@ def dynamics(
             [0, 0, -9.81].
         J: Inertia matrix (kg m^2).
         J_inv: Inverse inertia matrix (1/kg m^2).
-        KF: Motor force constant (N/rad^2).
-        KM: Motor torque constant (Nm/rad^2).
         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).
@@ -78,9 +74,7 @@ def dynamics(
     xp = array_namespace(pos)
     device = xp_device(pos)
     # Convert constants to the correct framework and device
-    mass, gravity_vec, KF, KM, J, J_inv = to_xp(
-        mass, gravity_vec, KF, KM, J, J_inv, xp=xp, device=device
-    )
+    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, 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
     )
@@ -91,20 +85,16 @@ def dynamics(
         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 = 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")
 
+    # Note that we are abusing the rotor_vel state as the thrust
     if rotor_vel is None:
-        rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
+        rotor_vel, rotor_vel_dot = cmd_f[..., None], None
     else:
-        rotor_vel_dot = (
-            1 / thrust_time_coef * (cmd_rotor_vel[..., None] - rotor_vel)
-        )  # - KM * rotor_vel**2
+        rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)
 
-    # forces_motor = KF * xp.sum(rotor_vel**2, axis=-1)
     forces_motor = rotor_vel[..., 0]
     thrust = acc_coef + cmd_f_coef * forces_motor
 
@@ -149,8 +139,6 @@ def symbolic_dynamics(
     gravity_vec: Array,
     J: Array,
     J_inv: Array,
-    KF: Array,
-    KM: Array,
     thrust_time_coef: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
@@ -176,6 +164,7 @@ def symbolic_dynamics(
     cmd_rpy = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
 
     # Defining the dynamics function
+    # Note that we are abusing the rotor_vel state as the thrust
     if model_rotor_vel:
         # motor_force2rotor_vel
         # cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)