Address comments

Author: amacati

crazyflow/control/transform.py

@@ -1,14 +1,12 @@
 """Transformations between physical parameters of the quadrotors.
 
-Conversions such as from motor forces to rotor speeds, or from thrust to PWM, are bundled in this
-module.
+Bundles conversions between motor forces, rotor velocities, and PWM commands.
 """
 
 from __future__ import annotations
 
 from typing import TYPE_CHECKING
 
-import array_api_extra as xpx
 from array_api_compat import array_namespace
 
 if TYPE_CHECKING:
@@ -32,38 +30,6 @@ def motor_force2rotor_vel(motor_forces: Array, rpm2thrust: Array) -> Array:
     ) / (2 * rpm2thrust[2])
 
 
-def rotor_vel2body_force(rotor_vel: Array, rpm2thrust: Array) -> Array:
-    """Convert rotor velocities to motor forces."""
-    xp = array_namespace(rotor_vel)
-    body_force = xp.zeros(rotor_vel.shape[:-1] + (3,), dtype=rotor_vel.dtype)
-    body_force = xpx.at(body_force)[..., 2].set(
-        xp.sum(
-            rpm2thrust[..., 0] + rpm2thrust[..., 1] * rotor_vel + rpm2thrust[..., 2] * rotor_vel**2,
-            axis=-1,
-        )
-    )
-    return body_force
-
-
-def rotor_vel2body_torque(
-    rotor_vel: Array, rpm2thrust: Array, rpm2torque: Array, L: float | Array, mixing_matrix: Array
-) -> Array:
-    """Convert rotor velocities to motor torques."""
-    xp = array_namespace(rotor_vel)
-    forces = rpm2thrust[..., 0] + rpm2thrust[..., 1] * rotor_vel + rpm2thrust[..., 2] * rotor_vel**2
-    torques_xy = (
-        xp.stack([xp.zeros_like(forces), xp.zeros_like(forces), forces])
-        @ mixing_matrix
-        * xp.stack([L, L, 0])
-    )
-    torques = (
-        rpm2torque[..., 0] + rpm2torque[..., 1] * rotor_vel + rpm2torque[..., 2] * rotor_vel**2
-    )
-    torques_z = xp.stack([xp.zeros_like(torques), xp.zeros_like(torques), torques])
-    body_torque = torques_xy + torques_z
-    return body_torque
-
-
 def force2pwm(thrust: Array | float, thrust_max: Array | float, pwm_max: Array | float) -> Array:
     """Convert thrust in N to thrust in PWM.
 

crazyflow/dynamics/first_principles/__init__.py

@@ -31,8 +31,7 @@
 \begin{aligned}
     \dot{\mathbf{p}} &= \mathbf{v}, \\
     \dot{\mathbf{q}} &= \tfrac{1}{2}
-        \begin{bmatrix}0 \\ {}^{\mathcal{B}}\boldsymbol{\omega}\end{bmatrix}
-        \otimes \mathbf{q}, \\
+        \mathbf{q} \otimes \begin{bmatrix} {}^{\mathcal{B}}\boldsymbol{\omega}\\0 \end{bmatrix}, \\
     m\dot{\mathbf{v}} &= m\mathbf{g}
         + R\,{}^{\mathcal{B}}\mathbf{f}_t
         + R\,{}^{\mathcal{B}}\mathbf{f}_a, \\

crazyflow/dynamics/so_rpy/__init__.py

@@ -8,8 +8,7 @@
 \begin{aligned}
     \dot{\mathbf{p}} &= \mathbf{v}, \\
     \dot{\mathbf{q}} &= \tfrac{1}{2}
-        \begin{bmatrix}0 \\ {}^{\mathcal{B}}\boldsymbol{\omega}\end{bmatrix}
-        \otimes \mathbf{q}, \\
+        \mathbf{q} \otimes \begin{bmatrix} {}^{\mathcal{B}}\boldsymbol{\omega}\\0 \end{bmatrix}, \\
     m\dot{\mathbf{v}} &= m\mathbf{g}
         + (c_{\mathrm{acc}} + c_f F_{\mathrm{cmd}})\,R\,\mathbf{e}_z, \\
     \ddot{\boldsymbol{\psi}} &=

crazyflow/dynamics/so_rpy_rotor/__init__.py

@@ -10,8 +10,7 @@
     \dot{F} &= \frac{1}{\tau}(F_{\mathrm{cmd}} - F), \\
     \dot{\mathbf{p}} &= \mathbf{v}, \\
     \dot{\mathbf{q}} &= \tfrac{1}{2}
-        \begin{bmatrix}0 \\ {}^{\mathcal{B}}\boldsymbol{\omega}\end{bmatrix}
-        \otimes \mathbf{q}, \\
+        \mathbf{q} \otimes \begin{bmatrix} {}^{\mathcal{B}}\boldsymbol{\omega}\\0 \end{bmatrix}, \\
     m\dot{\mathbf{v}} &= m\mathbf{g}
         + (c_{\mathrm{acc}} + c_f F)\,R\,\mathbf{e}_z, \\
     \ddot{\boldsymbol{\psi}} &=

crazyflow/dynamics/so_rpy_rotor/dynamics.py

@@ -142,7 +142,7 @@ def dynamics(
 
 
 def symbolic_dynamics(
-    model_rotor_vel: bool = False,
+    model_rotor_vel: bool = True,
     model_dist_f: bool = False,
     model_dist_t: bool = False,
     *,
@@ -166,7 +166,7 @@ def symbolic_dynamics(
 
     Args:
         model_rotor_vel: If ``True``, the scalar thrust state is included in ``X`` and first-order
-            thrust dynamics are modelled. Defaults to ``False``.
+            thrust dynamics are modelled. Defaults to ``_True``.
         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.

crazyflow/dynamics/so_rpy_rotor_drag/__init__.py

@@ -10,8 +10,7 @@
     \dot{F} &= \frac{1}{\tau}(F_{\mathrm{cmd}} - F), \\
     \dot{\mathbf{p}} &= \mathbf{v}, \\
     \dot{\mathbf{q}} &= \tfrac{1}{2}
-        \begin{bmatrix}0 \\ {}^{\mathcal{B}}\boldsymbol{\omega}\end{bmatrix}
-        \otimes \mathbf{q}, \\
+        \mathbf{q} \otimes \begin{bmatrix} {}^{\mathcal{B}}\boldsymbol{\omega}\\0 \end{bmatrix}, \\
     m\dot{\mathbf{v}} &= m\mathbf{g}
         + (c_{\mathrm{acc}} + c_f F)\,R\,\mathbf{e}_z
         + R\,D_b\,R^{\top}\mathbf{v}, \\

crazyflow/dynamics/symbols.py

@@ -1,33 +1,36 @@
-"""Symbols used in the symbolic drone dynamics."""
+"""Symbols used in the symbolic drone dynamics.
+
+Can be used to define symbolic CasADi expressions that are passed to model-based optimizers such as
+Acados.
+"""
 
 import casadi as cs
 
 from crazyflow.dynamics.utils import rotation
 
 # States
 px, py, pz = cs.MX.sym("px"), cs.MX.sym("py"), cs.MX.sym("pz")
-pos = cs.vertcat(px, py, pz)  # Position
-"""Symbolic drone position.
-
-Can be used to define symbolic CasADi expressions that are passed to model-based optimizers such as
-Acados.
-
-:meta hide-value:
-"""
-# States
+pos = cs.vertcat(px, py, pz)
+"""Symbolic drone position."""
 qw, qx, qy, qz = cs.MX.sym("qw"), cs.MX.sym("qx"), cs.MX.sym("qy"), cs.MX.sym("qz")
-quat = cs.vertcat(qx, qy, qz, qw)  # Quaternions
+quat = cs.vertcat(qx, qy, qz, qw)
+"""Symbolic drone orientation as xyzw quaternion."""
 rot = rotation.cs_quat2matrix(quat)  # Rotation matrix from body to world frame
 vx, vy, vz = cs.MX.sym("vx"), cs.MX.sym("vy"), cs.MX.sym("vz")
-vel = cs.vertcat(vx, vy, vz)  # Velocity
+vel = cs.vertcat(vx, vy, vz)
+"""Symbolic drone velocity."""
 wx, wy, wz = cs.MX.sym("wx"), cs.MX.sym("wy"), cs.MX.sym("wz")
-ang_vel = cs.vertcat(wx, wy, wz)  # Angular velocity
+ang_vel = cs.vertcat(wx, wy, wz)
+"""Symbolic drone angular velocity."""
 w1, w2, w3, w4 = cs.MX.sym("w1"), cs.MX.sym("w2"), cs.MX.sym("w3"), cs.MX.sym("w4")
-rotor_vel = cs.vertcat(w1, w2, w3, w4)  # Motor thrust
+rotor_vel = cs.vertcat(w1, w2, w3, w4)
+"""Symbolic rotor velocities."""
 dfx, dfy, dfz = cs.MX.sym("dfx"), cs.MX.sym("dfy"), cs.MX.sym("dfz")
-dist_f = cs.vertcat(dfx, dfy, dfz)  # Disturbance forces
+dist_f = cs.vertcat(dfx, dfy, dfz)
+"""Symbolic disturbance forces."""
 dtx, dty, dtz = cs.MX.sym("dtx"), cs.MX.sym("dty"), cs.MX.sym("dtz")
-dist_t = cs.vertcat(dtx, dty, dtz)  # Disturbance torques
+dist_t = cs.vertcat(dtx, dty, dtz)
+"""Symbolic disturbance torques."""
 
 # Inputs
 cmd_w1, cmd_w2, cmd_w3, cmd_w4 = (
@@ -37,10 +40,11 @@
     cs.MX.sym("cmd_w4"),
 )
 cmd_rotor_vel = cs.vertcat(cmd_w1, cmd_w2, cmd_w3, cmd_w4)
-cmd_force = cs.vertcat(cmd_w1, cmd_w2, cmd_w3, cmd_w4)
+"""Symbolic rotor velocity commands."""
 cmd_roll, cmd_pitch, cmd_yaw = (cs.MX.sym("cmd_roll"), cs.MX.sym("cmd_pitch"), cs.MX.sym("cmd_yaw"))
 cmd_thrust = cs.MX.sym("cmd_thrust")
 cmd_rpyt = cs.vertcat(cmd_roll, cmd_pitch, cmd_yaw, cmd_thrust)
+"""Symbolic roll/pitch/yaw/thrust commands."""
 
 # Special states for the so_rpy dynamics
 roll, pitch, yaw = cs.MX.sym("roll"), cs.MX.sym("pitch"), cs.MX.sym("yaw")

crazyflow/dynamics/transform.py

@@ -1,6 +1,6 @@
 import warnings
 from functools import partial
-from typing import Callable, Literal
+from typing import Callable
 
 import jax
 import jax.numpy as jnp
@@ -64,7 +64,7 @@ def __init__(
         *,
         num_envs: int = 1,
         max_episode_time: float = 10.0,
-        dynamics: Literal["so_rpy", "first_principles"] | Dynamics = Dynamics.so_rpy,
+        dynamics: Dynamics = Dynamics.so_rpy,
         drone: str = "cf2x_L250",
         freq: int = 500,
         device: str = "cpu",

crazyflow/envs/drone_env.py

@@ -1,5 +1,3 @@
-from typing import Literal
-
 import jax
 import jax.numpy as jnp
 import numpy as np
@@ -30,7 +28,7 @@ def __init__(
         *,
         num_envs: int = 1,
         max_episode_time: float = 10.0,
-        dynamics: Literal["so_rpy", "first_principles"] | Dynamics = Dynamics.so_rpy,
+        dynamics: Dynamics = Dynamics.so_rpy,
         drone: str = "cf2x_L250",
         freq: int = 500,
         device: str = "cpu",