Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Implement Rotation3d interpolation as slerp instead of lerp (#8529)

Browse Source 
Also replace arithmetic operators since they're not commutative, which
is confusing for users.
This commit is contained in:
Tyler Veness
2026-03-06 15:15:00 -08:00
committed by GitHub
parent 28176f1062
commit b29bde123f
22 changed files with 142 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp
View File

@@ -86,7 +86,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
const CoordinateSystem& from,
const CoordinateSystem& to) {
// Convert to NWU, then convert to the new coordinate system
return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
return translation.RotateBy(from.m_rotation)
.RotateBy(to.m_rotation.Inverse());
}
/**
@@ -101,7 +102,7 @@ class WPILIB_DLLEXPORT CoordinateSystem {
const CoordinateSystem& from,
const CoordinateSystem& to) {
// Convert to NWU, then convert to the new coordinate system
return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
return rotation.RotateBy(from.m_rotation).RotateBy(to.m_rotation.Inverse());
}
/**
@@ -133,7 +134,7 @@ class WPILIB_DLLEXPORT CoordinateSystem {
// coordRot is the rotation that converts between the coordinate systems
// when applied extrinsically. It first converts to NWU, then converts to
// the new coordinate system.
const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation);
const auto coordRot = from.m_rotation.RotateBy(to.m_rotation.Inverse());
// The new rotation is the extrinsic rotation from convert(zero) to
// convert(transformRot). That is, applying convertedRot extrinsically to
// convert(zero) produces convert(transformRot). In the below snippet, we
@@ -145,10 +146,10 @@ class WPILIB_DLLEXPORT CoordinateSystem {
// = (coordRot transformRot) coordRot⁻¹
//
// In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the
// change in order), and the equivalent for rot⁻¹ is -rot.
// change in order).
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
coordRot.Inverse().RotateBy(transform.Rotation().RotateBy(coordRot))};
}
private:

22
wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp
View File

@@ -256,9 +256,14 @@ class WPILIB_DLLEXPORT Pose3d {
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
return gcem::abs(this->Rotation()
.RelativeTo(a.Rotation())
.Angle()
.value()) <
gcem::abs(this->Rotation()
.RelativeTo(b.Rotation())
.Angle()
.value());
}
return aDistance < bDistance;
});
@@ -281,9 +286,14 @@ class WPILIB_DLLEXPORT Pose3d {
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
return gcem::abs(this->Rotation()
.RelativeTo(a.Rotation())
.Angle()
.value()) <
gcem::abs(this->Rotation()
.RelativeTo(b.Rotation())
.Angle()
.value());
}
return aDistance < bDistance;
});

18
wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp
View File

@@ -157,15 +157,6 @@ class WPILIB_DLLEXPORT Quaternion {
*/
constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); }
/**
* Matrix exponential of a quaternion.
*
* @param other the "Twist" that will be applied to this quaternion.
*/
constexpr Quaternion Exp(const Quaternion& other) const {
return other.Exp() * *this;
}
/**
* Matrix exponential of a quaternion.
*
@@ -196,15 +187,6 @@ class WPILIB_DLLEXPORT Quaternion {
Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
}
/**
* Log operator of a quaternion.
*
* @param other The quaternion to map this quaternion onto
*/
constexpr Quaternion Log(const Quaternion& other) const {
return (other * Inverse()).Log();
}
/**
* Log operator of a quaternion.
*

3
wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp
View File

@@ -118,8 +118,7 @@ class WPILIB_DLLEXPORT Rotation2d {
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
* Returns this rotation relative to another rotation.
*
* For example, <code>Rotation2d{10_deg} - Rotation2d{100_deg}</code> equals
* <code>Rotation2d{wpi::units::radian_t{-std::numbers::pi/2.0}}</code>

79
wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp
View File

@@ -282,53 +282,12 @@ class WPILIB_DLLEXPORT Rotation3d {
constexpr explicit Rotation3d(const Rotation2d& rotation)
: Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
/**
* Adds two rotations together. The other rotation is applied extrinsically to
* this rotation, which is equivalent to this rotation being applied
* intrinsically to the other rotation. See the class comment for definitions
* of extrinsic and intrinsic rotations.
*
* Note that `a - b + b` always equals `a`, but `b + (a - b)`
* sometimes doesn't. To apply a rotation offset, use either `offset =
* -measurement + actual; newAngle = angle + offset;` or `offset = actual -
* measurement; newAngle = offset + angle;`, depending on how the corrected
* angle needs to change as the input angle changes.
*
* @param other The rotation to add (applied extrinsically).
*
* @return The sum of the two rotations.
*/
constexpr Rotation3d operator+(const Rotation3d& other) const {
return RotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation. The new rotation is from the perspective of the other rotation
* (like Pose3d::operator-), so it needs to be applied intrinsically. See the
* class comment for definitions of extrinsic and intrinsic rotations.
*
* Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes
* doesn't. To apply a rotation offset, use either `offset = -measurement +
* actual; newAngle = angle + offset;` or `offset = actual - measurement;
* newAngle = offset + angle;`, depending on how the corrected angle needs to
* change as the input angle changes.
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations, from the perspective of
* the other rotation.
*/
constexpr Rotation3d operator-(const Rotation3d& other) const {
return *this + -other;
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; }
constexpr Rotation3d Inverse() const { return Rotation3d{m_q.Inverse()}; }
/**
* Multiplies the current rotation by a scalar.
@@ -338,16 +297,7 @@ class WPILIB_DLLEXPORT Rotation3d {
* @return The new scaled Rotation3d.
*/
constexpr Rotation3d operator*(double scalar) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{
Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(m_q.W())}};
} else {
return Rotation3d{
Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(-m_q.W())}};
}
return Rotation3d{}.Interpolate(*this, scalar);
}
/**
@@ -406,6 +356,29 @@ class WPILIB_DLLEXPORT Rotation3d {
return Rotation3d{other.m_q.Inverse() * m_q};
}
/**
* Interpolates between this rotation and another.
*
* @param endValue The other rotation.
* @param t How far between the two rotations we are (0 means this rotation, 1
* means other rotation).
* @return The interpolated value.
*/
constexpr Rotation3d Interpolate(Rotation3d endValue, double t) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
//
// slerp(q₀, q₁, t) = (q₁q₀⁻¹)ᵗq₀
//
// We negate the delta quaternion if necessary to take the shortest path
const auto& q0 = m_q;
const auto& q1 = endValue.m_q;
auto delta = q1 * q0.Inverse();
if (delta.W() < 0.0) {
delta = Quaternion{-delta.W(), -delta.X(), -delta.Y(), -delta.Z()};
}
return Rotation3d{delta.Pow(t) * q0};
}
/**
* Returns the quaternion representation of the Rotation3d.
*/
@@ -537,7 +510,7 @@ template <>
constexpr wpi::math::Rotation3d wpi::util::Lerp(
const wpi::math::Rotation3d& startValue,
const wpi::math::Rotation3d& endValue, double t) {
return (endValue - startValue) * t + startValue;
return startValue.Interpolate(endValue, t);
}
#include "wpi/math/geometry/proto/Rotation3dProto.hpp"

5
wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp
View File

@@ -153,7 +153,8 @@ class WPILIB_DLLEXPORT Transform3d {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
return Transform3d{(-Translation()).RotateBy(Rotation().Inverse()),
Rotation().Inverse()};
}
/**
@@ -207,7 +208,7 @@ constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
// To transform the global translation delta to be relative to the initial
// pose, rotate by the inverse of the initial pose's orientation.
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
.RotateBy(initial.Rotation().Inverse());
m_rotation = final.Rotation().RelativeTo(initial.Rotation());
}

15
wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp
View File

@@ -36,7 +36,7 @@ class TimeInterpolatableBuffer {
/**
* Create a new TimeInterpolatableBuffer.
*
* @param historySize The history size of the buffer.
* @param historySize The history size of the buffer.
* @param func The function used to interpolate between values.
*/
TimeInterpolatableBuffer(wpi::units::second_t historySize,
@@ -45,15 +45,20 @@ class TimeInterpolatableBuffer {
/**
* Create a new TimeInterpolatableBuffer. By default, the interpolation
* function is wpi::util::Lerp except for Pose2d, which uses the pose
* exponential.
* function is wpi::util::Lerp. If the arithmetic operators aren't supported
* (usually because they wouldn't be commutative), Interpolate() is used
* instead.
*
* @param historySize The history size of the buffer.
* @param historySize The history size of the buffer.
*/
explicit TimeInterpolatableBuffer(wpi::units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const T& start, const T& end, double t) {
return wpi::util::Lerp(start, end, t);
if constexpr (requires(T a, T b, double t) { a + (b - a) * t; }) {
return wpi::util::Lerp(start, end, t);
} else {
return start.Interpolate(end, t);
}
}) {}
/**

13
wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp
View File

@@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT Odometry3d {
m_previousAngle = m_pose.Rotation();
// When applied extrinsically, m_gyroOffset cancels the
// current gyroAngle and then rotates to m_pose.Rotation()
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
m_gyroOffset = gyroAngle.Inverse().RotateBy(m_pose.Rotation());
}
/**
@@ -68,7 +68,7 @@ class WPILIB_DLLEXPORT Odometry3d {
m_previousAngle = pose.Rotation();
// When applied extrinsically, m_gyroOffset cancels the
// current gyroAngle and then rotates to m_pose.Rotation()
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
m_gyroOffset = gyroAngle.Inverse().RotateBy(m_pose.Rotation());
m_previousWheelPositions = wheelPositions;
}
@@ -79,8 +79,8 @@ class WPILIB_DLLEXPORT Odometry3d {
*/
void ResetPose(const Pose3d& pose) {
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
m_gyroOffset =
m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation().Inverse())
.RotateBy(pose.Rotation());
m_pose = pose;
m_previousAngle = pose.Rotation();
}
@@ -101,7 +101,8 @@ class WPILIB_DLLEXPORT Odometry3d {
*/
void ResetRotation(const Rotation3d& rotation) {
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation);
m_gyroOffset =
m_gyroOffset.RotateBy(m_pose.Rotation().Inverse()).RotateBy(rotation);
m_pose = Pose3d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}
@@ -126,7 +127,7 @@ class WPILIB_DLLEXPORT Odometry3d {
const Pose3d& Update(const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions) {
auto angle = gyroAngle.RotateBy(m_gyroOffset);
auto angle_difference = (angle - m_previousAngle).ToVector();
auto angle_difference = angle.RelativeTo(m_previousAngle).ToVector();
auto twist2d =
m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);