Skip to content

Fix cs_quat2euler degrees and duplicate CasADi function names #152

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
malikdurmus wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix cs_quat2euler degrees and duplicate CasADi function names #152

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 7 additions & 3 deletions crazyflow/dynamics/utils/rotation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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()
Expand Down
30 changes: 30 additions & 0 deletions tests/unit/dynamics/test_rotation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)

@amacati amacati Nov 3, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This probably never reaches gimbal locks (which have some switching logic), so it would be great to add these as well. E.g.

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."

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,
]
Comment on lines +262 to +269

@amacati amacati Nov 3, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we collect these automatically instead?

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}"