Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix Rotation3d interpolation and document extrinsic vs intrinsic (#8544)

Browse Source 
Documents the extrinsic vs intrinsic semantics of `plus()` and
`minus()`. (`rotateBy()` was documented in [a previous
PR](https://github.com/wpilibsuite/allwpilib/pull/5508))
Fixes usage of `plus()` and `minus()` in `Rotation3d.interpolate()`.
(Fixes #8523)
Fixes incorrect usages of `plus()`, `minus()`, and `rotateBy()`
throughout `Odometry3d`.
Adds explanatory comments for some `plus()`, `minus()`, and `rotateBy()`
operations.
Fixes `TimeInterpolatableBuffer` not using twists for `Pose3d` (this was
just because I happened to notice it, it isn't really related to the PR)

To check all of our usages of `plus()`, `minus()`, and `rotateBy()`, I
marked them as deprecated, checked compile errors from `./gradlew
compileJava`, and then undeprecated them. You can see all of the spots
that showed up (at least on the Java side) by viewing the diff for
241109c.

I wanted to present this alternative to #8526 because the change has its
own quirks, there's little time before kickoff, and there would be no
code-side warning to teams (and mentors) already used to the current
behavior.
This commit is contained in:
Joseph Eng
2026-01-14 20:16:24 -08:00
committed by GitHub
parent 812a1b8e1a
commit 9e1258440b
29 changed files with 561 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp
View File

@@ -222,3 +222,18 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
EXPECT_LT(maxError, 0.125);
}
TEST_F(SwerveDriveOdometry3dTest, GyroOffset) {
m_odometry.ResetPosition(
frc::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero},
frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg},
{zero, zero, zero, zero});
EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
}