[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.

wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h

@@ -86,7 +86,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Translation3d Convert(const Translation3d& translation,
                                          const CoordinateSystem& from,
                                          const CoordinateSystem& to) {
-    return translation.RotateBy(from.m_rotation - to.m_rotation);
+    // Convert to NWU, then convert to the new coordinate system
+    return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
   }
 
   /**
@@ -100,7 +101,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Rotation3d Convert(const Rotation3d& rotation,
                                       const CoordinateSystem& from,
                                       const CoordinateSystem& to) {
-    return rotation.RotateBy(from.m_rotation - to.m_rotation);
+    // Convert to NWU, then convert to the new coordinate system
+    return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
   }
 
   /**
@@ -129,14 +131,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Transform3d Convert(const Transform3d& transform,
                                        const CoordinateSystem& from,
                                        const CoordinateSystem& to) {
-    const auto coordRot = from.m_rotation - to.m_rotation;
+    // 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);
+    // 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
+    // use matrix notation, so rotA rotB applies rotA extrinsically to rotB.
+    //
+    //   convertedRot convert(zero) = convert(transformRot)
+    //   convertedRot = convert(transformRot) convert(zero)⁻¹
+    //                = (coordRot transformRot) (coordRot zero)⁻¹
+    //                = (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.
     return Transform3d{
         Convert(transform.Translation(), from, to),
         (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
   }
 
  private:
-  // Rotation from this coordinate system to NWU coordinate system
+  // Rotation from this coordinate system to NWU coordinate system when applied
+  // extrinsically
   Rotation3d m_rotation;
 };
 

wpimath/src/main/native/include/frc/geometry/Pose2d.h

@@ -320,7 +320,7 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
 
 constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
-          other.Rotation() + m_rotation};
+          other.Rotation().RotateBy(m_rotation)};
 }
 
 constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -392,8 +392,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
 }
 
 constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  // Rotating the transform's rotation by the pose's rotation extrinsically is
+  // equivalent to rotating the pose's rotation by the transform's rotation
+  // intrinsically. (We define transforms as being applied intrinsically.)
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
-          other.Rotation() + m_rotation};
+          other.Rotation().RotateBy(m_rotation)};
 }
 
 constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {

wpimath/src/main/native/include/frc/geometry/Rotation2d.h

@@ -198,6 +198,19 @@ class WPILIB_DLLEXPORT Rotation2d {
             Cos() * other.Sin() + Sin() * other.Cos()};
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation.
+   *
+   * @param other The rotation describing the orientation of the new coordinate
+   * frame that the current rotation will be converted into.
+   *
+   * @return The current rotation relative to the new orientation of the
+   * coordinate frame.
+   */
+  constexpr Rotation2d RelativeTo(const Rotation2d& other) const {
+    return RotateBy(-other);
+  }
+
   /**
    * Returns matrix representation of this rotation.
    */

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -10,6 +10,7 @@
 #include <Eigen/Core>
 #include <fmt/format.h>
 #include <gcem.hpp>
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -24,7 +25,49 @@
 namespace frc {
 
 /**
- * A rotation in a 3D coordinate frame represented by a quaternion.
+ * A rotation in a 3D coordinate frame, represented by a quaternion. Note that
+ * unlike 2D rotations, 3D rotations are not always commutative, so changing the
+ * order of rotations changes the result.
+ *
+ * As an example of the order of rotations mattering, suppose we have a card
+ * that looks like this:
+ *
+ * <pre>
+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ * </pre>
+ *
+ * If we rotate 90º clockwise around the axis into the page, then flip around
+ * the left/right axis, we get one result:
+ *
+ * <pre>
+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ * </pre>
+ *
+ * If we flip around the left/right axis, then rotate 90º clockwise around the
+ * axis into the page, we get a different result:
+ *
+ * <pre>
+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ * </pre>
+ *
+ * Because order matters for 3D rotations, we need to distinguish between
+ * <em>extrinsic</em> rotations and <em>intrinsic</em> rotations. Rotating
+ * extrinsically means rotating around the global axes, while rotating
+ * intrinsically means rotating from the perspective of the other rotation. A
+ * neat property is that applying a series of rotations extrinsically is the
+ * same as applying the same series in the opposite order intrinsically.
  */
 class WPILIB_DLLEXPORT Rotation3d {
  public:
@@ -242,9 +285,18 @@ class WPILIB_DLLEXPORT Rotation3d {
       : Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
 
   /**
-   * Adds two rotations together.
+   * 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.
    *
-   * @param other The rotation to add.
+   * 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.
    */
@@ -254,11 +306,20 @@ class WPILIB_DLLEXPORT Rotation3d {
 
   /**
    * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
+   * 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.
+   * @return The difference between the two rotations, from the perspective of
+   * the other rotation.
    */
   constexpr Rotation3d operator-(const Rotation3d& other) const {
     return *this + -other;
@@ -323,6 +384,28 @@ class WPILIB_DLLEXPORT Rotation3d {
     return Rotation3d{other.m_q * m_q};
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation. Because the
+   * result is in the perspective of the given rotation, it must be applied
+   * intrinsically. See the class comment for definitions of extrinsic and
+   * intrinsic rotations.
+   *
+   * @param other The rotation describing the orientation of the new coordinate
+   * frame that the current rotation will be converted into.
+   *
+   * @return The current rotation relative to the new orientation of the
+   * coordinate frame.
+   */
+  constexpr Rotation3d RelativeTo(const Rotation3d& other) const {
+    // To apply a quaternion intrinsically, we must right-multiply by that
+    // quaternion. Therefore, "this_q relative to other_q" is the q such that
+    // other_q q = this_q:
+    //
+    //   other_q q = this_q
+    //   q = other_q⁻¹ this_q
+    return Rotation3d{other.m_q.Inverse() * m_q};
+  }
+
   /**
    * Returns the quaternion representation of the Rotation3d.
    */
@@ -449,5 +532,11 @@ void from_json(const wpi::json& json, Rotation3d& rotation);
 
 }  // namespace frc
 
+template <>
+constexpr frc::Rotation3d wpi::Lerp(const frc::Rotation3d& startValue,
+                                    const frc::Rotation3d& endValue, double t) {
+  return (endValue - startValue) * t + startValue;
+}
+
 #include "frc/geometry/proto/Rotation3dProto.h"
 #include "frc/geometry/struct/Rotation3dStruct.h"

wpimath/src/main/native/include/frc/geometry/Transform2d.h

@@ -167,13 +167,12 @@ class WPILIB_DLLEXPORT Transform2d {
 namespace frc {
 
 constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
-  // 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).
+  // 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());
 
-  m_rotation = final.Rotation() - initial.Rotation();
+  m_rotation = final.Rotation().RelativeTo(initial.Rotation());
 }
 
 constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {

wpimath/src/main/native/include/frc/geometry/Transform3d.h

@@ -16,7 +16,9 @@ namespace frc {
 class Pose3d;
 
 /**
- * Represents a transformation for a Pose3d in the pose's frame.
+ * Represents a transformation for a Pose3d in the pose's frame. Translation is
+ * applied before rotation. (The translation is applied in the pose's original
+ * frame, not the transformed frame.)
  */
 class WPILIB_DLLEXPORT Transform3d {
  public:
@@ -192,13 +194,12 @@ class WPILIB_DLLEXPORT Transform3d {
 namespace frc {
 
 constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
-  // 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).
+  // 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());
 
-  m_rotation = final.Rotation() - initial.Rotation();
+  m_rotation = final.Rotation().RelativeTo(initial.Rotation());
 }
 
 constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -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

wpimath/src/main/native/include/frc/kinematics/Odometry.h

@@ -45,7 +45,7 @@ class WPILIB_DLLEXPORT Odometry {
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
     m_previousAngle = m_pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
   }
 
   /**
@@ -62,7 +62,7 @@ class WPILIB_DLLEXPORT Odometry {
                      const WheelPositions& wheelPositions, const Pose2d& pose) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
     m_previousWheelPositions = wheelPositions;
   }
 
@@ -72,7 +72,8 @@ class WPILIB_DLLEXPORT Odometry {
    * @param pose The pose to reset to.
    */
   void ResetPose(const Pose2d& pose) {
-    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    m_gyroOffset =
+        m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
     m_pose = pose;
     m_previousAngle = pose.Rotation();
   }
@@ -92,7 +93,7 @@ class WPILIB_DLLEXPORT Odometry {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation2d& rotation) {
-    m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
+    m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation);
     m_pose = Pose2d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }
@@ -116,7 +117,7 @@ class WPILIB_DLLEXPORT Odometry {
    */
   const Pose2d& Update(const Rotation2d& gyroAngle,
                        const WheelPositions& wheelPositions) {
-    auto angle = gyroAngle + m_gyroOffset;
+    auto angle = gyroAngle.RotateBy(m_gyroOffset);
 
     auto twist =
         m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
@@ -136,7 +137,10 @@ class WPILIB_DLLEXPORT Odometry {
   Pose2d m_pose;
 
   WheelPositions m_previousWheelPositions;
+
+  // Always equal to m_pose.Rotation()
   Rotation2d m_previousAngle;
+
   Rotation2d m_gyroOffset;
 };
 

wpimath/src/main/native/include/frc/kinematics/Odometry3d.h

@@ -48,7 +48,9 @@ class WPILIB_DLLEXPORT Odometry3d {
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
     m_previousAngle = m_pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    // When applied extrinsically, m_gyroOffset cancels the
+    // current gyroAngle and then rotates to m_pose.Rotation()
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
   }
 
   /**
@@ -65,7 +67,9 @@ class WPILIB_DLLEXPORT Odometry3d {
                      const WheelPositions& wheelPositions, const Pose3d& pose) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    // When applied extrinsically, m_gyroOffset cancels the
+    // current gyroAngle and then rotates to m_pose.Rotation()
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
     m_previousWheelPositions = wheelPositions;
   }
 
@@ -75,7 +79,9 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param pose The pose to reset to.
    */
   void ResetPose(const Pose3d& pose) {
-    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    // 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_pose = pose;
     m_previousAngle = pose.Rotation();
   }
@@ -95,7 +101,8 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation3d& rotation) {
-    m_gyroOffset = m_gyroOffset + (rotation - m_pose.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_pose = Pose3d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }
@@ -119,7 +126,7 @@ class WPILIB_DLLEXPORT Odometry3d {
    */
   const Pose3d& Update(const Rotation3d& gyroAngle,
                        const WheelPositions& wheelPositions) {
-    auto angle = gyroAngle + m_gyroOffset;
+    auto angle = gyroAngle.RotateBy(m_gyroOffset);
     auto angle_difference = (angle - m_previousAngle).ToVector();
 
     auto twist2d =
@@ -145,7 +152,14 @@ class WPILIB_DLLEXPORT Odometry3d {
   Pose3d m_pose;
 
   WheelPositions m_previousWheelPositions;
+
+  // Always equal to m_pose.Rotation()
   Rotation3d m_previousAngle;
+
+  // Applying a rotation intrinsically to the measured gyro angle should cause
+  // the corrected angle to be rotated intrinsically in the same way, so the
+  // measured gyro angle must be applied intrinsically. This is equivalent to
+  // applying the offset extrinsically to the measured gyro angle.
   Rotation3d m_gyroOffset;
 };