rotmat: fix to_euler() pitch at the gimbal-lock limits - #1269
Open
dylanpulver wants to merge 1 commit into
Open
Conversation
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>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Matrix3.to_euler()computes pitch as-asin(c.x), but the two branches that guard|c.x| > 1return+/-piinstead of the limits of that expression,-/+pi/2(rotmat.py:199-202).from_euler()setsc.x = -sin(pitch), which is exactly-/+1.0at pitch+/-90deg, so those branches are reached by an ordinary round trip:after this change:
(0.0, -1.5707963267948966, 0.0).AP_Math's
Matrix3<T>::to_euler()uses-safe_asin(c.x), andsafe_asin()clamps to+/-M_PI_2, so the C++ library this file says it is a port of already returns-/+90deg here.The branches are unreachable from the suite by construction:
test_euler()iteratesfor 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:
pytest60 passed / 8 skipped before, 61 / 8 after;flake8 --select=E9,F63,F7,F82count 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 thec.x >= 1.0branch.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 itstheta == -pi/2branch; 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.