Skip to content

rotmat: fix to_euler() pitch at the gimbal-lock limits - #1269

Open
dylanpulver wants to merge 1 commit into
ArduPilot:masterfrom
dylanpulver:fix-rotmat-to-euler-pitch-limits
Open

rotmat: fix to_euler() pitch at the gimbal-lock limits#1269
dylanpulver wants to merge 1 commit into
ArduPilot:masterfrom
dylanpulver:fix-rotmat-to-euler-pitch-limits

Conversation

@dylanpulver

Copy link
Copy Markdown

Matrix3.to_euler() computes pitch as -asin(c.x), but the two branches that guard |c.x| > 1 return +/-pi instead of the limits of that expression, -/+pi/2 (rotmat.py:199-202). from_euler() sets c.x = -sin(pitch), which is exactly -/+1.0 at pitch +/-90 deg, so those branches are reached by an ordinary round trip:

>>> m = Matrix3(); m.from_euler(0, math.radians(-90), 0)
>>> m.to_euler()
(0.0, 3.141592653589793, 0.0)      # +180 deg of pitch, and the sign is flipped too

after this change: (0.0, -1.5707963267948966, 0.0).

AP_Math's Matrix3<T>::to_euler() uses -safe_asin(c.x), and safe_asin() clamps to +/-M_PI_2, so the C++ library this file says it is a port of already returns -/+90 deg here.

The branches are unreachable from the suite by construction: test_euler() iterates for p in range(-89, 89, 10). The code and that loop arrived in the same commit (823f8fe, 2012). The new test asserts the recovered pitch in degrees and that the recovered angles rebuild the same matrix.

Ran, at the repo root with the message definitions symlinked as CI does: pytest 60 passed / 8 skipped before, 61 / 8 after; flake8 --select=E9,F63,F7,F82 count 0. Non-vacuity: reverting rotmat.py fails the new test; so does a variant with the right magnitude but the original signs, and one that fixes only the c.x >= 1.0 branch.

Not tested: nothing outside the exactly-saturated inputs changes, and I did not exercise any downstream consumer (MAVProxy, mavextra's DCM estimators). quaternion.py's own _dcm_to_euler() has a separate, unrelated problem in its theta == -pi/2 branch; I left that out of this PR.

How I found it: a systematic pass over the Euler/DCM/quaternion conversions in this repo looking for branches whose returned value disagrees with the expression they are meant to clamp. I did not hit this in flight data. AI-assisted (Claude) for the search, the patch and the test, stated per ArduPilot's AGENTS.md.

Matrix3.to_euler() computes pitch as -asin(c.x), but the branches that
guard against |c.x| > 1 returned +/-pi instead of the limits of that
expression, -/+pi/2. from_euler(0, +/-pi/2, 0) sets c.x to exactly
-/+1.0, so a plain from_euler/to_euler round trip through the vertical
attitude comes back with 180 degrees of pitch, with the sign flipped
as well.

AP_Math's Matrix3<T>::to_euler() uses -safe_asin(c.x), and safe_asin
clamps to +/-M_PI_2, so this brings rotmat.py back in line with the
C++ library it is a port of.

test_euler() covers pitch in range(-89, 89, 10), so neither branch is
reachable from the existing tests. Added a test that asserts the
recovered pitch in degrees and that the recovered angles rebuild the
same matrix.

Co-Authored-By: Claude <noreply@anthropic.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant