[wpimath] Add 3D geometry classes (#4175)

Also clean up 2D geometry documentation.

wpimath/src/main/native/cpp/geometry/Pose2d.cpp

@@ -11,10 +11,10 @@
 using namespace frc;
 
 Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
 Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
-    : m_translation(x, y), m_rotation(rotation) {}
+    : m_translation(x, y), m_rotation(std::move(rotation)) {}
 
 Pose2d Pose2d::operator+(const Transform2d& other) const {
   return TransformBy(other);

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Pose3d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+namespace {
+
+/**
+ * Applies the hat operator to a rotation vector.
+ *
+ * It takes a rotation vector and returns the corresponding matrix
+ * representation of the Lie algebra element (a 3x3 rotation matrix).
+ *
+ * @param rotation The rotation vector.
+ * @return The rotation vector as a 3x3 rotation matrix.
+ */
+Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+  // Given a rotation vector <a, b, c>,
+  //         [ 0 -c  b]
+  // Omega = [ c  0 -a]
+  //         [-b  a  0]
+  return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
+                       {rotation(2), 0.0, -rotation(0)},
+                       {-rotation(1), rotation(0), 0.0}};
+}
+}  // namespace
+
+Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+               Rotation3d rotation)
+    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
+Pose3d Pose3d::operator+(const Transform3d& other) const {
+  return TransformBy(other);
+}
+
+Transform3d Pose3d::operator-(const Pose3d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform3d(pose.Translation(), pose.Rotation());
+}
+
+bool Pose3d::operator==(const Pose3d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Pose3d::operator!=(const Pose3d& other) const {
+  return !operator==(other);
+}
+
+Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          m_rotation + other.Rotation()};
+}
+
+Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
+  const Transform3d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+Pose3d Pose3d::Exp(const Twist3d& twist) const {
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(
+      Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  double thetaSq =
+      (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
+
+  // Get left Jacobian of SO3. See first line in right column of
+  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+  Matrixd<3, 3> J;
+  if (thetaSq < 1E-9 * 1E-9) {
+    // V = I + 0.5ω
+    J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+  } else {
+    double theta = std::sqrt(thetaSq);
+    // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
+    J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
+        (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+  }
+
+  // Get translation component
+  Vectord<3> translation =
+      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+
+  const Transform3d transform{Translation3d{units::meter_t{translation(0)},
+                                            units::meter_t{translation(1)},
+                                            units::meter_t{translation(2)}},
+                              Rotation3d{twist.rx, twist.ry, twist.rz}};
+
+  return *this + transform;
+}
+
+Twist3d Pose3d::Log(const Pose3d& end) const {
+  const auto transform = end.RelativeTo(*this);
+
+  Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  double thetaSq = rotVec.squaredNorm();
+
+  // Get left Jacobian inverse of SO3. See fourth line in right column of
+  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+  Matrixd<3, 3> Jinv;
+  if (thetaSq < 1E-9 * 1E-9) {
+    // J⁻¹ = I − 0.5ω + 1/12 ω²
+    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+  } else {
+    double theta = std::sqrt(thetaSq);
+    double halfTheta = 0.5 * theta;
+
+    // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
+    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
+           (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
+               thetaSq * OmegaSq;
+  }
+
+  // Get dtranslation component
+  Vectord<3> dtranslation =
+      Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
+                        transform.Z().value()};
+
+  return Twist3d{
+      units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
+      units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
+      units::radian_t{rotVec(1)},      units::radian_t{rotVec(2)}};
+}
+
+Pose2d Pose3d::ToPose2d() const {
+  return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
+}

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Quaternion.h"
+
+using namespace frc;
+
+Quaternion::Quaternion(double w, double x, double y, double z)
+    : m_r{w}, m_v{x, y, z} {}
+
+Quaternion Quaternion::operator*(const Quaternion& other) const {
+  // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+  const auto& r1 = m_r;
+  const auto& v1 = m_v;
+  const auto& r2 = other.m_r;
+  const auto& v2 = other.m_v;
+
+  Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
+                        v2(0) * v1(2) - v1(0) * v2(2),
+                        v1(0) * v2(1) - v2(0) * v1(1)};
+
+  Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
+  return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+}
+
+bool Quaternion::operator==(const Quaternion& other) const {
+  return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+}
+
+bool Quaternion::operator!=(const Quaternion& other) const {
+  return !operator==(other);
+}
+
+Quaternion Quaternion::Inverse() const {
+  return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+}
+
+Quaternion Quaternion::Normalize() const {
+  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  if (norm == 0.0) {
+    return Quaternion{};
+  } else {
+    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+  }
+}
+
+double Quaternion::W() const {
+  return m_r;
+}
+
+double Quaternion::X() const {
+  return m_v(0);
+}
+
+double Quaternion::Y() const {
+  return m_v(1);
+}
+
+double Quaternion::Z() const {
+  return m_v(2);
+}
+
+Eigen::Vector3d Quaternion::ToRotationVector() const {
+  // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+  // Sound State Representation through Encapsulation of Manifolds"
+  //
+  // https://arxiv.org/pdf/1107.1119.pdf
+  double norm = m_v.norm();
+
+  if (norm < 1e-9) {
+    return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
+  } else {
+    if (W() < 0.0) {
+      return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
+    } else {
+      return 2.0 * std::atan2(norm, W()) / norm * m_v;
+    }
+  }
+}

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -0,0 +1,138 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Rotation3d.h"
+
+#include <cmath>
+
+#include <wpi/numbers>
+
+#include "units/math.h"
+
+using namespace frc;
+
+Rotation3d::Rotation3d(const Quaternion& q) {
+  m_q = q.Normalize();
+}
+
+Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
+                       units::radian_t yaw) {
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+  double cr = units::math::cos(roll * 0.5);
+  double sr = units::math::sin(roll * 0.5);
+
+  double cp = units::math::cos(pitch * 0.5);
+  double sp = units::math::sin(pitch * 0.5);
+
+  double cy = units::math::cos(yaw * 0.5);
+  double sy = units::math::sin(yaw * 0.5);
+
+  m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
+                   cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+  double norm = axis.norm();
+  if (norm == 0.0) {
+    return;
+  }
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+  Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+  m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
+}
+
+Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
+  return RotateBy(other);
+}
+
+Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
+  return *this + -other;
+}
+
+Rotation3d Rotation3d::operator-() const {
+  return Rotation3d{m_q.Inverse()};
+}
+
+Rotation3d Rotation3d::operator*(double scalar) const {
+  // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+  if (m_q.W() >= 0.0) {
+    return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
+  } else {
+    return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
+  }
+}
+
+bool Rotation3d::operator==(const Rotation3d& other) const {
+  return m_q == other.m_q;
+}
+
+bool Rotation3d::operator!=(const Rotation3d& other) const {
+  return !operator==(other);
+}
+
+Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
+  return Rotation3d{other.m_q * m_q};
+}
+
+const Quaternion& Rotation3d::GetQuaternion() const {
+  return m_q;
+}
+
+units::radian_t Rotation3d::X() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  return units::radian_t{
+      std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+}
+
+units::radian_t Rotation3d::Y() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  double ratio = 2.0 * (w * y - z * x);
+  if (std::abs(ratio) >= 1.0) {
+    return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)};
+  } else {
+    return units::radian_t{std::asin(ratio)};
+  }
+}
+
+units::radian_t Rotation3d::Z() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  return units::radian_t{
+      std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+}
+
+Vectord<3> Rotation3d::Axis() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  if (norm == 0.0) {
+    return {0.0, 0.0, 0.0};
+  } else {
+    return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
+  }
+}
+
+units::radian_t Rotation3d::Angle() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
+}
+
+Rotation2d Rotation3d::ToRotation2d() const {
+  return Rotation2d{Z()};
+}

wpimath/src/main/native/cpp/geometry/Transform2d.cpp

@@ -19,7 +19,7 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
 }
 
 Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
 Transform2d Transform2d::Inverse() const {
   // We are rotating the difference between the translations

wpimath/src/main/native/cpp/geometry/Transform3d.cpp

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Transform3d.h"
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+Transform3d::Transform3d(Pose3d initial, 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).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Transform3d Transform3d::Inverse() const {
+  // 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()};
+}
+
+Transform3d Transform3d::operator+(const Transform3d& other) const {
+  return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
+}
+
+bool Transform3d::operator==(const Transform3d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Transform3d::operator!=(const Transform3d& other) const {
+  return !operator==(other);
+}

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Translation3d.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+Translation3d::Translation3d(units::meter_t x, units::meter_t y,
+                             units::meter_t z)
+    : m_x(x), m_y(y), m_z(z) {}
+
+Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
+  auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
+  m_x = rectangular.X();
+  m_y = rectangular.Y();
+  m_z = rectangular.Z();
+}
+
+units::meter_t Translation3d::Distance(const Translation3d& other) const {
+  return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
+                           units::math::pow<2>(other.m_y - m_y) +
+                           units::math::pow<2>(other.m_z - m_z));
+}
+
+units::meter_t Translation3d::Norm() const {
+  return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+}
+
+Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
+  Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
+  auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
+  return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
+                       units::meter_t{qprime.Z()}};
+}
+
+Translation2d Translation3d::ToTranslation2d() const {
+  return Translation2d{m_x, m_y};
+}
+
+Translation3d Translation3d::operator+(const Translation3d& other) const {
+  return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+}
+
+Translation3d Translation3d::operator-(const Translation3d& other) const {
+  return *this + -other;
+}
+
+Translation3d Translation3d::operator-() const {
+  return {-m_x, -m_y, -m_z};
+}
+
+Translation3d Translation3d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y, scalar * m_z};
+}
+
+Translation3d Translation3d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+bool Translation3d::operator==(const Translation3d& other) const {
+  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+         units::math::abs(m_y - other.m_y) < 1E-9_m &&
+         units::math::abs(m_z - other.m_z) < 1E-9_m;
+}
+
+bool Translation3d::operator!=(const Translation3d& other) const {
+  return !operator==(other);
+}

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

@@ -17,13 +17,12 @@ class json;
 namespace frc {
 
 /**
- * Represents a 2d pose containing translational and rotational elements.
+ * Represents a 2D pose containing translational and rotational elements.
  */
 class WPILIB_DLLEXPORT Pose2d {
  public:
   /**
    * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
    */
   constexpr Pose2d() = default;
 
@@ -36,8 +35,8 @@ class WPILIB_DLLEXPORT Pose2d {
   Pose2d(Translation2d translation, Rotation2d rotation);
 
   /**
-   * Convenience constructors that takes in x and y values directly instead of
-   * having to construct a Translation2d.
+   * Constructs a pose with x and y translations instead of a separate
+   * Translation2d.
    *
    * @param x The x component of the translational component of the pose.
    * @param y The y component of the translational component of the pose.
@@ -49,9 +48,11 @@ class WPILIB_DLLEXPORT Pose2d {
    * Transforms the pose by the given transformation and returns the new
    * transformed pose.
    *
+   * <pre>
    * [x_new]    [cos, -sin, 0][transform.x]
    * [y_new] += [sin,  cos, 0][transform.y]
-   * [t_new]    [0,    0,   1][transform.t]
+   * [t_new]    [  0,    0, 1][transform.t]
+   * </pre>
    *
    * @param other The transform to transform the pose by.
    *
@@ -152,7 +153,7 @@ class WPILIB_DLLEXPORT Pose2d {
    * @param twist The change in pose in the robot's coordinate frame since the
    * previous pose update. For example, if a non-holonomic robot moves forward
    * 0.01 meters and changes angle by 0.5 degrees since the previous pose
-   * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}.
    *
    * @return The new pose of the robot.
    */

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

@@ -0,0 +1,180 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Pose2d.h"
+#include "Transform3d.h"
+#include "Translation3d.h"
+#include "Twist3d.h"
+
+namespace frc {
+
+/**
+ * Represents a 3D pose containing translational and rotational elements.
+ */
+class WPILIB_DLLEXPORT Pose3d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   */
+  constexpr Pose3d() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose3d(Translation3d translation, Rotation3d rotation);
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+         Rotation3d rotation);
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose3d operator+(const Transform3d& other) const;
+
+  /**
+   * Returns the Transform3d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  Transform3d operator-(const Pose3d& other) const;
+
+  /**
+   * Checks equality between this Pose3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Pose3d& other) const;
+
+  /**
+   * Checks inequality between this Pose3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Pose3d& other) const;
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the pose's translation.
+   *
+   * @return The z component of the pose's translation.
+   */
+  units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose3d TransformBy(const Transform3d& other) const;
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   * the current pose will be converted into.
+   *
+   * @return The current pose relative to the new origin pose.
+   */
+  Pose3d RelativeTo(const Pose3d& other) const;
+
+  /**
+   * Obtain a new Pose3d from a (constant curvature) velocity.
+   *
+   * The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs exp() on the previous known
+   * field-relative pose with the argument being the twist, the user will
+   * receive the new field-relative pose.
+   *
+   * "Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the
+   * previous pose update. For example, if a non-holonomic robot moves forward
+   * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+   * update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
+   * 0.5_deg}}.
+   *
+   * @return The new pose of the robot.
+   */
+  Pose3d Exp(const Twist3d& twist) const;
+
+  /**
+   * Returns a Twist3d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   *
+   * @return The twist that maps this to end.
+   */
+  Twist3d Log(const Pose3d& end) const;
+
+  /**
+   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+   */
+  Pose2d ToPose2d() const;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Quaternion {
+ public:
+  /**
+   * Constructs a quaternion with a default angle of 0 degrees.
+   */
+  Quaternion() = default;
+
+  /**
+   * Constructs a quaternion with the given components.
+   *
+   * @param w W component of the quaternion.
+   * @param x X component of the quaternion.
+   * @param y Y component of the quaternion.
+   * @param z Z component of the quaternion.
+   */
+  Quaternion(double w, double x, double y, double z);
+
+  /**
+   * Multiply with another quaternion.
+   *
+   * @param other The other quaternion.
+   */
+  Quaternion operator*(const Quaternion& other) const;
+
+  /**
+   * Checks equality between this Quaternion and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Quaternion& other) const;
+
+  /**
+   * Checks inequality between this Quaternion and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Quaternion& other) const;
+
+  /**
+   * Returns the inverse of the quaternion.
+   */
+  Quaternion Inverse() const;
+
+  /**
+   * Normalizes the quaternion.
+   */
+  Quaternion Normalize() const;
+
+  /**
+   * Returns W component of the quaternion.
+   */
+  double W() const;
+
+  /**
+   * Returns X component of the quaternion.
+   */
+  double X() const;
+
+  /**
+   * Returns Y component of the quaternion.
+   */
+  double Y() const;
+
+  /**
+   * Returns Z component of the quaternion.
+   */
+  double Z() const;
+
+  /**
+   * Returns the rotation vector representation of this quaternion.
+   *
+   * This is also the log operator of SO(3).
+   */
+  Eigen::Vector3d ToRotationVector() const;
+
+ private:
+  double m_r = 1.0;
+  Eigen::Vector3d m_v{0.0, 0.0, 0.0};
+};
+
+}  // namespace frc

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

@@ -15,7 +15,7 @@ class json;
 namespace frc {
 
 /**
- * A rotation in a 2d coordinate frame represented by a point on the unit circle
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle
  * (cosine and sine).
  */
 class WPILIB_DLLEXPORT Rotation2d {

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

@@ -0,0 +1,153 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Quaternion.h"
+#include "Rotation2d.h"
+#include "frc/EigenCore.h"
+#include "units/angle.h"
+
+namespace frc {
+
+/**
+ * A rotation in a 3D coordinate frame.
+ */
+class WPILIB_DLLEXPORT Rotation3d {
+ public:
+  /**
+   * Constructs a Rotation3d with a default angle of 0 degrees.
+   */
+  Rotation3d() = default;
+
+  /**
+   * Constructs a Rotation3d from a quaternion.
+   *
+   * @param q The quaternion.
+   */
+  explicit Rotation3d(const Quaternion& q);
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * Extrinsic rotations occur in that order around the axes in the fixed global
+   * frame rather than the body frame.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll).
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
+   */
+  Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis
+   * doesn't have to be normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angle The rotation around the axis.
+   */
+  Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+
+  /**
+   * Adds two rotations together.
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  Rotation3d operator+(const Rotation3d& other) const;
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  Rotation3d operator-(const Rotation3d& other) const;
+
+  /**
+   * Takes the inverse of the current rotation.
+   *
+   * @return The inverse of the current rotation.
+   */
+  Rotation3d operator-() const;
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation3d.
+   */
+  Rotation3d operator*(double scalar) const;
+
+  /**
+   * Checks equality between this Rotation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Rotation3d& other) const;
+
+  /**
+   * Checks inequality between this Rotation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Rotation3d& other) const;
+
+  /**
+   * Adds the new rotation to the current rotation.
+   *
+   * @param other The rotation to rotate by.
+   *
+   * @return The new rotated Rotation3d.
+   */
+  Rotation3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Returns the quaternion representation of the Rotation3d.
+   */
+  const Quaternion& GetQuaternion() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll).
+   */
+  units::radian_t X() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch).
+   */
+  units::radian_t Y() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw).
+   */
+  units::radian_t Z() const;
+
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   */
+  Vectord<3> Axis() const;
+
+  /**
+   * Returns the angle in the axis-angle representation of this rotation.
+   */
+  units::radian_t Angle() const;
+
+  /**
+   * Returns a Rotation2d representing this Rotation3d projected into the X-Y
+   * plane.
+   */
+  Rotation2d ToRotation2d() const;
+
+ private:
+  Quaternion m_q;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Translation3d.h"
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Pose3d;
+
+/**
+ * Represents a transformation for a Pose3d.
+ */
+class WPILIB_DLLEXPORT Transform3d {
+ public:
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param final The final pose for the transformation.
+   */
+  Transform3d(Pose3d initial, Pose3d final);
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  Transform3d(Translation3d translation, Rotation3d rotation);
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform3d() = default;
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the transformation's translation.
+   *
+   * @return The z component of the transformation's translation.
+   */
+  units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  Transform3d Inverse() const;
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  Transform3d operator*(double scalar) const {
+    return Transform3d(m_translation * scalar, m_rotation * scalar);
+  }
+
+  /**
+   * Composes two transformations.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  Transform3d operator+(const Transform3d& other) const;
+
+  /**
+   * Checks equality between this Transform3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Transform3d& other) const;
+
+  /**
+   * Checks inequality between this Transform3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Transform3d& other) const;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+}  // namespace frc

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

@@ -16,12 +16,12 @@ class json;
 namespace frc {
 
 /**
- * Represents a translation in 2d space.
+ * Represents a translation in 2D space.
  * This object can be used to represent a point or a vector.
  *
  * This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
+ * When the robot is at the origin facing in the positive X direction, forward
+ * is positive X and left is positive Y.
  */
 class WPILIB_DLLEXPORT Translation2d {
  public:
@@ -49,10 +49,9 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d(units::meter_t distance, const Rotation2d& angle);
 
   /**
-   * Calculates the distance between two translations in 2d space.
+   * Calculates the distance between two translations in 2D space.
    *
-   * This function uses the pythagorean theorem to calculate the distance.
-   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
    *
    * @param other The translation to compute the distance to.
    *
@@ -63,14 +62,14 @@ class WPILIB_DLLEXPORT Translation2d {
   /**
    * Returns the X component of the translation.
    *
-   * @return The x component of the translation.
+   * @return The X component of the translation.
    */
   units::meter_t X() const { return m_x; }
 
   /**
    * Returns the Y component of the translation.
    *
-   * @return The y component of the translation.
+   * @return The Y component of the translation.
    */
   units::meter_t Y() const { return m_y; }
 
@@ -82,16 +81,18 @@ class WPILIB_DLLEXPORT Translation2d {
   units::meter_t Norm() const;
 
   /**
-   * Applies a rotation to the translation in 2d space.
+   * Applies a rotation to the translation in 2D space.
    *
    * This multiplies the translation vector by a counterclockwise rotation
    * matrix of the given angle.
    *
+   * <pre>
    * [x_new]   [other.cos, -other.sin][x]
    * [y_new] = [other.sin,  other.cos][y]
+   * </pre>
    *
-   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
-   * Translation2d of {0, 2}.
+   * For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will
+   * return a Translation2d of &lt;0, 2&gt;.
    *
    * @param other The rotation to rotate the translation by.
    *
@@ -100,11 +101,10 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d RotateBy(const Rotation2d& other) const;
 
   /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
+   * Returns the sum of two translations in 2D space.
    *
-   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
-   * Translation2d{3.0, 8.0}
+   * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} =
+   * Translation3d{3.0, 8.0}.
    *
    * @param other The translation to add.
    *
@@ -113,11 +113,10 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d operator+(const Translation2d& other) const;
 
   /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
+   * Returns the difference between two translations.
    *
    * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
-   * Translation2d{4.0, 2.0}
+   * Translation2d{4.0, 2.0}.
    *
    * @param other The translation to subtract.
    *
@@ -127,17 +126,17 @@ class WPILIB_DLLEXPORT Translation2d {
 
   /**
    * Returns the inverse of the current translation. This is equivalent to
-   * rotating by 180 degrees, flipping the point over both axes, or simply
-   * negating both components of the translation.
+   * rotating by 180 degrees, flipping the point over both axes, or negating all
+   * components of the translation.
    *
    * @return The inverse of the current translation.
    */
   Translation2d operator-() const;
 
   /**
-   * Multiplies the translation by a scalar and returns the new translation.
+   * Returns the translation multiplied by a scalar.
    *
-   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}.
    *
    * @param scalar The scalar to multiply by.
    *
@@ -146,9 +145,9 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d operator*(double scalar) const;
 
   /**
-   * Divides the translation by a scalar and returns the new translation.
+   * Returns the translation divided by a scalar.
    *
-   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}.
    *
    * @param scalar The scalar to divide by.
    *

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

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Rotation3d.h"
+#include "Translation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+/**
+ * Represents a translation in 3D space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes. When the
+ * robot is at the origin facing in the positive X direction, forward is
+ * positive X, left is positive Y, and up is positive Z.
+ */
+class WPILIB_DLLEXPORT Translation3d {
+ public:
+  /**
+   * Constructs a Translation3d with X, Y, and Z components equal to zero.
+   */
+  constexpr Translation3d() = default;
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
+
+  /**
+   * Constructs a Translation3d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  Translation3d(units::meter_t distance, const Rotation3d& angle);
+
+  /**
+   * Calculates the distance between two translations in 3D space.
+   *
+   * The distance between translations is defined as
+   * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  units::meter_t Distance(const Translation3d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns the Z component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  units::meter_t Z() const { return m_z; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  units::meter_t Norm() const;
+
+  /**
+   * Applies a rotation to the translation in 3D space.
+   *
+   * For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
+   * around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  Translation3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Returns a Translation2d representing this Translation3d projected into the
+   * X-Y plane.
+   */
+  Translation2d ToTranslation2d() const;
+
+  /**
+   * Returns the sum of two translations in 3D space.
+   *
+   * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
+   * Translation3d{3.0, 8.0, 11.0}.
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  Translation3d operator+(const Translation3d& other) const;
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
+   * Translation3d{4.0, 2.0, 0.0}.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  Translation3d operator-(const Translation3d& other) const;
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * negating all components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  Translation3d operator-() const;
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
+   * 9.0}.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  Translation3d operator*(double scalar) const;
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
+   * 2.25}.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  Translation3d operator/(double scalar) const;
+
+  /**
+   * Checks equality between this Translation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Translation3d& other) const;
+
+  /**
+   * Checks inequality between this Translation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Translation3d& other) const;
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+  units::meter_t m_z = 0_m;
+};
+
+}  // namespace frc

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

@@ -12,9 +12,9 @@
 
 namespace frc {
 /**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
+ * A change in distance along a 2D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose2ds from a Twist2d and
+ * vise versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */

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

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * A change in distance along a 3D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose3ds from a Twist3d and
+ * vise versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct WPILIB_DLLEXPORT Twist3d {
+  /**
+   * Linear "dx" component
+   */
+  units::meter_t dx = 0_m;
+
+  /**
+   * Linear "dy" component
+   */
+  units::meter_t dy = 0_m;
+
+  /**
+   * Linear "dz" component
+   */
+  units::meter_t dz = 0_m;
+
+  /**
+   * Rotation vector x component.
+   */
+  units::radian_t rx = 0_rad;
+
+  /**
+   * Rotation vector y component.
+   */
+  units::radian_t ry = 0_rad;
+
+  /**
+   * Rotation vector z component.
+   */
+  units::radian_t rz = 0_rad;
+
+  /**
+   * Checks equality between this Twist3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Twist3d& other) const {
+    return units::math::abs(dx - other.dx) < 1E-9_m &&
+           units::math::abs(dy - other.dy) < 1E-9_m &&
+           units::math::abs(dz - other.dz) < 1E-9_m &&
+           units::math::abs(rx - other.rx) < 1E-9_rad &&
+           units::math::abs(ry - other.ry) < 1E-9_rad &&
+           units::math::abs(rz - other.rz) < 1E-9_rad;
+  }
+
+  /**
+   * Checks inequality between this Twist3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Twist3d& other) const { return !operator==(other); }
+
+  /**
+   * Scale this by a given factor.
+   *
+   * @param factor The factor by which to scale.
+   * @return The scaled Twist3d.
+   */
+  Twist3d operator*(double factor) const {
+    return Twist3d{dx * factor, dy * factor, dz * factor,
+                   rx * factor, ry * factor, rz * factor};
+  }
+};
+}  // namespace frc