[wpimath] Add TimeInterpolatableBuffer (#2695)

These classes are useful for storing previous robot positions to use in conjunction with the upcoming pose estimators.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: cttew <cttewari@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/MathUtil.java

@@ -84,4 +84,17 @@ public final class MathUtil {
   public static double angleModulus(double angleRadians) {
     return inputModulus(angleRadians, -Math.PI, Math.PI);
   }
+
+  /**
+   * Perform linear interpolation between two values.
+   *
+   * @param startValue The value to start at.
+   * @param endValue The value to end at.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @SuppressWarnings("ParameterName")
+  public static double interpolate(double startValue, double endValue, double t) {
+    return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -8,12 +8,13 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** Represents a 2d pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d {
+public class Pose2d implements Interpolatable<Pose2d> {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
 
@@ -242,4 +243,18 @@ public class Pose2d {
   public int hashCode() {
     return Objects.hash(m_translation, m_rotation);
   }
+
+  @Override
+  @SuppressWarnings("ParameterName")
+  public Pose2d interpolate(Pose2d endValue, double t) {
+    if (t < 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      var twist = this.log(endValue);
+      var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
+      return this.exp(scaledTwist);
+    }
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -8,12 +8,14 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Rotation2d {
+public class Rotation2d implements Interpolatable<Rotation2d> {
   private final double m_value;
   private final double m_cos;
   private final double m_sin;
@@ -198,4 +200,10 @@ public class Rotation2d {
   public int hashCode() {
     return Objects.hash(m_value);
   }
+
+  @Override
+  @SuppressWarnings("ParameterName")
+  public Rotation2d interpolate(Rotation2d endValue, double t) {
+    return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t));
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -8,6 +8,8 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /**
@@ -20,7 +22,7 @@ import java.util.Objects;
 @SuppressWarnings({"ParameterName", "MemberName"})
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Translation2d {
+public class Translation2d implements Interpolatable<Translation2d> {
   private final double m_x;
   private final double m_y;
 
@@ -196,4 +198,11 @@ public class Translation2d {
   public int hashCode() {
     return Objects.hash(m_x, m_y);
   }
+
+  @Override
+  public Translation2d interpolate(Translation2d endValue, double t) {
+    return new Translation2d(
+        MathUtil.interpolate(this.getX(), endValue.getX(), t),
+        MathUtil.interpolate(this.getY(), endValue.getY(), t));
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * An object should extend interpolatable if you wish to interpolate between a lower and upper
+ * bound, such as a robot position on the field between timesteps. This behavior can be linear or
+ * nonlinear.
+ *
+ * @param <T> The class that is interpolatable.
+ */
+public interface Interpolatable<T> {
+  /**
+   * Return the interpolated value. This object is assumed to be the starting position, or lower
+   * bound.
+   *
+   * @param endValue The upper bound, or end.
+   * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+   * @return The interpolated value.
+   */
+  @SuppressWarnings("ParameterName")
+  T interpolate(T endValue, double t);
+}

wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java

@@ -0,0 +1,149 @@
+// 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.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.NavigableMap;
+import java.util.TreeMap;
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application
+ * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot
+ * pose at the time when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * @param <T> The type stored in this buffer.
+ */
+public class TimeInterpolatableBuffer<T> {
+  private final double m_historySize;
+  private final InterpolateFunction<T> m_interpolatingFunc;
+  private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
+
+  private TimeInterpolatableBuffer(
+      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+    this.m_historySize = historySizeSeconds;
+    this.m_interpolatingFunc = interpolateFunction;
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer.
+   *
+   * @param interpolateFunction The function used to interpolate between values.
+   * @param historySizeSeconds The history size of the buffer.
+   * @param <T> The type of data to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T> TimeInterpolatableBuffer<T> createBuffer(
+      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+    return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
+   *
+   * @param historySizeSeconds The history size of the buffer.
+   * @param <T> The type of {@link Interpolatable} to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
+      double historySizeSeconds) {
+    return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer to store Double values.
+   *
+   * @param historySizeSeconds The history size of the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySizeSeconds) {
+    return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds);
+  }
+
+  /**
+   * Add a sample to the buffer.
+   *
+   * @param timeSeconds The timestamp of the sample.
+   * @param sample The sample object.
+   */
+  public void addSample(double timeSeconds, T sample) {
+    cleanUp(timeSeconds);
+    m_buffer.put(timeSeconds, sample);
+  }
+
+  /**
+   * Removes samples older than our current history size.
+   *
+   * @param time The current timestamp.
+   */
+  private void cleanUp(double time) {
+    while (!m_buffer.isEmpty()) {
+      var entry = m_buffer.firstEntry();
+      if (time - entry.getKey() >= m_historySize) {
+        m_buffer.remove(entry.getKey());
+      } else {
+        return;
+      }
+    }
+  }
+
+  /** Clear all old samples. */
+  public void clear() {
+    m_buffer.clear();
+  }
+
+  /**
+   * Sample the buffer at the given time. If the buffer is empty, this will return null.
+   *
+   * @param timeSeconds The time at which to sample.
+   * @return The interpolated value at that timestamp. Might be null.
+   */
+  @SuppressWarnings("UnnecessaryParentheses")
+  public T getSample(double timeSeconds) {
+    if (m_buffer.isEmpty()) {
+      return null;
+    }
+
+    // Special case for when the requested time is the same as a sample
+    var nowEntry = m_buffer.get(timeSeconds);
+    if (nowEntry != null) {
+      return nowEntry;
+    }
+
+    var topBound = m_buffer.ceilingEntry(timeSeconds);
+    var bottomBound = m_buffer.floorEntry(timeSeconds);
+
+    // Return null if neither sample exists, and the opposite bound if the other is null
+    if (topBound == null && bottomBound == null) {
+      return null;
+    } else if (topBound == null) {
+      return bottomBound.getValue();
+    } else if (bottomBound == null) {
+      return topBound.getValue();
+    } else {
+      // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
+      // between the current time and bottom bound) and (the difference between top and bottom
+      // bounds).
+      return m_interpolatingFunc.interpolate(
+          bottomBound.getValue(),
+          topBound.getValue(),
+          ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+    }
+  }
+
+  public interface InterpolateFunction<T> {
+    /**
+     * Return the interpolated value. This object is assumed to be the starting position, or lower
+     * bound.
+     *
+     * @param start The lower bound, or start.
+     * @param end The upper bound, or end.
+     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+     * @return The interpolated value.
+     */
+    @SuppressWarnings("ParameterName")
+    T interpolate(T start, T end, double t);
+  }
+}