mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user