Skip to content

Commit

Permalink
Use gyro angle instead of robot angle for odometry (#2081)
Browse files Browse the repository at this point in the history 
The odometry classes previously took in the robot angle as an argument, meaning that users had to take care of offsetting the gyro themselves to accurately report the robot angle. This change will make it so that users will not have to worry about resetting gyros and adding offsets themselves, as this will be handled by the odometry classes.
  • Loading branch information
@prateekma @PeterJohnson
prateekma authored and PeterJohnson committed Nov 16, 2019
1 parent 1b66ead commit 841ef91
Show file tree
Hide file tree
Showing 23 changed files with 258 additions and 77 deletions.
8 changes: 6 additions & 2 deletions wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View file
Expand Up @@ -10,18 +10,22 @@
using namespace frc;

DifferentialDriveOdometry::DifferentialDriveOdometry(
DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}

const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& angle,
units::second_t currentTime, const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;

auto angle = gyroAngle + m_gyroOffset;

auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);

Expand Down
6 changes: 5 additions & 1 deletion wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View file
Expand Up @@ -10,18 +10,22 @@
using namespace frc;

MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}

const Pose2d& MecanumDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& angle,
units::second_t currentTime, const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;

auto angle = gyroAngle + m_gyroOffset;

auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);

Expand Down
21 changes: 15 additions & 6 deletions wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View file
Expand Up @@ -33,19 +33,26 @@ class DifferentialDriveOdometry {
* Constructs a DifferentialDriveOdometry object.
*
* @param kinematics The differential drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());

/**
* Resets the robot's position on the field.
*
* 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.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose) {
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}

/**
Expand All @@ -63,13 +70,13 @@ class DifferentialDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& angle,
const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds);

/**
Expand All @@ -80,21 +87,23 @@ class DifferentialDriveOdometry {
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& angle,
const Pose2d& Update(const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
wheelSpeeds);
}

private:
DifferentialDriveKinematics m_kinematics;
Pose2d m_pose;

units::second_t m_previousTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
};
} // namespace frc
21 changes: 15 additions & 6 deletions wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
View file
Expand Up @@ -31,19 +31,26 @@ class MecanumDriveOdometry {
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());

/**
* Resets the robot's position on the field.
*
* 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.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose) {
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}

/**
Expand All @@ -61,13 +68,13 @@ class MecanumDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& angle,
const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);

/**
Expand All @@ -78,14 +85,15 @@ class MecanumDriveOdometry {
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& angle,
const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
wheelSpeeds);
}

private:
Expand All @@ -94,6 +102,7 @@ class MecanumDriveOdometry {

units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};

} // namespace frc
20 changes: 14 additions & 6 deletions wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View file
Expand Up @@ -36,19 +36,26 @@ class SwerveDriveOdometry {
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());

/**
* Resets the robot's position on the field.
*
* 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.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose) {
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}

/**
Expand All @@ -66,7 +73,7 @@ class SwerveDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
Expand All @@ -75,7 +82,7 @@ class SwerveDriveOdometry {
*/
template <typename... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& angle,
const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);

/**
Expand All @@ -86,17 +93,17 @@ class SwerveDriveOdometry {
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& Update(const Rotation2d& angle,
const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
moduleStates...);
}

Expand All @@ -106,6 +113,7 @@ class SwerveDriveOdometry {

units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};

} // namespace frc
Expand Down
8 changes: 6 additions & 2 deletions wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View file
Expand Up @@ -10,20 +10,24 @@
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}

template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& angle,
units::second_t currentTime, const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;

auto angle = gyroAngle + m_gyroOffset;

auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
static_cast<void>(dtheta);

Expand Down
21 changes: 17 additions & 4 deletions wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View file
Expand Up @@ -17,9 +17,9 @@ using namespace frc;

TEST(DifferentialDriveOdometry, OneIteration) {
DifferentialDriveKinematics kinematics{0.381_m * 2};
DifferentialDriveOdometry odometry{kinematics};
DifferentialDriveOdometry odometry{kinematics, 0_rad};

odometry.ResetPosition(Pose2d());
odometry.ResetPosition(Pose2d(), 0_rad);
DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
Expand All @@ -31,9 +31,9 @@ TEST(DifferentialDriveOdometry, OneIteration) {

TEST(DifferentialDriveOdometry, QuarterCircle) {
DifferentialDriveKinematics kinematics{0.381_m * 2};
DifferentialDriveOdometry odometry{kinematics};
DifferentialDriveOdometry odometry{kinematics, 0_rad};

odometry.ResetPosition(Pose2d());
odometry.ResetPosition(Pose2d(), 0_rad);
DifferentialDriveWheelSpeeds wheelSpeeds{
0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
Expand All @@ -44,3 +44,16 @@ TEST(DifferentialDriveOdometry, QuarterCircle) {
EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
}

TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) {
DifferentialDriveKinematics kinematics{0.381_m * 2};
DifferentialDriveOdometry odometry{kinematics, Rotation2d(90_deg)};

odometry.UpdateWithTime(0_s, Rotation2d(90_deg), {});
const auto& pose =
odometry.UpdateWithTime(1_s, Rotation2d(90_deg), {1_mps, 1_mps});

EXPECT_NEAR(pose.Translation().X().to<double>(), 1.0, kEpsilon);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}
21 changes: 17 additions & 4 deletions wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View file
Expand Up @@ -18,11 +18,11 @@ class MecanumDriveOdometryTest : public ::testing::Test {
Translation2d m_br{-12_m, -12_m};

MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
MecanumDriveOdometry odometry{kinematics};
MecanumDriveOdometry odometry{kinematics, 0_rad};
};

TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.ResetPosition(Pose2d());
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};

Expand All @@ -35,7 +35,7 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
}

TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.ResetPosition(Pose2d());
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};

odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
Expand All @@ -47,7 +47,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
}

TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
odometry.ResetPosition(Pose2d());
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
39.986_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
Expand All @@ -57,3 +57,16 @@ TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
}

TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));

MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};

odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);

EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}

0 comments on commit 841ef91

Please sign in to comment.