Add encoder distance overload to DifferentialDriveOdometry (#2096)

Also force encoders to be reset to zero on pose reset.

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp

@@ -37,3 +37,24 @@ const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
 
   return m_pose;
 }
+
+const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
+                                                units::meter_t leftDistance,
+                                                units::meter_t rightDistance) {
+  auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
+  auto deltaRightDistance = rightDistance - m_prevRightDistance;
+
+  m_prevLeftDistance = leftDistance;
+  m_prevRightDistance = rightDistance;
+
+  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+  auto angle = gyroAngle + m_gyroOffset;
+
+  auto newPose = m_pose.Exp(
+      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
+
+  m_previousAngle = angle;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}

wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h

@@ -23,9 +23,14 @@ namespace frc {
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  *
- * Note: It is important to reset both your encoders to zero before you start
- * using this class. Only reset your encoders ONCE. You should not reset your
- * encoders even if you want to reset your robot's pose.
+ * There are two ways of tracking the robot's position on the field with this
+ * class: one involving using encoder velocities and the other involving encoder
+ * positions. It is very important that only one type of odometry is used with
+ * each instantiation of this class.
+ *
+ * Note: If you are using the encoder positions / distances method, it is
+ * important that you reset your encoders to zero before using this class. Any
+ * subsequent pose resets also require the encoders to be reset to zero.
  */
 class DifferentialDriveOdometry {
  public:
@@ -43,6 +48,9 @@ class DifferentialDriveOdometry {
   /**
    * Resets the robot's position on the field.
    *
+   * If you are using the encoder distances method instead of the velocity
+   * method, you NEED to reset your encoders (to zero) when calling this method.
+   *
    * The gyroscope angle does not need to be reset here on the user's robot
    * code. The library automatically takes care of offsetting the gyro angle.
    *
@@ -53,6 +61,9 @@ class DifferentialDriveOdometry {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
     m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+    m_prevLeftDistance = 0_m;
+    m_prevRightDistance = 0_m;
   }
 
   /**
@@ -79,6 +90,20 @@ class DifferentialDriveOdometry {
                                const Rotation2d& gyroAngle,
                                const DifferentialDriveWheelSpeeds& wheelSpeeds);
 
+  /**
+   * Updates the robot position on the field using distance measurements from
+   * encoders. This method is more numerically accurate than using velocities to
+   * integrate the pose and is also advantageous for teams that are using lower
+   * CPR encoders.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                       units::meter_t rightDistance);
+
   /**
    * Updates the robot's position on the field using forward kinematics and
    * integration of the pose over time. This method automatically calculates
@@ -105,5 +130,8 @@ class DifferentialDriveOdometry {
   units::second_t m_previousTime = -1_s;
   Rotation2d m_gyroOffset;
   Rotation2d m_previousAngle;
+
+  units::meter_t m_prevLeftDistance = 0_m;
+  units::meter_t m_prevRightDistance = 0_m;
 };
 }  // namespace frc

wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp

@@ -57,3 +57,15 @@ TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) {
   EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
   EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
 }
+
+TEST(DifferentialDriveOdometry, EncoderDistances) {
+  DifferentialDriveKinematics kinematics{0.381_m * 2};
+  DifferentialDriveOdometry odometry{kinematics, Rotation2d(45_deg)};
+
+  const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
+                                     units::meter_t(5 * wpi::math::pi));
+
+  EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java

@@ -21,9 +21,14 @@ import edu.wpi.first.wpilibj.geometry.Twist2d;
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  *
- * <p>Note: It is important to reset both your encoders to zero before you start
- * using this class. Only reset your encoders ONCE. You should not reset your
- * encoders even if you want to reset your robot's pose.
+ * <p>There are two ways of tracking the robot's position on the field with this
+ * class: one involving using encoder velocities and the other involving encoder
+ * positions. It is very important that only one type of odometry is used with
+ * each instantiation of this class.
+ *
+ * <p>Note: If you are using the encoder positions / distances method, it is important
+ * that you reset your encoders to zero before using this class. Any subsequent pose
+ * resets also require the encoders to be reset to zero.
  */
 public class DifferentialDriveOdometry {
   private final DifferentialDriveKinematics m_kinematics;
@@ -33,6 +38,9 @@ public class DifferentialDriveOdometry {
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
 
+  private double m_prevLeftDistance;
+  private double m_prevRightDistance;
+
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
@@ -59,8 +67,10 @@ public class DifferentialDriveOdometry {
   }
 
   /**
-   * Resets the robot's position on the field. Do NOT zero your encoders if you
-   * call this function at any other time except initialization.
+   * Resets the robot's position on the field.
+   *
+   * <p>If you are using the encoder distances method instead of the velocity method,
+   * you NEED to reset your encoders (to zero) when calling this method.
    *
    * <p>The gyroscope angle does not need to be reset here on the user's robot code.
    * The library automatically takes care of offsetting the gyro angle.
@@ -72,6 +82,9 @@ public class DifferentialDriveOdometry {
     m_poseMeters = poseMeters;
     m_previousAngle = poseMeters.getRotation();
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+
+    m_prevLeftDistance = 0.0;
+    m_prevRightDistance = 0.0;
   }
 
   /**
@@ -115,6 +128,36 @@ public class DifferentialDriveOdometry {
     return m_poseMeters;
   }
 
+  /**
+   * Updates the robot position on the field using distance measurements from encoders. This
+   * method is more numerically accurate than using velocities to integrate the pose and
+   * is also advantageous for teams that are using lower CPR encoders.
+   *
+   * @param gyroAngle           The angle reported by the gyroscope.
+   * @param leftDistanceMeters  The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
+                       double rightDistanceMeters) {
+    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
+
+    m_prevLeftDistance = leftDistanceMeters;
+    m_prevRightDistance = rightDistanceMeters;
+
+    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var newPose = m_poseMeters.exp(
+        new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+
   /**
    * Updates the robot's position on the field using forward kinematics and
    * integration of the pose over time. This method automatically calculates the

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java

@@ -67,4 +67,15 @@ class DifferentialDriveOdometryTest {
     );
   }
 
+  @Test
+  void testOdometryWithEncoderDistances() {
+    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+    var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
+
+    assertAll(
+        () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
 }