From 843d7a99a18685f737ef76b1de8334f5b4fc692d Mon Sep 17 00:00:00 2001 From: ratheron Date: Tue, 6 Oct 2026 20:09:16 +0200 Subject: [PATCH 1/5] Fix quat_dot computation for scalar-last quaternions --- crazyflow/dynamics/first_principles/dynamics.py | 2 +- crazyflow/dynamics/so_rpy/dynamics.py | 2 +- crazyflow/dynamics/so_rpy_rotor/dynamics.py | 2 +- crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py | 2 +- crazyflow/dynamics/utils/rotation.py | 14 +++++++------- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/crazyflow/dynamics/first_principles/dynamics.py b/crazyflow/dynamics/first_principles/dynamics.py index 38b85f22..30e0b033 100644 --- a/crazyflow/dynamics/first_principles/dynamics.py +++ b/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 diff --git a/crazyflow/dynamics/so_rpy/dynamics.py b/crazyflow/dynamics/so_rpy/dynamics.py index b031909f..06fd2a8b 100644 --- a/crazyflow/dynamics/so_rpy/dynamics.py +++ b/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( diff --git a/crazyflow/dynamics/so_rpy_rotor/dynamics.py b/crazyflow/dynamics/so_rpy_rotor/dynamics.py index 4bb3ad68..6cbed5eb 100644 --- a/crazyflow/dynamics/so_rpy_rotor/dynamics.py +++ b/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( diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py index 103ba270..f2335976 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py +++ b/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( diff --git a/crazyflow/dynamics/utils/rotation.py b/crazyflow/dynamics/utils/rotation.py index 5366c7b5..27591627 100644 --- a/crazyflow/dynamics/utils/rotation.py +++ b/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 scalar-last 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) From d7c9b3ed0a1ce6b0ea7ece91ef804bcb3cb7fa79 Mon Sep 17 00:00:00 2001 From: ratheron Date: Tue, 6 Oct 2026 20:24:16 +0200 Subject: [PATCH 2/5] Add test for ang_vel2quat_dot --- tests/unit/dynamics/test_rotation.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index da46999e..b3cb3d8c 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -29,6 +29,22 @@ 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(): + quats = R.random(1000).as_quat() + ang_vels = create_uniform_ang_vel() + dt = 1e-5 + rot = R.from_quat(quats) + quat_next = (rot * R.from_rotvec(ang_vels * dt)).as_quat() + quat_prev = (rot * R.from_rotvec(-ang_vels * dt)).as_quat() + 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()) From 3ed418fd2a15967b6982d8af8b39c5871db85a47 Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 7 Oct 2026 14:36:08 +0200 Subject: [PATCH 3/5] Rearrange test --- tests/unit/dynamics/test_rotation.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index b3cb3d8c..b0ea2856 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -31,10 +31,11 @@ def create_uniform_ang_vel(N: int = 1000, scale: float = 10) -> NDArray: @pytest.mark.unit def test_ang_vel2quat_dot(): - quats = R.random(1000).as_quat() - ang_vels = create_uniform_ang_vel() + N = 1000 + rot = R.random(N) + ang_vels = create_uniform_ang_vel(N) dt = 1e-5 - rot = R.from_quat(quats) + 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() quat_next *= np.sign(np.sum(quat_next * quats, axis=-1, keepdims=True)) From 3bae11260855f5d1b16089a5287f50a4d435f488 Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 7 Oct 2026 15:19:28 +0200 Subject: [PATCH 4/5] Apply suggestions --- crazyflow/dynamics/utils/rotation.py | 5 ++--- tests/unit/dynamics/test_rotation.py | 1 + 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/crazyflow/dynamics/utils/rotation.py b/crazyflow/dynamics/utils/rotation.py index 27591627..408f8f62 100644 --- a/crazyflow/dynamics/utils/rotation.py +++ b/crazyflow/dynamics/utils/rotation.py @@ -16,7 +16,7 @@ def ang_vel2quat_dot(quat: Array, ang_vel: Array) -> Array: - """Calculates the scalar-last quaternion derivative based on a body angular velocity.""" + """Calculates the quaternion derivative based on a body angular velocity.""" xp = array_namespace(quat) # Split angular velocity x = ang_vel[..., 0: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. diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index b0ea2856..10888e9a 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -38,6 +38,7 @@ def test_ang_vel2quat_dot(): 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; canonical=True can split pairs near w=0 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) From d114e5a854ef037d930f13e0440d9a47238135c3 Mon Sep 17 00:00:00 2001 From: Martin Schuck <57562633+amacati@users.noreply.github.com> Date: Wed, 7 Oct 2026 17:32:40 +0300 Subject: [PATCH 5/5] Improve comment on quaternion sign selection logic --- tests/unit/dynamics/test_rotation.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index 10888e9a..cdb1646d 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -38,7 +38,8 @@ def test_ang_vel2quat_dot(): 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; canonical=True can split pairs near w=0 + # 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)