From 6f8b45d4bd508f35333f13a9322ea51c2f9cb6de Mon Sep 17 00:00:00 2001 From: Yusuf Malik Durmus Date: Tue, 3 Nov 2026 15:03:38 +0300 Subject: [PATCH 1/3] Fix cs_quat2euler degrees and duplicate CasADi function names - Convert radians to degrees in cs_quat2euler(degrees=True) instead of the inverse - Give the CasADi derivative conversion functions unique names --- crazyflow/dynamics/utils/rotation.py | 10 +++++++--- tests/unit/dynamics/test_rotation.py | 30 ++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/crazyflow/dynamics/utils/rotation.py b/crazyflow/dynamics/utils/rotation.py index 408f8f62c..03f127bde 100644 --- a/crazyflow/dynamics/utils/rotation.py +++ b/crazyflow/dynamics/utils/rotation.py @@ -305,7 +305,7 @@ def elementary_basis_index(axis: str) -> int: ) if degrees: - angles = (cs.np.pi / 180.0) * cs.horzcat(angles) + angles = (180.0 / cs.np.pi) * angles return angles @@ -483,7 +483,9 @@ def create_cs_ang_vel_deriv2rpy_rates_deriv() -> cs.Function: W_dot = cs.vertcat(row1, row2, row3) rpy_rates_deriv = W_dot @ ang_vel + cs_ang_vel2rpy_rates(quat, ang_vel_deriv) - return cs.Function("cs_ang_vel2rpy_rates", [quat, ang_vel, ang_vel_deriv], [rpy_rates_deriv]) + return cs.Function( + "cs_ang_vel_deriv2rpy_rates_deriv", [quat, ang_vel, ang_vel_deriv], [rpy_rates_deriv] + ) cs_ang_vel_deriv2rpy_rates_deriv = create_cs_ang_vel_deriv2rpy_rates_deriv() @@ -528,7 +530,9 @@ def create_cs_rpy_rates_deriv2ang_vel_deriv() -> cs.Function: W_dot = cs.vertcat(row1, row2, row3) ang_vel_deriv = W_dot @ rpy_rates + cs_rpy_rates2ang_vel(quat, rpy_rates_deriv) - return cs.Function("cs_ang_vel2rpy_rates", [quat, rpy_rates, rpy_rates_deriv], [ang_vel_deriv]) + return cs.Function( + "cs_rpy_rates_deriv2ang_vel_deriv", [quat, rpy_rates, rpy_rates_deriv], [ang_vel_deriv] + ) cs_rpy_rates_deriv2ang_vel_deriv = create_cs_rpy_rates_deriv2ang_vel_deriv() diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index cdb1646df..bf23a4b7a 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -5,6 +5,7 @@ from typing import TYPE_CHECKING import array_api_strict as xp +import casadi as cs import numpy as np import pytest from scipy.spatial.transform import Rotation as R @@ -240,3 +241,32 @@ def test_rpy2matrix_symbolic(): # compare casadi/symbolic implementation to scipy mat_cs = np.array(rotation.cs_rpy2matrix_func(rpy)) assert np.allclose(mat_cs, mat_scipy, atol=tol), "Symbolic rpy->matrix differs from scipy." + + +@pytest.mark.unit +@pytest.mark.parametrize("seq", ["xyz", "zyx", "zxz", "XYZ", "ZYX", "ZXZ"]) +@pytest.mark.parametrize("degrees", [False, True]) +def test_quat2euler_symbolic(seq: str, degrees: bool): + rot = R.random(100) + quat = cs.MX.sym("quat", 4) + quat2euler = cs.Function("quat2euler", [quat], [rotation.cs_quat2euler(quat, seq, degrees)]) + period = 360.0 if degrees else 2 * np.pi + for q, euler_scipy in zip(rot.as_quat(), rot.as_euler(seq, degrees=degrees)): + euler_cs = np.array(quat2euler(q)).flatten() + diff = (euler_cs - euler_scipy + period / 2) % period - period / 2 # Wrap to [-p/2, p/2) + assert np.allclose(diff, 0.0, atol=tol), "Symbolic quat->euler differs from scipy." + + +@pytest.mark.unit +def test_symbolic_function_names_unique(): + fns = [ + rotation.cs_ang_vel2rpy_rates, + rotation.cs_rpy_rates2ang_vel, + rotation.cs_ang_vel_deriv2rpy_rates_deriv, + rotation.cs_rpy_rates_deriv2ang_vel_deriv, + rotation.cs_quat2matrix_func, + rotation.cs_rpy2matrix_func, + ] + names = [fn.name() for fn in fns] + # Duplicate names produce conflicting C symbols when generating code for several functions + assert len(set(names)) == len(names), f"CasADi function names are not unique: {names}" From cc18ccdf56feb723d6b64eedfaa012b79766fc5e Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 18 Nov 2026 17:45:27 +0100 Subject: [PATCH 2/3] Autocollect cs functions --- tests/unit/dynamics/test_rotation.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index bf23a4b7a..7658313eb 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -255,18 +255,18 @@ def test_quat2euler_symbolic(seq: str, degrees: bool): euler_cs = np.array(quat2euler(q)).flatten() diff = (euler_cs - euler_scipy + period / 2) % period - period / 2 # Wrap to [-p/2, p/2) assert np.allclose(diff, 0.0, atol=tol), "Symbolic quat->euler differs from scipy." + lock = [0.0, np.pi] if seq[0] == seq[2] else [np.pi / 2, -np.pi / 2] + rot = R.from_euler(seq, [[0.3, angle, 0.4] for angle in lock]) + with pytest.warns(UserWarning, match="Gimbal lock"): + reference = rot.as_euler(seq, degrees=degrees) + for q, euler_scipy in zip(rot.as_quat(), reference): + euler_cs = np.array(quat2euler(q)).flatten() + assert np.allclose(euler_cs, euler_scipy, atol=tol), "Symbolic quat->euler differs at lock." @pytest.mark.unit def test_symbolic_function_names_unique(): - fns = [ - rotation.cs_ang_vel2rpy_rates, - rotation.cs_rpy_rates2ang_vel, - rotation.cs_ang_vel_deriv2rpy_rates_deriv, - rotation.cs_rpy_rates_deriv2ang_vel_deriv, - rotation.cs_quat2matrix_func, - rotation.cs_rpy2matrix_func, - ] - names = [fn.name() for fn in fns] + names = [fn.name() for fn in vars(rotation).values() if isinstance(fn, cs.Function)] + assert names, "No CasADi functions found in the rotation module." # Duplicate names produce conflicting C symbols when generating code for several functions assert len(set(names)) == len(names), f"CasADi function names are not unique: {names}" From 084281e5b34a9e0d28196e232034e87cd752d517 Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 18 Nov 2026 19:03:16 +0100 Subject: [PATCH 3/3] Remove wrapping --- tests/unit/dynamics/test_rotation.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index 7658313eb..5e5a02459 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -247,14 +247,12 @@ def test_rpy2matrix_symbolic(): @pytest.mark.parametrize("seq", ["xyz", "zyx", "zxz", "XYZ", "ZYX", "ZXZ"]) @pytest.mark.parametrize("degrees", [False, True]) def test_quat2euler_symbolic(seq: str, degrees: bool): - rot = R.random(100) + rot = R.random(100, rng=0) quat = cs.MX.sym("quat", 4) quat2euler = cs.Function("quat2euler", [quat], [rotation.cs_quat2euler(quat, seq, degrees)]) - period = 360.0 if degrees else 2 * np.pi for q, euler_scipy in zip(rot.as_quat(), rot.as_euler(seq, degrees=degrees)): euler_cs = np.array(quat2euler(q)).flatten() - diff = (euler_cs - euler_scipy + period / 2) % period - period / 2 # Wrap to [-p/2, p/2) - assert np.allclose(diff, 0.0, atol=tol), "Symbolic quat->euler differs from scipy." + assert np.allclose(euler_cs, euler_scipy, atol=tol), "Symbolic quat->euler differs." lock = [0.0, np.pi] if seq[0] == seq[2] else [np.pi / 2, -np.pi / 2] rot = R.from_euler(seq, [[0.3, angle, 0.4] for angle in lock]) with pytest.warns(UserWarning, match="Gimbal lock"):