Rename dynamics file, simplify so_rpy model, simplify models wrapper

Author: amacati

crazyflow/dynamics/core.py

@@ -4,9 +4,8 @@
 
 import inspect
 import tomllib
-import warnings
 from enum import Enum
-from functools import partial, wraps
+from functools import partial
 from pathlib import Path
 from typing import TYPE_CHECKING, Any, Callable, ParamSpec, TypeVar
 
@@ -15,8 +14,6 @@
 if TYPE_CHECKING:
     from types import ModuleType
 
-    from crazyflow._typing import Array  # To be changed to array_api_typing later
-
 
 F = TypeVar("F", bound=Callable[..., Any])
 P = ParamSpec("P")
@@ -26,14 +23,7 @@
 def supports(rotor_dynamics: bool = True) -> Callable[[F], F]:
     """Decorator that declares which optional inputs a dynamics function supports.
 
-    Wraps the decorated function so that:
-
-    * If ``rotor_dynamics=False`` and the caller passes ``rotor_vel``, a ``ValueError`` is raised
-      immediately.
-    * If ``rotor_dynamics=True`` and the caller omits ``rotor_vel``, a ``UserWarning`` is issued and
-      the commanded value is used directly.
-
-    The decorator also attaches a ``__dynamics_features__`` attribute to the wrapper, which
+    The decorator attaches a ``__dynamics_features__`` attribute to the wrapper, which
     [dynamics_features][crazyflow.dynamics.dynamics_features] reads.
 
     Args:
@@ -42,30 +32,12 @@ def supports(rotor_dynamics: bool = True) -> Callable[[F], F]:
             Defaults to ``True``.
 
     Returns:
-        A decorator that wraps the dynamics function with the capability checks.
+        The function decorated with capability flags.
     """
 
     def decorator(fn: F) -> F:
-        @wraps(fn)
-        def wrapper(
-            pos: Array,
-            quat: Array,
-            vel: Array,
-            ang_vel: Array,
-            cmd: Array,
-            rotor_vel: Array | None = None,
-            *args: Any,
-            **kwargs: Any,
-        ) -> tuple[Array, Array, Array, Array, Array | None]:
-            if not rotor_dynamics and rotor_vel is not None:
-                raise ValueError("Rotor dynamics not supported, but rotor_vel is provided.")
-            if rotor_dynamics and rotor_vel is None:
-                warnings.warn("Rotor velocity not provided, using commanded rotor velocity.")
-            return fn(pos, quat, vel, ang_vel, cmd, rotor_vel, *args, **kwargs)
-
-        wrapper.__dynamics_features__ = {"rotor_dynamics": rotor_dynamics}
-
-        return wrapper  # type: ignore
+        fn.__dynamics_features__ = {"rotor_dynamics": rotor_dynamics}
+        return fn
 
     return decorator
 

crazyflow/dynamics/first_principles/__init__.py

@@ -82,7 +82,7 @@
 moment of inertia, \(M\) is the \(3\times 4\) mixing matrix, and \(\mathbf{m}_z\) is its last row.
 """
 
-from crazyflow.dynamics.first_principles.model import (
+from crazyflow.dynamics.first_principles.dynamics import (
     Params,
     dynamics,
     sim_dynamics,

…yflow/dynamics/first_principles/model.py …ow/dynamics/first_principles/dynamics.pycrazyflow/dynamics/first_principles/model.py renamed to crazyflow/dynamics/first_principles/dynamics.py crazyflow/dynamics/first_principles/model.py renamed to crazyflow/dynamics/first_principles/dynamics.py

@@ -26,7 +26,7 @@
 \(c_{\dot{\psi}}\), \(c_u\) are identified from flight data.
 """
 
-from crazyflow.dynamics.so_rpy.model import (
+from crazyflow.dynamics.so_rpy.dynamics import (
     Params,
     dynamics,
     sim_dynamics,

crazyflow/dynamics/so_rpy/__init__.py

@@ -13,7 +13,6 @@
 
 from __future__ import annotations
 
-import logging
 from typing import TYPE_CHECKING
 
 import casadi as cs
@@ -41,7 +40,6 @@ def dynamics(
     vel: Array,
     ang_vel: Array,
     cmd: Array,
-    rotor_vel: Array | None = None,
     dist_f: Array | None = None,
     dist_t: Array | None = None,
     *,
@@ -63,7 +61,6 @@ def dynamics(
         vel: Velocity of the drone (m/s).
         ang_vel: Angular velocity of the drone (rad/s).
         cmd: Roll pitch yaw (rad) and collective thrust (N) command.
-        rotor_vel: Speed of the 4 motors (RPMs). Kept for compatibility with the dynamics signature.
         dist_f: Disturbance force (N) in the world frame acting on the CoM.
         dist_t: Disturbance torque (Nm) in the world frame acting on the CoM.
 
@@ -92,9 +89,7 @@ def dynamics(
     rot = R.from_quat(quat)
     euler_angles = rot.as_euler("xyz")
 
-    rotor_vel_dot = None
     thrust = acc_coef + cmd_f_coef * cmd_f
-
     drone_z_axis = rot.as_matrix()[..., -1]
 
     pos_dot = vel
@@ -121,11 +116,10 @@ def dynamics(
         torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
         ang_vel_dot = (J_inv @ torque[..., None])[..., 0]
 
-    return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot
+    return pos_dot, quat_dot, vel_dot, ang_vel_dot
 
 
 def symbolic_dynamics(
-    model_rotor_vel: bool = False,
     model_dist_f: bool = False,
     model_dist_t: bool = False,
     *,
@@ -147,9 +141,6 @@ def symbolic_dynamics(
     [symbolic_dynamics][crazyflow.dynamics.first_principles.symbolic_dynamics].
 
     Args:
-        model_rotor_vel: If ``True``, a scalar rotor-velocity state is appended to ``X`` (for
-            interface compatibility only — ``so_rpy`` has no thrust dynamics and will log a
-            warning).  Defaults to ``False``.
         model_dist_f: If ``True``, a 3-D force disturbance is appended to ``X``.
         model_dist_t: If ``True``, a 3-D torque disturbance is appended to ``X``.
         mass: Drone mass in kg.
@@ -178,11 +169,8 @@ def symbolic_dynamics(
     symbols.rpy = _rpy_quat
     symbols.drpy = _drpy_quat
     X_dot_euler, X_euler, U_euler, Y_euler = symbolic_dynamics_euler(
-        model_rotor_vel=model_rotor_vel,
         mass=mass,
         gravity_vec=gravity_vec,
-        J=J,
-        J_inv=J_inv,
         acc_coef=acc_coef,
         cmd_f_coef=cmd_f_coef,
         rpy_coef=rpy_coef,
@@ -194,9 +182,6 @@ def symbolic_dynamics(
 
     # States and Inputs
     X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
-    if model_rotor_vel:
-        logging.getLogger(__name__).warning("so_rpy dynamics do not support thrust dynamics")
-        X = cs.vertcat(X, symbols.rotor_vel)
     if model_dist_f:
         X = cs.vertcat(X, symbols.dist_f)
     if model_dist_t:
@@ -234,12 +219,9 @@ def symbolic_dynamics(
 
 
 def symbolic_dynamics_euler(
-    model_rotor_vel: bool = False,
     *,
     mass: float,
     gravity_vec: Array,
-    J: Array,
-    J_inv: Array,
     acc_coef: Array,
     cmd_f_coef: Array,
     rpy_coef: Array,
@@ -253,12 +235,8 @@ def symbolic_dynamics_euler(
     inside CasADi-based solvers.
 
     Args:
-        model_rotor_vel: If ``True``, a scalar rotor-velocity state is appended
-            to ``X`` for interface compatibility (no dynamics are modelled).
         mass: Drone mass in kg.
         gravity_vec: Gravity vector, shape ``(3,)``.
-        J: Inertia matrix, shape ``(3, 3)``.
-        J_inv: Inverse inertia matrix, shape ``(3, 3)``.
         acc_coef: Scalar acceleration offset coefficient.
         cmd_f_coef: Collective-thrust-to-acceleration coefficient.
         rpy_coef: RPY state feedback coefficient, shape ``(3,)``.
@@ -275,9 +253,6 @@ def symbolic_dynamics_euler(
     """
     # States and Inputs
     X = cs.vertcat(symbols.pos, symbols.rpy, symbols.vel, symbols.drpy)
-    if model_rotor_vel:
-        logging.getLogger(__name__).warning("so_rpy dynamics do not support thrust dynamics")
-        X = cs.vertcat(X, symbols.rotor_vel)
     U = symbols.cmd_rpyt
     cmd_rpy = U[:3]
     cmd_thrust = U[-1]
@@ -351,7 +326,7 @@ def create(n_worlds: int, n_drones: int, drone: str, device: Device) -> Params:
 def sim_dynamics(data: SimData) -> SimData:
     """Compute the forces and torques from the so_rpy dynamics."""
     params: Params = data.params
-    vel, _, acc, ang_acc, _ = dynamics(
+    vel, _, acc, ang_acc = dynamics(
         pos=data.states.pos,
         quat=data.states.quat,
         vel=data.states.vel,

crazyflow/dynamics/so_rpy/model.py crazyflow/dynamics/so_rpy/dynamics.pycrazyflow/dynamics/so_rpy/model.py renamed to crazyflow/dynamics/so_rpy/dynamics.py

@@ -26,7 +26,7 @@
 \(R = {}^{\mathcal{I}}R_{\mathcal{B}}(\mathbf{q})\) is the rotation from body to world frame.
 """
 
-from crazyflow.dynamics.so_rpy_rotor.model import (
+from crazyflow.dynamics.so_rpy_rotor.dynamics import (
     Params,
     dynamics,
     sim_dynamics,

crazyflow/dynamics/so_rpy_rotor/__init__.py

@@ -28,7 +28,7 @@
 \(D_b\) is the diagonal body-frame aerodynamic drag matrix.
 """
 
-from crazyflow.dynamics.so_rpy_rotor_drag.model import (
+from crazyflow.dynamics.so_rpy_rotor_drag.dynamics import (
     Params,
     dynamics,
     sim_dynamics,

crazyflow/dynamics/so_rpy_rotor/model.py …zyflow/dynamics/so_rpy_rotor/dynamics.pycrazyflow/dynamics/so_rpy_rotor/model.py renamed to crazyflow/dynamics/so_rpy_rotor/dynamics.py crazyflow/dynamics/so_rpy_rotor/model.py renamed to crazyflow/dynamics/so_rpy_rotor/dynamics.py

@@ -40,9 +40,7 @@ def symbolic_from_sim(
             )
         case Dynamics.so_rpy:
             return parametrize(so_rpy_symbolic_dynamics, sim.drone)(
-                model_rotor_vel=model_rotor_vel,
-                model_dist_f=model_dist_f,
-                model_dist_t=model_dist_t,
+                model_dist_f=model_dist_f, model_dist_t=model_dist_t
             )
         case _:
             raise ValueError(f"Dynamics mode {sim.dynamics} not supported")