Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +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

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);
}