From 45201d15fc10a2ec2cbc3cd518872ddcf8bc6c7f Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Tue, 19 Nov 2019 12:56:34 -0500 Subject: [PATCH] Add encoder distance overload to DifferentialDriveOdometry (#2096) Also force encoders to be reset to zero on pose reset. --- .../kinematics/DifferentialDriveOdometry.cpp | 21 ++++++++ .../kinematics/DifferentialDriveOdometry.h | 34 ++++++++++-- .../DifferentialDriveOdometryTest.cpp | 12 +++++ .../kinematics/DifferentialDriveOdometry.java | 53 +++++++++++++++++-- .../DifferentialDriveOdometryTest.java | 11 ++++ 5 files changed, 123 insertions(+), 8 deletions(-) diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index 49a138e818..b4b3c49ec4 100644 --- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp +++ b/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; +} diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index eee829f45f..f67330aa47 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/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 diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp index 105ed29e97..1feae3a1f8 100644 --- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -57,3 +57,15 @@ TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) { EXPECT_NEAR(pose.Translation().Y().to(), 0.0, kEpsilon); EXPECT_NEAR(pose.Rotation().Degrees().to(), 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(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Translation().Y().to(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, kEpsilon); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java index 68e43d480a..530edbfbc9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java +++ b/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. * - *

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. */ 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. + * + *

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. @@ -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 diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java index 3870bcbc74..789526f68c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java +++ b/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) + ); + } }