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

[wpimath] Position Delta Odometry for Swerve (#4493)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:36 -04:00
committed by GitHub
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -8,20 +8,27 @@
#include "frc/estimator/SwerveDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::SwerveDriveKinematics<4> 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::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{}, frc::Pose2d{}, kinematics,
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d{}};
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
frc::Pose2d{},
{fl, fr, bl, br},
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},
@@ -54,10 +61,9 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
}
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}};
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.1_rad}};
visionPoses.push_back(lastVisionPose);
lastVisionUpdateTime = t;
}
@@ -66,11 +72,116 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
bl.angle = moduleStates[2].angle;
br.angle = moduleStates[3].angle;
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
moduleStates, {fl, fr, bl, br});
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(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::SwerveDriveKinematics<4> 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::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
frc::Pose2d{},
{fl, fr, bl, br},
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_rad}};
visionPoses.push_back(lastVisionPose);
lastVisionUpdateTime = t;
}
auto moduleStates = kinematics.ToSwerveModuleStates(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
fl.distance += groundTruthState.velocity * dt +
0.5 * groundTruthState.acceleration * dt * dt;
fr.distance += groundTruthState.velocity * dt +
0.5 * groundTruthState.acceleration * dt * dt;
bl.distance += groundTruthState.velocity * dt +
0.5 * groundTruthState.acceleration * dt * dt;
br.distance += groundTruthState.velocity * dt +
0.5 * groundTruthState.acceleration * dt * dt;
fl.angle = groundTruthState.pose.Rotation();
fr.angle = groundTruthState.pose.Rotation();
bl.angle = groundTruthState.pose.Rotation();
br.angle = groundTruthState.pose.Rotation();
auto xhat = estimator.UpdateWithTime(
t, frc::Rotation2d{distribution(generator) * 0.05_rad}, moduleStates,
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();