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

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

View File

@@ -14,6 +14,8 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
#include "units/time.h"
namespace frc {
@@ -155,7 +157,8 @@ class TimeInterpolatableBuffer {
std::function<T(const T&, const T&, double)> m_interpolatingFunc;
};
// Template specialization to ensure that Pose2d uses pose exponential
// Template specializations to ensure that Pose2d and Pose3d use pose
// exponential
template <>
inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
units::second_t historySize)
@@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
}
}) {}
template <>
inline TimeInterpolatableBuffer<Pose3d>::TimeInterpolatableBuffer(
units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const Pose3d& start, const Pose3d& end, double t) {
if (t < 0) {
return start;
} else if (t >= 1) {
return end;
} else {
Twist3d twist = start.Log(end);
Twist3d scaledTwist = twist * t;
return start.Exp(scaledTwist);
}
}) {}
} // namespace frc