Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Position Delta Odometry for Mecanum (#4514)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:59 -04:00
committed by GitHub
parent 4170ec6107
commit 901fc555f4
28 changed files with 1222 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
View File

@@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
@@ -43,9 +44,13 @@ class MecanumControllerCommandTest {
private Rotation2d m_angle = new Rotation2d(0);
private double m_frontLeftSpeed;
private double m_frontLeftDistance;
private double m_rearLeftSpeed;
private double m_rearLeftDistance;
private double m_frontRightSpeed;
private double m_frontRightDistance;
private double m_rearRightSpeed;
private double m_rearRightDistance;
private final ProfiledPIDController m_rotController =
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
@@ -66,7 +71,10 @@ class MecanumControllerCommandTest {
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(
m_kinematics, new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
m_kinematics,
new Rotation2d(0),
new MecanumDriveWheelPositions(),
new Pose2d(0, 0, new Rotation2d(0)));
public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
@@ -75,13 +83,13 @@ class MecanumControllerCommandTest {
this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
}
public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
return new MecanumDriveWheelSpeeds(
m_frontLeftSpeed, m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftDistance, m_frontRightDistance, m_rearLeftDistance, m_rearRightDistance);
}
public Pose2d getRobotPose() {
m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds());
m_odometry.update(m_angle, getCurrentWheelDistances());
return m_odometry.getPoseMeters();
}
@@ -117,6 +125,10 @@ class MecanumControllerCommandTest {
while (!command.isFinished()) {
command.execute();
m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
m_frontLeftDistance += m_frontLeftSpeed * 0.005;
m_rearLeftDistance += m_rearLeftSpeed * 0.005;
m_frontRightDistance += m_frontRightSpeed * 0.005;
m_rearRightDistance += m_rearRightSpeed * 0.005;
SimHooks.stepTiming(0.005);
}
m_timer.stop();

20
wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
View File

@@ -32,9 +32,13 @@ class MecanumControllerCommandTest : public ::testing::Test {
frc::Rotation2d m_angle{0_rad};
units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
units::meter_t m_frontLeftDistance = 0.0_m;
units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
units::meter_t m_rearLeftDistance = 0.0_m;
units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
units::meter_t m_frontRightDistance = 0.0_m;
units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
units::meter_t m_rearRightDistance = 0.0_m;
frc::ProfiledPIDController<units::radians> m_rotController{
1, 0, 0,
@@ -55,6 +59,7 @@ class MecanumControllerCommandTest : public ::testing::Test {
frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
getCurrentWheelDistances(),
frc::Pose2d{0_m, 0_m, 0_rad}};
void SetUp() override { frc::sim::PauseTiming(); }
@@ -66,8 +71,17 @@ class MecanumControllerCommandTest : public ::testing::Test {
m_rearLeftSpeed, m_rearRightSpeed};
}
frc::MecanumDriveWheelPositions getCurrentWheelDistances() {
return frc::MecanumDriveWheelPositions{
m_frontLeftDistance,
m_rearLeftDistance,
m_frontRightDistance,
m_rearRightDistance,
};
}
frc::Pose2d getRobotPose() {
m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
m_odometry.Update(m_angle, getCurrentWheelDistances());
return m_odometry.GetPose();
}
};
@@ -105,6 +119,10 @@ TEST_F(MecanumControllerCommandTest, ReachesReference) {
while (!command.IsFinished()) {
command.Execute();
m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
m_frontLeftDistance += m_frontLeftSpeed * 5_ms;
m_rearLeftDistance += m_rearLeftSpeed * 5_ms;
m_frontRightDistance += m_frontRightSpeed * 5_ms;
m_rearRightDistance += m_rearRightSpeed * 5_ms;
frc::sim::StepTiming(5_ms);
}
m_timer.Stop();

9
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -11,6 +11,13 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
frc::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances() const {
return {units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_backLeftEncoder.GetDistance()},
units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
const auto frontLeftFeedforward =
m_feedforward.Calculate(wheelSpeeds.frontLeft);
@@ -52,5 +59,5 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentState());
m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentWheelDistances());
}

4
wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
View File

@@ -31,6 +31,7 @@ class Drivetrain {
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
frc::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
@@ -69,7 +70,8 @@ class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d()};
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d(),
GetCurrentWheelDistances()};
// Gains are for example purposes only - must be determined for your own
// robot!

22
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -28,7 +28,8 @@ DriveSubsystem::DriveSubsystem()
m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
kRearRightEncoderReversed},
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d{}} {
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(),
getCurrentWheelDistances(), frc::Pose2d{}} {
// Set the distance per pulse for the encoders
m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -43,13 +44,7 @@ DriveSubsystem::DriveSubsystem()
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(
m_gyro.GetRotation2d(),
frc::MecanumDriveWheelSpeeds{
units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
units::meters_per_second_t{m_rearLeftEncoder.GetRate()},
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
units::meters_per_second_t{m_rearRightEncoder.GetRate()}});
m_odometry.Update(m_gyro.GetRotation2d(), getCurrentWheelDistances());
}
void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
@@ -102,6 +97,14 @@ frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
units::meters_per_second_t{m_rearRightEncoder.GetRate()}});
}
frc::MecanumDriveWheelPositions DriveSubsystem::getCurrentWheelDistances() {
return (frc::MecanumDriveWheelPositions{
units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_rearLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_rearRightEncoder.GetDistance()}});
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
@@ -123,5 +126,6 @@ frc::Pose2d DriveSubsystem::GetPose() {
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d(),
getCurrentWheelDistances());
}

7
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -81,6 +81,13 @@ class DriveSubsystem : public frc2::SubsystemBase {
*/
frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
/**
* Gets the distances travelled by each wheel.
*
* @return the distances travelled by each wheel.
*/
frc::MecanumDriveWheelPositions getCurrentWheelDistances();
/**
* Sets the drive MotorControllers to a desired voltage.
*/

10
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -15,6 +15,13 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
frc::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const {
return {units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_backLeftEncoder.GetDistance()},
units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
std::function<void(units::meters_per_second_t, const frc::Encoder&,
frc2::PIDController&, frc::PWMSparkMax&)>
@@ -49,7 +56,8 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState());
m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState(),
GetCurrentDistances());
// Also apply vision measurements. We use 0.3 seconds in the past as an
// example -- on a real robot, this must be calculated based either on latency

12
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
View File

@@ -32,6 +32,7 @@ class Drivetrain {
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
frc::MecanumDriveWheelPositions GetCurrentDistances() const;
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
@@ -75,7 +76,12 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{0_deg, frc::Pose2d{},
m_kinematics, {0.1, 0.1, 0.1},
{0.05}, {0.1, 0.1, 0.1}};
frc::MecanumDrivePoseEstimator m_poseEstimator{
0_deg,
frc::Pose2d{},
GetCurrentDistances(),
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
{0.1, 0.1, 0.1}};
};

18
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
@@ -48,7 +49,7 @@ public class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d());
new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d(), getCurrentDistances());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
@@ -76,6 +77,19 @@ public class Drivetrain {
m_backRightEncoder.getRate());
}
/**
* Returns the current distances measured by the drivetrain.
*
* @return The current distances measured by the drivetrain.
*/
public MecanumDriveWheelPositions getCurrentDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_backLeftEncoder.getDistance(),
m_backRightEncoder.getDistance());
}
/**
* Set the desired speeds for each wheel.
*
@@ -126,6 +140,6 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(m_gyro.getRotation2d(), getCurrentState());
m_odometry.update(m_gyro.getRotation2d(), getCurrentDistances());
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
@@ -58,7 +59,10 @@ public class DriveSubsystem extends SubsystemBase {
// Odometry class for tracking robot pose
MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
new MecanumDriveOdometry(
DriveConstants.kDriveKinematics,
m_gyro.getRotation2d(),
new MecanumDriveWheelPositions());
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
@@ -77,13 +81,7 @@ public class DriveSubsystem extends SubsystemBase {
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
m_gyro.getRotation2d(),
new MecanumDriveWheelSpeeds(
m_frontLeftEncoder.getRate(),
m_rearLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_rearRightEncoder.getRate()));
m_odometry.update(m_gyro.getRotation2d(), getCurrentWheelDistances());
}
/**
@@ -101,7 +99,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_odometry.resetPosition(pose, m_gyro.getRotation2d(), getCurrentWheelDistances());
}
/**
@@ -186,6 +184,19 @@ public class DriveSubsystem extends SubsystemBase {
m_rearRightEncoder.getRate());
}
/**
* Gets the current wheel distance measurements.
*
* @return the current wheel distance measurements in a MecanumDriveWheelPositions object.
*/
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_rearLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_rearRightEncoder.getDistance());
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*

21
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogGyro;
@@ -57,9 +58,10 @@ public class Drivetrain {
new MecanumDrivePoseEstimator(
m_gyro.getRotation2d(),
new Pose2d(),
getCurrentDistances(),
m_kinematics,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(Units.degreesToRadians(0.01)),
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
// Gains are for example purposes only - must be determined for your own robot!
@@ -88,6 +90,19 @@ public class Drivetrain {
m_backRightEncoder.getRate());
}
/**
* Returns the current distances measured by the drivetrain.
*
* @return The current distances measured by the drivetrain.
*/
public MecanumDriveWheelPositions getCurrentDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_backLeftEncoder.getDistance(),
m_backRightEncoder.getDistance());
}
/**
* Set the desired speeds for each wheel.
*
@@ -138,7 +153,7 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState());
m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState(), getCurrentDistances());
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.

123
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -14,9 +14,12 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.function.BiConsumer;
@@ -29,7 +32,7 @@ import java.util.function.BiConsumer;
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 20 ms, then you should change the nominal delta time using
* the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
* Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
* Pose2d, MecanumDriveWheelPositions, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
*
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
@@ -37,19 +40,21 @@ import java.util.function.BiConsumer;
* <p>The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
*
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
* <p><strong> x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading, followed by the distance driven by the front
* left, front right, rear left, and rear right wheels.
*
* <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
* in the field coordinate system.
* <p><strong> u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ </strong> containing x velocity, y
* velocity, and angular rate in the field coordinate system, followed by the velocity of the front
* left, front right, rear left, and rear right wheels.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
*/
public class MecanumDrivePoseEstimator {
private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
private final UnscentedKalmanFilter<N7, N7, N5> m_observer;
private final MecanumDriveKinematics m_kinematics;
private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
private final BiConsumer<Matrix<N7, N1>, Matrix<N3, N1>> m_visionCorrect;
private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
private final double m_nominalDt; // Seconds
@@ -65,13 +70,14 @@ public class MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
* meters and radians.
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
* s_rr]ᵀ, with units in meters and radians, followed by meters.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
* to trust sensor readings from the gyro less. This matrix is in the form [theta], with units
* in radians.
* to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr,
* s_rl, s_rr], with units in radians, followed by meters.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
@@ -79,13 +85,15 @@ public class MecanumDrivePoseEstimator {
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
initialPoseMeters,
wheelPositions,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
@@ -98,13 +106,14 @@ public class MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in radians.
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
* s_rr]ᵀ, with units in meters and radians, followed by meters.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
* to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr,
* s_rl, s_rr], with units in radians, followed by meters.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
@@ -113,19 +122,20 @@ public class MecanumDrivePoseEstimator {
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs,
double nominalDtSeconds) {
m_nominalDt = nominalDtSeconds;
m_observer =
new UnscentedKalmanFilter<>(
Nat.N3(),
Nat.N1(),
Nat.N7(),
Nat.N5(),
(x, u) -> u,
(x, u) -> x.extractRowVector(2),
(x, u) -> x.block(Nat.N5(), Nat.N1(), 2, 0),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
@@ -146,7 +156,7 @@ public class MecanumDrivePoseEstimator {
Nat.N3(),
u,
y,
(x, u1) -> x,
(x, u1) -> x.block(Nat.N3(), Nat.N1(), 0, 0),
m_visionContR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -155,7 +165,19 @@ public class MecanumDrivePoseEstimator {
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters);
var xhat =
VecBuilder.fill(
poseVec.get(0, 0),
poseVec.get(1, 0),
poseVec.get(2, 0),
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
m_observer.setXhat(xhat);
}
/**
@@ -181,13 +203,26 @@ public class MecanumDrivePoseEstimator {
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters);
var xhat =
VecBuilder.fill(
poseVec.get(0, 0),
poseVec.get(1, 0),
poseVec.get(2, 0),
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
m_observer.setXhat(xhat);
m_prevTimeSeconds = -1;
@@ -227,7 +262,7 @@ public class MecanumDrivePoseEstimator {
var sample = m_poseBuffer.getSample(timestampSeconds);
if (sample.isPresent()) {
m_visionCorrect.accept(
new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
new MatBuilder<>(Nat.N7(), Nat.N1()).fill(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
StateSpaceUtil.poseTo3dVector(
getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
}
@@ -273,10 +308,14 @@ public class MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
public Pose2d update(
Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds,
MecanumDriveWheelPositions wheelPositions) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds, wheelPositions);
}
/**
@@ -287,10 +326,14 @@ public class MecanumDrivePoseEstimator {
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double currentTimeSeconds,
Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds,
MecanumDriveWheelPositions wheelPositions) {
double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
m_prevTimeSeconds = currentTimeSeconds;
@@ -302,10 +345,24 @@ public class MecanumDrivePoseEstimator {
new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
.rotateBy(angle);
var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
var u =
VecBuilder.fill(
fieldRelativeVelocities.getX(),
fieldRelativeVelocities.getY(),
omega,
wheelSpeeds.frontLeftMetersPerSecond,
wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond,
wheelSpeeds.rearRightMetersPerSecond);
m_previousAngle = angle;
var localY = VecBuilder.fill(angle.getRadians());
var localY =
VecBuilder.fill(
angle.getRadians(),
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
m_observer.predict(u, dt);
m_observer.correct(u, localY);

23
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import org.ejml.simple.SimpleMatrix;
/**
@@ -153,6 +154,28 @@ public class MecanumDriveKinematics {
chassisSpeedsVector.get(2, 0));
}
/**
* Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
* method is often used for odometry -- determining the robot's position on the field using
* changes in the distance driven by each wheel on the robot.
*
* @param wheelDeltas The distances driven by each wheel.
* @return The resulting Twist2d.
*/
public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
var wheelDeltasVector = new SimpleMatrix(4, 1);
wheelDeltasVector.setColumn(
0,
0,
wheelDeltas.frontLeftMeters,
wheelDeltas.frontRightMeters,
wheelDeltas.rearLeftMeters,
wheelDeltas.rearRightMeters);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}
/**
* Construct inverse kinematics matrix from wheel locations.
*

89
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -8,8 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.util.WPIUtilJNI;
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
@@ -21,7 +19,7 @@ import edu.wpi.first.util.WPIUtilJNI;
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private MecanumDriveWheelPositions m_previousWheelPositions;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
@@ -31,14 +29,24 @@ public class MecanumDriveOdometry {
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry(
MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_previousWheelPositions =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
@@ -47,9 +55,13 @@ public class MecanumDriveOdometry {
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
public MecanumDriveOdometry(
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, new Pose2d());
}
/**
@@ -60,11 +72,19 @@ public class MecanumDriveOdometry {
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousWheelPositions =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
}
/**
@@ -78,48 +98,37 @@ public class MecanumDriveOdometry {
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in the current time as a parameter to calculate period (difference
* between two timestamps). The period is used to calculate the change in distance from a
* velocity. This also takes in an angle parameter which is used instead of the angular rate that
* is calculated from forward kinematics.
* over time. This method takes in an angle parameter which is used instead of the angular rate
* that is calculated from forward kinematics, in addition to the current distance measurement at
* each wheel.
*
* @param currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @param wheelPositions The distances driven by each wheel.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose =
m_poseMeters.exp(
new Twist2d(
chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
var wheelDeltas =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
var twist = m_kinematics.toTwist2d(wheelDeltas);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_poseMeters.exp(twist);
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
m_previousWheelPositions =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
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 current time to calculate period
* (difference between two timestamps). The period is used to calculate the change in distance
* from a velocity. This also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
}
}

49
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java Normal file
View File

@@ -0,0 +1,49 @@
// 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.kinematics;
public class MecanumDriveWheelPositions {
/** Distance measured by the front left wheel. */
public double frontLeftMeters;
/** Distance measured by the front right wheel. */
public double frontRightMeters;
/** Distance measured by the rear left wheel. */
public double rearLeftMeters;
/** Distance measured by the rear right wheel. */
public double rearRightMeters;
/** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
public MecanumDriveWheelPositions() {}
/**
* Constructs a MecanumDriveWheelPositions.
*
* @param frontLeftMeters Distance measured by the front left wheel.
* @param frontRightMeters Distance measured by the front right wheel.
* @param rearLeftMeters Distance measured by the rear left wheel.
* @param rearRightMeters Distance measured by the rear right wheel.
*/
public MecanumDriveWheelPositions(
double frontLeftMeters,
double frontRightMeters,
double rearLeftMeters,
double rearRightMeters) {
this.frontLeftMeters = frontLeftMeters;
this.frontRightMeters = frontRightMeters;
this.rearLeftMeters = rearLeftMeters;
this.rearRightMeters = rearRightMeters;
}
@Override
public String toString() {
return String.format(
"MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
+ "Rear Left: %.2f m, Rear Right: %.2f m)",
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
}

81
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -13,32 +13,47 @@ using namespace frc;
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const MecanumDriveWheelPositions& wheelPositions,
MecanumDriveKinematics kinematics,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
: m_observer([](const Vectord<7>& x, const Vectord<7>& u) { return u; },
[](const Vectord<7>& x, const Vectord<7>& u) {
return x.block<5, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<7, 7>(2),
frc::AngleMean<5, 7>(0), frc::AngleResidual<7>(2),
frc::AngleResidual<5>(0), frc::AngleAdd<7>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create vision correction mechanism.
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_visionCorrect = [&](const Vectord<7>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
u, y,
[](const Vectord<7>& x, const Vectord<7>& u) {
return x.template block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, 7>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<7>(2), frc::AngleAdd<7>(2));
};
// Set initial state.
m_observer.SetXhat(PoseTo3dVector(initialPose));
auto poseVec = PoseTo3dVector(initialPose);
auto xhat = Vectord<7>{
poseVec(0),
poseVec(1),
poseVec(2),
wheelPositions.frontLeft.value(),
wheelPositions.frontRight.value(),
wheelPositions.rearLeft.value(),
wheelPositions.rearRight.value(),
};
m_observer.SetXhat(xhat);
// Calculate offsets.
m_gyroOffset = initialPose.Rotation() - gyroAngle;
@@ -52,12 +67,24 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
m_observer.SetXhat(PoseTo3dVector(pose));
auto poseVec = PoseTo3dVector(pose);
auto xhat = Vectord<7>{
poseVec(0),
poseVec(1),
poseVec(2),
wheelPositions.frontLeft.value(),
wheelPositions.frontRight.value(),
wheelPositions.rearLeft.value(),
wheelPositions.rearRight.value(),
};
m_observer.SetXhat(xhat);
m_prevTime = -1_s;
@@ -73,21 +100,23 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Vectord<3>::Zero(),
m_visionCorrect(Vectord<7>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
}
Pose2d frc::MecanumDrivePoseEstimator::Update(
const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
wheelSpeeds);
wheelSpeeds, wheelPositions);
}
Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds) {
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
@@ -99,10 +128,18 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy(
angle);
Vectord<3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(), omega.value()};
Vectord<7> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(),
omega.value(),
wheelSpeeds.frontLeft.value(),
wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(),
wheelSpeeds.rearRight.value()};
Vectord<1> localY{angle.Radians().value()};
Vectord<5> localY{angle.Radians().value(), wheelPositions.frontLeft.value(),
wheelPositions.frontRight.value(),
wheelPositions.rearLeft.value(),
wheelPositions.rearRight.value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());

12
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -49,6 +49,18 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const {
Vectord<4> wheelDeltasVector{
wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {units::meter_t{twistVector(0)}, // NOLINT
units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,

35
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -8,32 +8,37 @@
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
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;
const Pose2d& MecanumDriveOdometry::Update(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
MecanumDriveWheelPositions wheelDeltas{
wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
wheelPositions.frontRight - m_previousWheelPositions.frontRight,
wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
wheelPositions.rearRight - m_previousWheelPositions.rearRight,
};
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
auto twist = m_kinematics.ToTwist2d(wheelDeltas);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_previousWheelPositions = wheelPositions;
m_pose = {newPose.Translation(), angle};
return m_pose;

49
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -35,11 +35,15 @@ namespace frc {
* The state-space system used internally has the following states (x), inputs
* (u), and outputs (y):
*
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading.
* <strong> x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> in the field
* coordinate system containing x position, y position, and heading, followed
* by the distance driven by the front left, front right, rear left, and rear
* right wheels.
*
* <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
* and angular velocity in the field coordinate system.
* <strong> u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ </strong> containing
* x velocity, y velocity, and angular rate in the field coordinate system,
* followed by the velocity of the front left, front right, rear left, and rear
* right wheels.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
@@ -52,17 +56,21 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param initialPose The starting pose estimate.
* @param wheelPositions The distance measured by each wheel.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form [x, y, theta]ᵀ, with units
* in meters and radians.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement.
* Increase this number to trust sensor
* readings from the gyro less. This matrix is
* in the form [theta], with units in radians.
* is in the form [x, y, theta, s_fl, s_fr,
* s_rl, s_rr]ᵀ, with units in meters and
* radians, followed by meters.
* @param localMeasurementStdDevs Standard deviation of the gyro
* measurement. Increase this number to trust
* sensor readings from the gyro less. This
* matrix is in the form [theta, s_fl, s_fr,
* s_rl, s_rr], with units in radians,
* followed by meters.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
@@ -74,9 +82,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*/
MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const MecanumDriveWheelPositions& wheelPositions,
MecanumDriveKinematics kinematics,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 20_ms);
@@ -105,8 +114,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances measured at each wheel.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Gets the pose of the robot at the current time as estimated by the Extended
@@ -190,10 +201,12 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds);
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
@@ -203,17 +216,19 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds);
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
private:
UnscentedKalmanFilter<3, 3, 1> m_observer;
UnscentedKalmanFilter<7, 7, 5> m_observer;
MecanumDriveKinematics m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
std::function<void(const Vectord<7>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix3d m_visionContR;

14
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
View File

@@ -9,7 +9,9 @@
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "wpimath/MathShared.h"
@@ -114,6 +116,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
/**
* Performs forward kinematics to return the resulting Twist2d from the
* given wheel position deltas. This method is often used for odometry --
* determining the robot's position on the field using data from the
* distance driven by each wheel on the robot.
*
* @param wheelDeltas The change in distance driven by each wheel.
*
* @return The resulting chassis speed.
*/
Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
private:
mutable Matrixd<4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;

41
wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
View File

@@ -30,10 +30,12 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions,
const Pose2d& initialPose = Pose2d{});
/**
@@ -44,11 +46,14 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_previousWheelPositions = wheelPositions;
}
/**
@@ -59,45 +64,23 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @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& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
* the current time to calculate period (difference between two timestamps).
* The period is used to calculate the change in distance from a velocity.
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
* integration of the pose over time. This method takes in an angle parameter
* which is used instead of the angular rate that is calculated from forward
* kinematics, in addition to the current distance measurement at each wheel.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @param wheelPositions The current distances measured by each wheel.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
}
const MecanumDriveWheelPositions wheelPositions);
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
MecanumDriveWheelPositions m_previousWheelPositions;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};

36
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h Normal file
View File

@@ -0,0 +1,36 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "units/length.h"
namespace frc {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
/**
* Distance driven by the front-left wheel.
*/
units::meter_t frontLeft = 0_m;
/**
* Distance driven by the front-right wheel.
*/
units::meter_t frontRight = 0_m;
/**
* Distance driven by the rear-left wheel.
*/
units::meter_t rearLeft = 0_m;
/**
* Distance driven by the rear-right wheel.
*/
units::meter_t rearRight = 0_m;
};
} // namespace frc

117
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
@@ -20,19 +21,22 @@ import org.junit.jupiter.api.Test;
class MecanumDrivePoseEstimatorTest {
@Test
void testAccuracy() {
void testAccuracyFacingTrajectory() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
var wheelPositions = new MecanumDriveWheelPositions();
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
wheelPositions,
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.05),
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
VecBuilder.fill(0.1, 0.1, 0.1));
var trajectory =
@@ -90,6 +94,11 @@ class MecanumDrivePoseEstimatorTest {
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
var xHat =
estimator.updateWithTime(
t,
@@ -97,7 +106,107 @@ class MecanumDrivePoseEstimatorTest {
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
wheelSpeeds);
wheelSpeeds,
wheelPositions);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
}
@Test
void testAccuracyFacingXAxis() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
var wheelPositions = new MecanumDriveWheelPositions();
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
wheelPositions,
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
VecBuilder.fill(0.1, 0.1, 0.1));
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
new TrajectoryConfig(0.5, 2));
var rand = new Random(5190);
final double dt = 0.02;
double t = 0.0;
final double visionUpdateRate = 0.1;
Pose2d lastVisionPose = null;
double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
if (lastVisionUpdateTime + visionUpdateRate < t) {
if (lastVisionPose != null) {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose =
new Pose2d(
new Translation2d(
groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
groundTruthState.poseMeters.getTranslation().getY()
+ rand.nextGaussian() * 0.1),
new Rotation2d(rand.nextGaussian() * 0.1)
.plus(groundTruthState.poseMeters.getRotation()));
lastVisionUpdateTime = t;
}
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getCos(),
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getSin(),
0));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
var xHat =
estimator.updateWithTime(
t, new Rotation2d(rand.nextGaussian() * 0.05), wheelSpeeds, wheelPositions);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());

66
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -44,6 +44,17 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testStraightLineForwardKinematicsKinematicsWithDeltas() {
var wheelDeltas = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
var twist = m_kinematics.toTwist2d(wheelDeltas);
assertAll(
() -> assertEquals(3.536, twist.dx, 0.1),
() -> assertEquals(0, twist.dy, 0.1),
() -> assertEquals(0, twist.dtheta, 0.1));
}
@Test
void testStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
@@ -67,6 +78,17 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testStrafeForwardKinematicsKinematicsWithDeltas() {
var wheelDeltas = new MecanumDriveWheelPositions(-2.828427, 2.828427, 2.828427, -2.828427);
var twist = m_kinematics.toTwist2d(wheelDeltas);
assertAll(
() -> assertEquals(0, twist.dx, 0.1),
() -> assertEquals(2.8284, twist.dy, 0.1),
() -> assertEquals(0, twist.dtheta, 0.1));
}
@Test
void testRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
@@ -90,6 +112,17 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testRotationForwardKinematicsKinematicsWithDeltas() {
var wheelDeltas = new MecanumDriveWheelPositions(-150.79645, 150.79645, -150.79645, 150.79645);
var twist = m_kinematics.toTwist2d(wheelDeltas);
assertAll(
() -> assertEquals(0, twist.dx, 0.1),
() -> assertEquals(0, twist.dy, 0.1),
() -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
}
@Test
void testMixedTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
@@ -113,6 +146,17 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testMixedTranslationRotationForwardKinematicsKinematicsWithDeltas() {
var wheelDeltas = new MecanumDriveWheelPositions(-17.677670, 20.51, -13.44, 16.26);
var twist = m_kinematics.toTwist2d(wheelDeltas);
assertAll(
() -> assertEquals(1.413, twist.dx, 0.1),
() -> assertEquals(2.122, twist.dy, 0.1),
() -> assertEquals(0.707, twist.dtheta, 0.1));
}
@Test
void testOffCenterRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
@@ -136,6 +180,17 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testOffCenterRotationForwardKinematicsKinematicsWithDeltas() {
var wheelDeltas = new MecanumDriveWheelPositions(0, 16.971, -16.971, 33.941);
var twist = m_kinematics.toTwist2d(wheelDeltas);
assertAll(
() -> assertEquals(8.48525, twist.dx, 0.1),
() -> assertEquals(-8.48525, twist.dy, 0.1),
() -> assertEquals(0.707, twist.dtheta, 0.1));
}
@Test
void testOffCenterTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
@@ -159,6 +214,17 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testOffCenterRotationTranslationForwardKinematicsKinematicsWithDeltas() {
var wheelDeltas = new MecanumDriveWheelPositions(2.12, 21.92, -12.02, 36.06);
var twist = m_kinematics.toTwist2d(wheelDeltas);
assertAll(
() -> assertEquals(12.02, twist.dx, 0.1),
() -> assertEquals(-7.07, twist.dy, 0.1),
() -> assertEquals(0.707, twist.dtheta, 0.1));
}
@Test
void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);

212
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
View File

@@ -10,6 +10,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
class MecanumDriveOdometryTest {
@@ -21,15 +25,19 @@ class MecanumDriveOdometryTest {
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions();
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, new Rotation2d());
new MecanumDriveOdometry(m_kinematics, new Rotation2d(), zero);
@Test
void testMultipleConsecutiveUpdates() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
m_odometry.resetPosition(new Pose2d(), new Rotation2d(), wheelPositions);
m_odometry.update(new Rotation2d(), wheelPositions);
var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);
assertAll(
() -> assertEquals(secondPose.getX(), 0.0, 0.01),
@@ -40,10 +48,11 @@ class MecanumDriveOdometryTest {
@Test
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());
m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
var pose = m_odometry.update(new Rotation2d(), wheelPositions);
assertAll(
() -> assertEquals(0.3536, pose.getX(), 0.01),
@@ -55,10 +64,11 @@ class MecanumDriveOdometryTest {
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());
m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);
assertAll(
() -> assertEquals(8.4855, pose.getX(), 0.01),
@@ -70,14 +80,188 @@ class MecanumDriveOdometryTest {
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
m_odometry.resetPosition(
new Pose2d(new Translation2d(), fieldAngle), gyro, new MecanumDriveWheelPositions());
var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.update(gyro, new MecanumDriveWheelPositions());
var pose = m_odometry.update(gyro, speeds);
assertAll(
() -> assertEquals(3.536, pose.getX(), 0.1),
() -> assertEquals(0.0, pose.getY(), 0.1),
() -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1));
}
@Test
void testAccuracyFacingTrajectory() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
var wheelPositions = new MecanumDriveWheelPositions();
var odometry =
new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
new TrajectoryConfig(0.5, 2));
var rand = new Random(5190);
final double dt = 0.02;
double t = 0.0;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
double odometryDistanceTravelled = 0;
double trajectoryDistanceTravelled = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
trajectoryDistanceTravelled +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
var lastPose = odometry.getPoseMeters();
var xHat =
odometry.update(
groundTruthState
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
wheelPositions);
odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, maxError, 0.3, "Incorrect max error");
assertEquals(
1.0,
odometryDistanceTravelled / trajectoryDistanceTravelled,
0.05,
"Incorrect distance travelled");
}
@Test
void testAccuracyFacingXAxis() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
var wheelPositions = new MecanumDriveWheelPositions();
var odometry =
new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
new TrajectoryConfig(0.5, 2));
var rand = new Random(5190);
final double dt = 0.02;
double t = 0.0;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
double odometryDistanceTravelled = 0;
double trajectoryDistanceTravelled = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
trajectoryDistanceTravelled +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getCos(),
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getSin(),
0));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
var lastPose = odometry.getPoseMeters();
var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions);
odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, maxError, 0.3, "Incorrect max error");
assertEquals(
1.0,
odometryDistanceTravelled / trajectoryDistanceTravelled,
0.05,
"Incorrect distance travelled");
}
}

105
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -8,20 +8,23 @@
#include "frc/estimator/MecanumDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/MecanumDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
frc::Rotation2d{}, frc::Pose2d{}, kinematics,
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}};
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
wheelPositions,
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
{0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
@@ -66,11 +69,99 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
wheelSpeeds);
wheelSpeeds, wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.125);
}
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
wheelPositions,
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
{0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 0.02_s;
units::second_t t = 0_s;
units::second_t kVisionUpdateRate = 0.1_s;
frc::Pose2d lastVisionPose;
units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
std::vector<frc::Pose2d> visionPoses;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
if (lastVisionPose != frc::Pose2d{}) {
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose =
groundTruthState.pose +
frc::Transform2d{
frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}};
visionPoses.push_back(lastVisionPose);
lastVisionUpdateTime = t;
}
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t, frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelSpeeds,
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();

60
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -40,6 +40,15 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
MecanumDriveWheelPositions wheelDeltas{5_m, 5_m, 5_m, 5_m};
auto twist = kinematics.ToTwist2d(wheelDeltas);
EXPECT_NEAR(5.0, twist.dx.value(), 0.1);
EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
@@ -59,6 +68,15 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
MecanumDriveWheelPositions wheelDeltas{-5_m, 5_m, 5_m, -5_m};
auto twist = kinematics.ToTwist2d(wheelDeltas);
EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
EXPECT_NEAR(5.0, twist.dy.value(), 0.1);
EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t{2 * std::numbers::pi}};
@@ -80,6 +98,16 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
MecanumDriveWheelPositions wheelDeltas{-150.79644737_m, 150.79644737_m,
-150.79644737_m, 150.79644737_m};
auto twist = kinematics.ToTwist2d(wheelDeltas);
EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
EXPECT_NEAR(2 * std::numbers::pi, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
@@ -101,6 +129,18 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
MixedRotationTranslationForwardKinematicsWithDeltas) {
MecanumDriveWheelPositions wheelDeltas{-17.677670_m, 20.506097_m, -13.435_m,
16.26_m};
auto twist = kinematics.ToTwist2d(wheelDeltas);
EXPECT_NEAR(1.41335, twist.dx.value(), 0.1);
EXPECT_NEAR(2.1221, twist.dy.value(), 0.1);
EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
@@ -121,6 +161,16 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
OffCenterRotationForwardKinematicsWithDeltas) {
MecanumDriveWheelPositions wheelDeltas{0_m, 16.971_m, -16.971_m, 33.941_m};
auto twist = kinematics.ToTwist2d(wheelDeltas);
EXPECT_NEAR(8.48525, twist.dx.value(), 0.1);
EXPECT_NEAR(-8.48525, twist.dy.value(), 0.1);
EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationInverseKinematics) {
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
@@ -143,6 +193,16 @@ TEST_F(MecanumDriveKinematicsTest,
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationForwardKinematicsWithDeltas) {
MecanumDriveWheelPositions wheelDeltas{2.12_m, 21.92_m, -12.02_m, 36.06_m};
auto twist = kinematics.ToTwist2d(wheelDeltas);
EXPECT_NEAR(12.02, twist.dx.value(), 0.1);
EXPECT_NEAR(-7.07, twist.dy.value(), 0.1);
EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Desaturate(5.5_mps);

171
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -2,7 +2,11 @@
// 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.
#include <limits>
#include <random>
#include "frc/kinematics/MecanumDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
using namespace frc;
@@ -14,17 +18,19 @@ class MecanumDriveOdometryTest : public ::testing::Test {
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
MecanumDriveWheelPositions zero;
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
MecanumDriveOdometry odometry{kinematics, 0_rad};
MecanumDriveOdometry odometry{kinematics, 0_rad, zero};
};
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.ResetPosition(Pose2d{}, 0_rad);
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
odometry.UpdateWithTime(0_s, 0_deg, wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, 0_deg, wheelSpeeds);
odometry.ResetPosition(Pose2d{}, 0_rad, wheelDeltas);
odometry.Update(0_deg, wheelDeltas);
auto secondPose = odometry.Update(0_deg, wheelDeltas);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -32,11 +38,12 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.ResetPosition(Pose2d{}, 0_rad);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
odometry.ResetPosition(Pose2d{}, 0_rad, zero);
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, 0_deg, speeds);
odometry.Update(0_deg, MecanumDriveWheelPositions{});
auto pose = odometry.Update(0_deg, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -44,11 +51,11 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.ResetPosition(Pose2d{}, 0_rad);
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
39.986_mps};
odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, 90_deg, speeds);
odometry.ResetPosition(Pose2d{}, 0_rad, zero);
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(0_deg, MecanumDriveWheelPositions{});
auto pose = odometry.Update(90_deg, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -56,14 +63,140 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.ResetPosition(Pose2d{}, 90_deg);
odometry.ResetPosition(Pose2d{}, 90_deg, zero);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
odometry.UpdateWithTime(0_s, 90_deg, MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, 90_deg, speeds);
odometry.Update(90_deg, MecanumDriveWheelPositions{});
auto pose = odometry.Update(90_deg, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 0.02_s;
units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
EXPECT_LT(maxError, 0.125);
}
TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 0.02_s;
units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
EXPECT_LT(maxError, 0.125);
}