Add trajectory generation using hermite splines (#1843)

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

@@ -68,6 +68,14 @@ class Pose2d {
    */
   Pose2d& operator+=(const Transform2d& other);
 
+  /**
+   * Returns the Transform2d 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.
+   */
+  Transform2d operator-(const Pose2d& other) const;
+
   /**
    * Checks equality between this Pose2d and another object.
    *
@@ -145,6 +153,16 @@ class Pose2d {
    */
   Pose2d Exp(const Twist2d& twist) const;
 
+  /**
+   * Returns a Twist2d 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.
+   */
+  Twist2d Log(const Pose2d& end) const;
+
  private:
   Translation2d m_translation;
   Rotation2d m_rotation;

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

@@ -97,6 +97,14 @@ class Rotation2d {
    */
   Rotation2d operator-() const;
 
+  /**
+   * Multiplies the current rotation by a scalar.
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation2d.
+   */
+  Rotation2d operator*(double scalar) const;
+
   /**
    * Checks equality between this Rotation2d and another object.
    *

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

@@ -53,6 +53,16 @@ class Transform2d {
    */
   const Rotation2d& Rotation() const { return m_rotation; }
 
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  Transform2d operator*(double scalar) const {
+    return Transform2d(m_translation * scalar, m_rotation * scalar);
+  }
+
   /**
    * Checks equality between this Transform2d and another object.
    *