Simplify wrappers, add drone-models test suite to crazyflow

Author: amacati

crazyflow/dynamics/core.py

@@ -14,7 +14,6 @@
 if TYPE_CHECKING:
     from types import ModuleType
 
-
 F = TypeVar("F", bound=Callable[..., Any])
 P = ParamSpec("P")
 R = TypeVar("R")

crazyflow/dynamics/first_principles/dynamics.py

@@ -14,6 +14,7 @@
 
 from __future__ import annotations
 
+import warnings
 from typing import TYPE_CHECKING
 
 import casadi as cs
@@ -106,6 +107,7 @@ def dynamics(
     rot_mat = rot.inv().as_matrix()  # from world to body
     # Rotor dynamics
     if rotor_vel is None:
+        warnings.warn("Rotor velocity not provided, using commanded rotor velocity.")
         rotor_vel, rotor_vel_dot = cmd, None
     else:
         rotor_vel_dot = xp.where(

crazyflow/dynamics/so_rpy/dynamics.py

@@ -1,8 +1,9 @@
 """Second-order fitted RPY dynamics (no rotor dynamics).
 
-This module implements a simplified quadrotor dynamics where the rotational dynamics are modelled as a fitted second-order linear system driven by roll, pitch,
-and yaw (RPY) commands, and the translational dynamics are driven by the collective
-thrust command.  Motor spin-up dynamics are not modelled.
+This module implements a simplified quadrotor dynamics where the rotational dynamics are modelled as
+a fitted second-order linear system driven by roll, pitch, and yaw (RPY) commands, and the
+translational dynamics are driven by the collective thrust command.  Motor spin-up dynamics are not
+modelled.
 
 The command interface is ``[roll_rad, pitch_rad, yaw_rad, thrust_N]``.
 

crazyflow/dynamics/so_rpy_rotor/dynamics.py

@@ -14,6 +14,7 @@
 
 from __future__ import annotations
 
+import warnings
 from typing import TYPE_CHECKING
 
 import casadi as cs
@@ -103,6 +104,7 @@ def dynamics(
 
     # Note that we are abusing the rotor_vel state as the thrust
     if rotor_vel is None:
+        warnings.warn("Rotor velocity not provided, using commanded rotor velocity.")
         rotor_vel, rotor_vel_dot = cmd_f[..., None], None
     else:
         rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)

crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py

@@ -16,6 +16,7 @@
 
 from __future__ import annotations
 
+import warnings
 from typing import TYPE_CHECKING
 
 import casadi as cs
@@ -116,6 +117,7 @@ def dynamics(
 
     # Note that we are abusing the rotor_vel state as the thrust
     if rotor_vel is None:
+        warnings.warn("Rotor velocity not provided, using commanded rotor velocity.")
         rotor_vel, rotor_vel_dot = cmd_f[..., None], None
     else:
         rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)

tests/integration/dynamics/__init__.py

@@ -0,0 +1,57 @@
+"""Tests for identification pipeline."""
+
+from __future__ import annotations
+
+import array_api_compat.numpy as np
+import pytest
+
+from crazyflow.dynamics.utils.data_utils import derivatives_svf, preprocessing
+from crazyflow.dynamics.utils.identification import sys_id_rotation, sys_id_translation
+
+
+@pytest.mark.integration
+def test_sys_id_rotation():
+    time = np.linspace(0.0, 1.0, 100)
+    pos = np.stack((np.cos(2 * np.pi * time), np.sin(2 * np.pi * time), time), axis=-1)
+    quat = np.zeros((100, 4))
+    quat[:, 3] = 1.0  # No rotation
+    cmd_rpy = np.zeros((100, 3))
+    cmd_f = np.ones(100) * 0.03
+    data = {
+        "time": np.linspace(0.0, 1.0, 100),
+        "pos": pos,
+        "quat": quat,
+        "cmd_rpy": cmd_rpy,
+        "cmd_f": cmd_f,
+    }
+
+    data = preprocessing(data)
+    data["rpy"] = np.roll(cmd_rpy, shift=10, axis=0)
+    data = derivatives_svf(data)
+
+    sys_id_rotation(data)
+
+
+@pytest.mark.integration
+@pytest.mark.parametrize("dynamics", ["so_rpy", "so_rpy_rotor", "so_rpy_rotor_drag"])
+def test_sys_id_translation(dynamics: str):
+    mass = 0.03
+    time = np.linspace(0.0, 1.0, 100)
+    phi = 2 * np.pi * time
+    pos = np.stack((np.cos(phi) * 0.1, np.sin(phi) * 0.1, np.roll(np.cos(phi), 10)), axis=-1)
+    quat = np.zeros((100, 4))
+    quat[:, 3] = 1.0  # No rotation
+    cmd_rpy = np.zeros((100, 3))
+    cmd_f = (np.ones(100) + np.cos(phi)) * mass * 9.81
+    data = {
+        "time": np.linspace(0.0, 1.0, 100),
+        "pos": pos,
+        "quat": quat,
+        "cmd_rpy": cmd_rpy,
+        "cmd_f": cmd_f,
+    }
+
+    data = preprocessing(data)
+    data = derivatives_svf(data)
+
+    sys_id_translation(dynamics=dynamics, mass=mass, data=data)