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..5e5a02459 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,30 @@ 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, rng=0) + quat = cs.MX.sym("quat", 4) + quat2euler = cs.Function("quat2euler", [quat], [rotation.cs_quat2euler(quat, seq, degrees)]) + for q, euler_scipy in zip(rot.as_quat(), rot.as_euler(seq, degrees=degrees)): + euler_cs = np.array(quat2euler(q)).flatten() + 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"): + 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(): + 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}"