Fix quat_dot

crazyflow/dynamics/first_principles/dynamics.py

@@ -295,7 +295,7 @@ def symbolic_dynamics(
 
     # Rotational equation of motion
     xi = cs.vertcat(
-        cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
+        cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0)
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     torques_sum = torques_motor_vec

crazyflow/dynamics/so_rpy/dynamics.py

@@ -234,7 +234,7 @@ def symbolic_dynamics(
 
     # Rotational equation of motion
     xi = cs.vertcat(
-        cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
+        cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0)
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(

crazyflow/dynamics/so_rpy_rotor/dynamics.py

@@ -269,7 +269,7 @@ def symbolic_dynamics(
 
     # Rotational equation of motion
     xi = cs.vertcat(
-        cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
+        cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0)
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(

crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py

@@ -296,7 +296,7 @@ def symbolic_dynamics(
 
     # Rotational equation of motion
     xi = cs.vertcat(
-        cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
+        cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0)
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(

crazyflow/dynamics/utils/rotation.py

@@ -16,7 +16,7 @@
 
 
 def ang_vel2quat_dot(quat: Array, ang_vel: Array) -> Array:
-    """Calculates the quaternion derivative based on an angular velocity."""
+    """Calculates the quaternion derivative based on a body angular velocity."""
     xp = array_namespace(quat)
     # Split angular velocity
     x = ang_vel[..., 0:1]
@@ -31,14 +31,14 @@ def ang_vel2quat_dot(quat: Array, ang_vel: Array) -> Array:
         ],
         axis=-2,
     )
-    # First row of Xi
-    xi1 = xp.concat((xp.zeros_like(x), -ang_vel), axis=-1)
-    # Second to fourth rows of Xi
+    # First to third rows of Xi
     ang_vel_col = xp.expand_dims(ang_vel, axis=-1)  # (..., 3, 1)
-    xi2 = xp.concat((ang_vel_col, -ang_vel_skew), axis=-1)  # (..., 3, 4)
+    xi1 = xp.concat((-ang_vel_skew, ang_vel_col), axis=-1)  # (..., 3, 4)
+    # Fourth row of Xi
+    xi2 = xp.concat((-ang_vel, xp.zeros_like(x)), axis=-1)
     # Combine into Xi
-    xi1_exp = xp.expand_dims(xi1, axis=-2)  # (..., 1, 4)
-    xi = xp.concat((xi1_exp, xi2), axis=-2)  # (..., 4, 4)
+    xi2_exp = xp.expand_dims(xi2, axis=-2)  # (..., 1, 4)
+    xi = xp.concat((xi1, xi2_exp), axis=-2)  # (..., 4, 4)
     # Quaternion derivative
     quat_exp = xp.expand_dims(quat, axis=-1)  # (..., 4, 1)
     result = 0.5 * xp.matmul(xi, quat_exp)  # (..., 4, 1)
@@ -199,8 +199,7 @@ def cs_quat2euler(quat: cs.MX, seq: str = "xyz", degrees: bool = False) -> cs.MX
     CasADi-based solvers.
 
     Args:
-        quat: CasADi ``MX`` column vector of length 4, in scalar-last (xyzw)
-            convention.
+        quat: CasADi ``MX`` column vector of length 4 (xyzw).
         seq: Three-character axis sequence string.  Lowercase letters (e.g.
             ``"xyz"``) denote extrinsic rotations; uppercase (e.g. ``"XYZ"``)
             denote intrinsic rotations.  Consecutive axes must differ.

tests/unit/dynamics/test_rotation.py

@@ -29,6 +29,25 @@ def create_uniform_ang_vel(N: int = 1000, scale: float = 10) -> NDArray:
     return np.random.uniform(-scale, scale, size=(N, 3))
 
 
+@pytest.mark.unit
+def test_ang_vel2quat_dot():
+    N = 1000
+    rot = R.random(N)
+    ang_vels = create_uniform_ang_vel(N)
+    dt = 1e-5
+    quats = rot.as_quat()
+    quat_next = (rot * R.from_rotvec(ang_vels * dt)).as_quat()
+    quat_prev = (rot * R.from_rotvec(-ang_vels * dt)).as_quat()
+    # Pick the sign of ±q closest to quats. Using canonical is not sufficient here, as it
+    # splits quaternions with small signed w differences, but large xyz agreement
+    quat_next *= np.sign(np.sum(quat_next * quats, axis=-1, keepdims=True))
+    quat_prev *= np.sign(np.sum(quat_prev * quats, axis=-1, keepdims=True))
+    quat_dot_fd = (quat_next - quat_prev) / (2 * dt)
+
+    quat_dot = rotation.ang_vel2quat_dot(xp.asarray(quats), xp.asarray(ang_vels))
+    assert np.allclose(quat_dot, quat_dot_fd, atol=tol), "Quaternion derivative is off."
+
+
 @pytest.mark.unit
 def test_ang_vel2rpy_rates_two_way():
     quats = xp.asarray(create_uniform_quats())