Add geometry classes (#1766)

These classes introduce ways to represent poses and provide easy ways to transform, rotate, and translate poses across 2d space. This classes will be especially useful for a planned odometry and kinematics suite.

Furthermore, these classes can also be used to simply represent waypoints on a field, do superstructure motion planning, etc.

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

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Pose2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+    : m_translation(translation), m_rotation(rotation) {}
+
+Pose2d::Pose2d(double x, double y, Rotation2d rotation)
+    : m_translation(x, y), m_rotation(rotation) {}
+
+Pose2d Pose2d::operator+(const Transform2d& other) const {
+  return TransformBy(other);
+}
+
+Pose2d& Pose2d::operator+=(const Transform2d& other) {
+  m_translation += other.Translation().RotateBy(m_rotation);
+  m_rotation += other.Rotation();
+  return *this;
+}
+
+Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          m_rotation + other.Rotation()};
+}
+
+Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
+  const Transform2d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+Pose2d Pose2d::Exp(const Twist2d& twist) const {
+  const auto dx = twist.dx;
+  const auto dy = twist.dy;
+  const auto dtheta = twist.dtheta;
+
+  const auto sinTheta = std::sin(dtheta);
+  const auto cosTheta = std::cos(dtheta);
+
+  double s, c;
+  if (std::abs(dtheta) < 1E-9) {
+    s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+    c = 0.5 * dtheta;
+  } else {
+    s = sinTheta / dtheta;
+    c = (1 - cosTheta) / dtheta;
+  }
+
+  const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
+                              Rotation2d{cosTheta, sinTheta}};
+
+  return *this + transform;
+}

wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Rotation2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+Rotation2d::Rotation2d(double value)
+    : m_value(value), m_cos(std::cos(value)), m_sin(std::sin(value)) {}
+
+Rotation2d::Rotation2d(double x, double y) {
+  const auto magnitude = std::hypot(x, y);
+  if (magnitude > 1e-6) {
+    m_sin = y / magnitude;
+    m_cos = x / magnitude;
+  } else {
+    m_sin = 0.0;
+    m_cos = 1.0;
+  }
+  m_value = std::atan2(m_sin, m_cos);
+}
+
+Rotation2d Rotation2d::FromDegrees(double degrees) {
+  return Rotation2d(Deg2Rad(degrees));
+}
+
+Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+  return RotateBy(other);
+}
+
+Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
+  double cos = Cos() * other.Cos() - Sin() * other.Sin();
+  double sin = Cos() * other.Sin() + Sin() * other.Cos();
+  m_cos = cos;
+  m_sin = sin;
+  m_value = std::atan2(m_sin, m_cos);
+  return *this;
+}
+
+Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+  return *this + -other;
+}
+
+Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
+  *this += -other;
+  return *this;
+}
+
+Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
+
+Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+  return {Cos() * other.Cos() - Sin() * other.Sin(),
+          Cos() * other.Sin() + Sin() * other.Cos()};
+}

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

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Transform2d.h"
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+Transform2d::Transform2d(Pose2d initial, 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).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
+    : m_translation(translation), m_rotation(rotation) {}

wpilibc/src/main/native/cpp/geometry/Translation2d.cpp

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Translation2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+Translation2d::Translation2d(double x, double y) : m_x(x), m_y(y) {}
+
+double Translation2d::Distance(const Translation2d& other) const {
+  return std::hypot(other.m_x - m_x, other.m_y - m_y);
+}
+
+double Translation2d::Norm() const { return std::hypot(m_x, m_y); }
+
+Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+  return {m_x * other.Cos() - m_y * other.Sin(),
+          m_x * other.Sin() + m_y * other.Cos()};
+}
+
+Translation2d Translation2d::operator+(const Translation2d& other) const {
+  return {X() + other.X(), Y() + other.Y()};
+}
+
+Translation2d& Translation2d::operator+=(const Translation2d& other) {
+  m_x += other.m_x;
+  m_y += other.m_y;
+  return *this;
+}
+
+Translation2d Translation2d::operator-(const Translation2d& other) const {
+  return *this + -other;
+}
+
+Translation2d& Translation2d::operator-=(const Translation2d& other) {
+  *this += -other;
+  return *this;
+}
+
+Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
+
+Translation2d Translation2d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y};
+}
+
+Translation2d& Translation2d::operator*=(double scalar) {
+  m_x *= scalar;
+  m_y *= scalar;
+  return *this;
+}
+
+Translation2d Translation2d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+Translation2d& Translation2d::operator/=(double scalar) {
+  *this *= (1.0 / scalar);
+  return *this;
+}

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

@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Transform2d.h"
+#include "Translation2d.h"
+#include "Twist2d.h"
+
+namespace frc {
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+class Pose2d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   * (Translation2d{0, 0} and Rotation{0})
+   */
+  constexpr Pose2d() = 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.
+   */
+  Pose2d(Translation2d translation, Rotation2d rotation);
+
+  /**
+   * Convenience constructors that takes in x and y values directly instead of
+   * having to construct a 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.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose2d(double x, double y, Rotation2d rotation);
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [0,    0,   1][transform.t]
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose2d operator+(const Transform2d& other) const;
+
+  /**
+   * Transforms the current pose by the transformation.
+   *
+   * This is similar to the + operator, except that it mutates the current
+   * object.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return Reference to the new mutated object.
+   */
+  Pose2d& operator+=(const Transform2d& other);
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  const Translation2d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  const Rotation2d& 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.
+   */
+  Pose2d TransformBy(const Transform2d& 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.
+   */
+  Pose2d RelativeTo(const Pose2d& other) const;
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * See <https://file.tavsys.net/control/state-space-guide.pdf> section on
+   * nonlinear pose estimation for derivation.
+   *
+   * 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 .5 degrees since the previous pose update,
+   * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   *
+   * @return The new pose of the robot.
+   */
+  Pose2d Exp(const Twist2d& twist) const;
+
+ private:
+  Translation2d m_translation;
+  Rotation2d m_rotation;
+};
+}  // namespace frc

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

@@ -0,0 +1,172 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+class Rotation2d {
+ public:
+  /**
+   * Constructs a Rotation2d with a default angle of 0 degrees.
+   */
+  constexpr Rotation2d() = default;
+
+  /**
+   * Constructs a Rotation2d with the given radian value.
+   *
+   * @param value The value of the angle in radians.
+   */
+  explicit Rotation2d(double value);
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine)
+   * components. The x and y don't have to be normalized.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  Rotation2d(double x, double y);
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   *
+   * @return The rotation object with the desired angle value.
+   */
+  static Rotation2d FromDegrees(double degrees);
+
+  /**
+   * Adds two rotations together, with the result being bounded between -kPi and
+   * kPi.
+   *
+   * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
+   * Rotation2d{-kPi/2}
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  Rotation2d operator+(const Rotation2d& other) const;
+
+  /**
+   * Adds a rotation to the current rotation.
+   *
+   * This is similar to the + operator except that it mutates the current
+   * object.
+   *
+   * @param other The rotation to add.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Rotation2d& operator+=(const Rotation2d& other);
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
+   * Rotation2d{-kPi/2}
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  Rotation2d operator-(const Rotation2d& other) const;
+
+  /**
+   * Subtracts the new rotation from the current rotation.
+   *
+   * This is similar to the - operator except that it mutates the current
+   * object.
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Rotation2d& operator-=(const Rotation2d& other);
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of
+   * the current angular value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  Rotation2d operator-() const;
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   *
+   * value_new = std::atan2(cos_new, sin_new)
+   *
+   * @param other The rotation to rotate by.
+   *
+   * @return The new rotated Rotation2d.
+   */
+  Rotation2d RotateBy(const Rotation2d& other) const;
+
+  /**
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  double Radians() const { return m_value; }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  double Degrees() const { return Rad2Deg(m_value); }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  double Cos() const { return m_cos; }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  double Sin() const { return m_sin; }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  double Tan() const { return m_sin / m_cos; }
+
+ private:
+  double m_value = 0;
+  double m_cos = 1;
+  double m_sin = 0;
+
+  constexpr static double kPi = 3.14159265358979323846;
+
+  template <typename T>
+  static T Rad2Deg(const T& rad) {
+    return rad * 180.0 / kPi;
+  }
+
+  template <typename T>
+  static T Deg2Rad(const T& deg) {
+    return deg * kPi / 180.0;
+  }
+};
+}  // namespace frc

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

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Translation2d.h"
+
+namespace frc {
+
+class Pose2d;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+class Transform2d {
+ 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.
+   */
+  Transform2d(Pose2d initial, Pose2d 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.
+   */
+  Transform2d(Translation2d translation, Rotation2d rotation);
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform2d() = default;
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  const Translation2d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  const Rotation2d& Rotation() const { return m_rotation; }
+
+ private:
+  Translation2d m_translation;
+  Rotation2d m_rotation;
+};
+}  // namespace frc

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

@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Rotation2d.h"
+
+namespace frc {
+
+/**
+ * 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.
+ */
+class Translation2d {
+ public:
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  constexpr Translation2d() = default;
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  Translation2d(double x, double y);
+
+  /**
+   * 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)
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  double Distance(const Translation2d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  double X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  double Y() const { return m_y; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  double Norm() const;
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   *
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+   * Translation2d of {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  Translation2d RotateBy(const Rotation2d& other) const;
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to
+   * vector addition.
+   *
+   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+   * Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  Translation2d operator+(const Translation2d& other) const;
+
+  /**
+   * Adds the new translation to the current translation.
+   *
+   * This is similar to the + operator, except that the current object is
+   * mutated.
+   *
+   * @param other The translation to add.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator+=(const Translation2d& other);
+
+  /**
+   * Subtracts the other translation from the other translation and returns the
+   * difference.
+   *
+   * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  Translation2d operator-(const Translation2d& other) const;
+
+  /**
+   * Subtracts the new translation from the current translation.
+   *
+   * This is similar to the - operator, except that the current object is
+   * mutated.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator-=(const Translation2d& other);
+
+  /**
+   * 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.
+   *
+   * @return The inverse of the current translation.
+   */
+  Translation2d operator-() const;
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  Translation2d operator*(double scalar) const;
+
+  /**
+   * Multiplies the current translation by a scalar.
+   *
+   * This is similar to the * operator, except that current object is mutated.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator*=(double scalar);
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  Translation2d operator/(double scalar) const;
+
+  /*
+   * Divides the current translation by a scalar.
+   *
+   * This is similar to the / operator, except that current object is mutated.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator/=(double scalar);
+
+ private:
+  double m_x = 0;
+  double m_y = 0;
+};
+}  // namespace frc

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

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+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 Twist can be used to represent a difference between two poses.
+ */
+struct Twist2d {
+  /**
+   * Linear "dx" component
+   */
+  double dx = 0;
+
+  /**
+   * Linear "dy" component
+   */
+  double dy = 0;
+
+  /**
+   * Angular "dtheta" component (radians)
+   */
+  double dtheta = 0;
+};
+}  // namespace frc

wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Pose2dTest, TransformBy) {
+  const Pose2d initial{1.0, 2.0, Rotation2d::FromDegrees(45.0)};
+  const Transform2d transform{Translation2d{5.0, 0.0},
+                              Rotation2d::FromDegrees(5.0)};
+
+  const auto transformed = initial + transform;
+
+  EXPECT_NEAR(transformed.Translation().X(), 1 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Translation().Y(), 2 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Rotation().Degrees(), 50.0, kEpsilon);
+}
+
+TEST(Pose2dTest, RelativeTo) {
+  const Pose2d initial{0.0, 0.0, Rotation2d::FromDegrees(45.0)};
+  const Pose2d final{5.0, 5.0, Rotation2d::FromDegrees(45.0)};
+
+  const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+  EXPECT_NEAR(finalRelativeToInitial.Translation().X(), 5.0 * std::sqrt(2.0),
+              kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Translation().Y(), 0.0, kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees(), 0.0, kEpsilon);
+}

wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+static constexpr double kPi = 3.14159265358979323846;
+
+TEST(Rotation2dTest, RadiansToDegrees) {
+  const Rotation2d one{kPi / 3};
+  const Rotation2d two{kPi / 4};
+
+  EXPECT_NEAR(one.Degrees(), 60.0, kEpsilon);
+  EXPECT_NEAR(two.Degrees(), 45.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, DegreesToRadians) {
+  const auto one = Rotation2d::FromDegrees(45.0);
+  const auto two = Rotation2d::FromDegrees(30.0);
+
+  EXPECT_NEAR(one.Radians(), kPi / 4.0, kEpsilon);
+  EXPECT_NEAR(two.Radians(), kPi / 6.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByFromZero) {
+  const Rotation2d zero;
+  auto sum = zero + Rotation2d::FromDegrees(90.0);
+
+  EXPECT_NEAR(sum.Radians(), kPi / 2.0, kEpsilon);
+  EXPECT_NEAR(sum.Degrees(), 90.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByNonZero) {
+  auto rot = Rotation2d::FromDegrees(90.0);
+  rot += Rotation2d::FromDegrees(30.0);
+
+  EXPECT_NEAR(rot.Degrees(), 120.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Minus) {
+  const auto one = Rotation2d::FromDegrees(70.0);
+  const auto two = Rotation2d::FromDegrees(30.0);
+
+  EXPECT_NEAR((one - two).Degrees(), 40.0, kEpsilon);
+}

wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation2dTest, Sum) {
+  const Translation2d one{1.0, 3.0};
+  const Translation2d two{2.0, 5.0};
+
+  const auto sum = one + two;
+
+  EXPECT_NEAR(sum.X(), 3.0, kEpsilon);
+  EXPECT_NEAR(sum.Y(), 8.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Difference) {
+  const Translation2d one{1.0, 3.0};
+  const Translation2d two{2.0, 5.0};
+
+  const auto difference = one - two;
+
+  EXPECT_NEAR(difference.X(), -1.0, kEpsilon);
+  EXPECT_NEAR(difference.Y(), -2.0, kEpsilon);
+}
+
+TEST(Translation2dTest, RotateBy) {
+  const Translation2d another{3.0, 0.0};
+  const auto rotated = another.RotateBy(Rotation2d::FromDegrees(90.0));
+
+  EXPECT_NEAR(rotated.X(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated.Y(), 3.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Multiplication) {
+  const Translation2d original{3.0, 5.0};
+  const auto mult = original * 3;
+
+  EXPECT_NEAR(mult.X(), 9.0, kEpsilon);
+  EXPECT_NEAR(mult.Y(), 15.0, kEpsilon);
+}
+
+TEST(Translation2d, Division) {
+  const Translation2d original{3.0, 5.0};
+  const auto div = original / 2;
+
+  EXPECT_NEAR(div.X(), 1.5, kEpsilon);
+  EXPECT_NEAR(div.Y(), 2.5, kEpsilon);
+}
+
+TEST(Translation2dTest, Norm) {
+  const Translation2d one{3.0, 5.0};
+  EXPECT_NEAR(one.Norm(), std::hypot(3, 5), kEpsilon);
+}
+
+TEST(Translation2dTest, Distance) {
+  const Translation2d one{1, 1};
+  const Translation2d two{6, 6};
+  EXPECT_NEAR(one.Distance(two), 5 * std::sqrt(2), kEpsilon);
+}
+
+TEST(Translation2dTest, UnaryMinus) {
+  const Translation2d original{-4.5, 7};
+  const auto inverted = -original;
+
+  EXPECT_NEAR(inverted.X(), 4.5, kEpsilon);
+  EXPECT_NEAR(inverted.Y(), -7, kEpsilon);
+}

wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Twist2dTest, Straight) {
+  const Twist2d straight{5.0, 0.0, 0.0};
+  const auto straightPose = Pose2d().Exp(straight);
+
+  EXPECT_NEAR(straightPose.Translation().X(), 5.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Translation().Y(), 0.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Rotation().Radians(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, QuarterCircle) {
+  const Twist2d quarterCircle{5.0 / 2.0 * 3.14159265358979323846, 0,
+                              3.14159265358979323846 / 2.0};
+  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+
+  EXPECT_NEAR(quarterCirclePose.Translation().X(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Translation().Y(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees(), 90.0, kEpsilon);
+}
+
+TEST(Twist2dTest, DiagonalNoDtheta) {
+  const Twist2d diagonal{2.0, 2.0, 0.0};
+  const auto diagonalPose = Pose2d().Exp(diagonal);
+
+  EXPECT_NEAR(diagonalPose.Translation().X(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Translation().Y(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Rotation().Degrees(), 0.0, kEpsilon);
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java

@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+public class Pose2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   * (Translation2d{0, 0} and Rotation{0})
+   */
+  public Pose2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * 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.
+   */
+  public Pose2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Convenience constructors that takes in x and y values directly instead of
+   * having to construct a 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.
+   * @param rotation The rotational component of the pose.
+   */
+  @SuppressWarnings("ParameterName")
+  public Pose2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * <p>The matrix multiplication is as follows
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [0,    0,   1][transform.t]
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d plus(Transform2d other) {
+    return transformBy(other);
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  public Rotation2d getRotation() {
+    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.
+   */
+  public Pose2d transformBy(Transform2d other) {
+    return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+            m_rotation.plus(other.getRotation()));
+  }
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * <p>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.
+   */
+  public Pose2d relativeTo(Pose2d other) {
+    var transform = new Transform2d(other, this);
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
+   * Controls Engineering in the FIRST Robotics Competition</a>
+   * section on nonlinear pose estimation for derivation.
+   *
+   * <p>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.
+   *
+   * <p>"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 .5 degrees since the previous pose update,
+   *              the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * @return The new pose of the robot.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d exp(Twist2d twist) {
+    double dx = twist.dx;
+    double dy = twist.dy;
+    double dtheta = twist.dtheta;
+
+    double sinTheta = Math.sin(dtheta);
+    double cosTheta = Math.cos(dtheta);
+
+    double s;
+    double c;
+    if (Math.abs(dtheta) < 1E-9) {
+      s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+      c = 0.5 * dtheta;
+    } else {
+      s = sinTheta / dtheta;
+      c = (1 - cosTheta) / dtheta;
+    }
+    var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
+            new Rotation2d(cosTheta, sinTheta));
+
+    return this.plus(transform);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java

@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+public class Rotation2d {
+  private final double m_value;
+  private final double m_cos;
+  private final double m_sin;
+
+  /**
+   * Constructs a Rotation2d with a default angle of 0 degrees.
+   */
+  public Rotation2d() {
+    m_value = 0.0;
+    m_cos = 1.0;
+    m_sin = 0.0;
+  }
+
+  /**
+   * Constructs a Rotation2d with the given radian value.
+   * The x and y don't have to be normalized.
+   *
+   * @param value The value of the angle in radians.
+   */
+  public Rotation2d(double value) {
+    m_value = value;
+    m_cos = Math.cos(value);
+    m_sin = Math.sin(value);
+  }
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine)
+   * components.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  @SuppressWarnings("ParameterName")
+  public Rotation2d(double x, double y) {
+    double magnitude = Math.hypot(x, y);
+    if (magnitude > 1e-6) {
+      m_sin = y / magnitude;
+      m_cos = x / magnitude;
+    } else {
+      m_sin = 0.0;
+      m_cos = 1.0;
+    }
+    m_value = Math.atan2(m_sin, m_cos);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromDegrees(double degrees) {
+    return new Rotation2d(Math.toRadians(degrees));
+  }
+
+  /**
+   * Adds two rotations together, with the result being bounded between -kPi and
+   * kPi.
+   *
+   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
+   * Rotation2d{-kPi/2}
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation2d plus(Rotation2d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
+   * Rotation2d{-kPi/2}
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation2d minus(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of
+   * the current angular value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation2d unaryMinus() {
+    return new Rotation2d(-m_value);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <p>The matrix multiplication is as follows:
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = atan2(cos_new, sin_new)
+   *
+   * @param other The rotation to rotate by.
+   * @return The new rotated Rotation2d.
+   */
+  public Rotation2d rotateBy(Rotation2d other) {
+    return new Rotation2d(
+            m_cos * other.m_cos - m_sin * other.m_sin,
+            m_cos * other.m_sin + m_sin * other.m_cos
+    );
+  }
+
+  /*
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  public double getRadians() {
+    return m_value;
+  }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  public double getDegrees() {
+    return Math.toDegrees(m_value);
+  }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  public double getCos() {
+    return m_cos;
+  }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  public double getSin() {
+    return m_sin;
+  }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  public double getTan() {
+    return m_sin / m_cos;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+public class Transform2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param last    The final pose for the transformation.
+   */
+  public Transform2d(Pose2d initial, Pose2d last) {
+    // 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 = last.getTranslation().minus(initial.getTranslation())
+            .rotateBy(initial.getRotation().unaryMinus());
+
+    m_rotation = last.getRotation().minus(initial.getRotation());
+  }
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation    Rotational component of the transform.
+   */
+  public Transform2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  public Transform2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java

@@ -0,0 +1,165 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * <p>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.
+ */
+@SuppressWarnings({"ParameterName", "MemberName"})
+public class Translation2d {
+  private final double m_x;
+  private final double m_y;
+
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  public Translation2d() {
+    this(0.0, 0.0);
+  }
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  public Translation2d(double x, double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Calculates the distance between two translations in 2d space.
+   *
+   * <p>This function uses the pythagorean theorem to calculate the distance.
+   * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation2d other) {
+    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.hypot(m_x, m_y);
+  }
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * <p>This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+   * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+   * Translation2d of {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateBy(Rotation2d other) {
+    return new Translation2d(
+            m_x * other.getCos() - m_y * other.getSin(),
+            m_x * other.getSin() + m_y * other.getCos()
+    );
+  }
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to
+   * vector addition.
+   *
+   * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+   * Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation2d plus(Translation2d other) {
+    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+  }
+
+  /**
+   * Subtracts the other translation from the other translation and returns the
+   * difference.
+   *
+   * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}
+   *
+   * @param other The translation to subtract.
+   * @return The difference between the two translations.
+   */
+  public Translation2d minus(Translation2d other) {
+    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+  }
+
+  /**
+   * 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.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation2d unaryMinus() {
+    return new Translation2d(-m_x, -m_y);
+  }
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled translation.
+   */
+  public Translation2d times(double scalar) {
+    return new Translation2d(m_x * scalar, m_y * scalar);
+  }
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation2d div(double scalar) {
+    return new Translation2d(m_x / scalar, m_y / scalar);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+/**
+ * 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.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+@SuppressWarnings("MemberName")
+public class Twist2d {
+  /**
+   * Linear "dx" component.
+   */
+  public double dx;
+
+  /**
+   * Linear "dy" component.
+   */
+  public double dy;
+
+  /**
+   * Angular "dtheta" component (radians).
+   */
+  public double dtheta;
+
+  public Twist2d() {
+  }
+
+  /**
+   * Constructs a Twist2d with the given values.
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dtheta Change in angle relative to robot.
+   */
+  public Twist2d(double dx, double dy, double dtheta) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dtheta = dtheta;
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Pose2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testTransformBy() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+        Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRelativeTo() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0),
+            kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Rotation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testRadiansToDegrees() {
+    var one = new Rotation2d(Math.PI / 3);
+    var two = new Rotation2d(Math.PI / 4);
+
+    assertAll(
+        () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
+        () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    var one = Rotation2d.fromDegrees(45.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertAll(
+        () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
+        () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByFromZero() {
+    var zero = new Rotation2d();
+    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
+        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByNonZero() {
+    var rot = Rotation2d.fromDegrees(90.0);
+    rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+  }
+
+  @Test
+  void testMinus() {
+    var one = Rotation2d.fromDegrees(70.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java

@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Translation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
+        () -> assertEquals(sum.getY(), 8.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
+        () -> assertEquals(difference.getY(), -2.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateBy() {
+    var another = new Translation2d(3.0, 0.0);
+    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
+        () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation2d(3.0, 5.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
+        () -> assertEquals(mult.getY(), 15.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation2d(3.0, 5.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(div.getX(), 1.5, kEpsilon),
+        () -> assertEquals(div.getY(), 2.5, kEpsilon)
+    );
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation2d(-4.5, 7);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
+        () -> assertEquals(inverted.getY(), -7, kEpsilon)
+    );
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Twist2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testStraightLineTwist() {
+    var straight = new Twist2d(5.0, 0.0, 0.0);
+    var straightPose = new Pose2d().exp(straight);
+
+    assertAll(
+        () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testQuarterCirleTwist() {
+    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+    var quarterCirclePose = new Pose2d().exp(quarterCircle);
+
+    assertAll(
+        () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist2d(2.0, 2.0, 0.0);
+    var diagonalPose = new Pose2d().exp(diagonal);
+
+    assertAll(
+        () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+}