mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-27 02:01:42 +00:00
[wpimath] Add 3D odometry and pose estimation (#7119)
This commit is contained in:
Joseph Eng
@@ -0,0 +1,40 @@
|
||||
// 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.
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/kinematics/DifferentialDriveOdometry3d.h"
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(DifferentialDriveOdometry3dTest, Initialize) {
|
||||
DifferentialDriveOdometry3d odometry{
|
||||
frc::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
|
||||
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
|
||||
|
||||
const frc::Pose3d& pose = odometry.GetPose();
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
|
||||
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
|
||||
EXPECT_NEAR(pose.Z().value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
|
||||
DifferentialDriveOdometry3d odometry{frc::Rotation3d{0_deg, 0_deg, 45_deg},
|
||||
0_m, 0_m};
|
||||
|
||||
const auto& pose = odometry.Update(frc::Rotation3d{0_deg, 0_deg, 135_deg},
|
||||
0_m, units::meter_t{5 * std::numbers::pi});
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
|
||||
}
|
||||
@@ -0,0 +1,223 @@
|
||||
// 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.
|
||||
|
||||
#include <limits>
|
||||
#include <random>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "frc/kinematics/MecanumDriveOdometry3d.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
class MecanumDriveOdometry3dTest : public ::testing::Test {
|
||||
protected:
|
||||
Translation2d m_fl{12_m, 12_m};
|
||||
Translation2d m_fr{12_m, -12_m};
|
||||
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};
|
||||
MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, zero};
|
||||
};
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, Initialize) {
|
||||
MecanumDriveOdometry3d odometry{
|
||||
kinematics, frc::Rotation3d{}, zero,
|
||||
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
|
||||
|
||||
const frc::Pose3d& pose = odometry.GetPose();
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 1, 1e-9);
|
||||
EXPECT_NEAR(pose.Y().value(), 2, 1e-9);
|
||||
EXPECT_NEAR(pose.Z().value(), 0, 1e-9);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, 1e-9);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
|
||||
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
|
||||
|
||||
odometry.ResetPosition(frc::Rotation3d{}, wheelDeltas, Pose3d{});
|
||||
|
||||
odometry.Update(frc::Rotation3d{}, wheelDeltas);
|
||||
auto secondPose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
|
||||
|
||||
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(secondPose.Z().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(secondPose.Rotation().ToRotation2d().Radians().value(), 0.0,
|
||||
0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, TwoIterations) {
|
||||
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
|
||||
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
|
||||
0.3536_m};
|
||||
|
||||
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
|
||||
auto pose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
|
||||
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(pose.Z().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
|
||||
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
|
||||
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
|
||||
39.986_m};
|
||||
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
|
||||
auto pose =
|
||||
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
|
||||
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
|
||||
EXPECT_NEAR(pose.Z().value(), 0, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
|
||||
odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
|
||||
|
||||
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
|
||||
0.3536_m};
|
||||
|
||||
auto pose =
|
||||
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
|
||||
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(pose.Z().value(), 0.0, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, 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::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
|
||||
wheelPositions};
|
||||
|
||||
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 = 20_ms;
|
||||
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(
|
||||
frc::Rotation3d{groundTruthState.pose.Rotation() +
|
||||
frc::Rotation2d{distribution(generator) * 0.05_rad}},
|
||||
wheelPositions);
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation().ToTranslation2d())
|
||||
.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(MecanumDriveOdometry3dTest, 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::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
|
||||
wheelPositions};
|
||||
|
||||
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 = 20_ms;
|
||||
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::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
|
||||
wheelPositions);
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation().ToTranslation2d())
|
||||
.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);
|
||||
}
|
||||
@@ -85,7 +85,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
|
||||
frc::MecanumDriveWheelPositions wheelPositions;
|
||||
|
||||
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
|
||||
wheelPositions, frc::Pose2d{}};
|
||||
wheelPositions};
|
||||
|
||||
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
|
||||
@@ -148,7 +148,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
|
||||
frc::MecanumDriveWheelPositions wheelPositions;
|
||||
|
||||
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
|
||||
wheelPositions, frc::Pose2d{}};
|
||||
wheelPositions};
|
||||
|
||||
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
|
||||
|
||||
@@ -0,0 +1,224 @@
|
||||
// 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.
|
||||
|
||||
#include <limits>
|
||||
#include <random>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "frc/kinematics/SwerveDriveKinematics.h"
|
||||
#include "frc/kinematics/SwerveDriveOdometry3d.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "frc/trajectory/TrajectoryConfig.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 0.01;
|
||||
|
||||
class SwerveDriveOdometry3dTest : public ::testing::Test {
|
||||
protected:
|
||||
Translation2d m_fl{12_m, 12_m};
|
||||
Translation2d m_fr{12_m, -12_m};
|
||||
Translation2d m_bl{-12_m, 12_m};
|
||||
Translation2d m_br{-12_m, -12_m};
|
||||
|
||||
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
|
||||
SwerveModulePosition zero;
|
||||
SwerveDriveOdometry3d<4> m_odometry{
|
||||
m_kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
|
||||
};
|
||||
|
||||
TEST_F(SwerveDriveOdometry3dTest, Initialize) {
|
||||
SwerveDriveOdometry3d odometry{
|
||||
m_kinematics,
|
||||
frc::Rotation3d{},
|
||||
{zero, zero, zero, zero},
|
||||
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
|
||||
|
||||
const frc::Pose3d& pose = odometry.GetPose();
|
||||
|
||||
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
|
||||
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
|
||||
EXPECT_NEAR(pose.Z().value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveOdometry3dTest, TwoIterations) {
|
||||
SwerveModulePosition position{0.5_m, 0_deg};
|
||||
|
||||
m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero},
|
||||
Pose3d{});
|
||||
|
||||
m_odometry.Update(frc::Rotation3d{}, {zero, zero, zero, zero});
|
||||
|
||||
auto pose = m_odometry.Update(frc::Rotation3d{},
|
||||
{position, position, position, position});
|
||||
|
||||
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) {
|
||||
SwerveModulePosition fl{18.85_m, 90_deg};
|
||||
SwerveModulePosition fr{42.15_m, 26.565_deg};
|
||||
SwerveModulePosition bl{18.85_m, -90_deg};
|
||||
SwerveModulePosition br{42.15_m, -26.565_deg};
|
||||
|
||||
m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero},
|
||||
Pose3d{});
|
||||
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg},
|
||||
{fl, fr, bl, br});
|
||||
|
||||
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
|
||||
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon);
|
||||
EXPECT_NEAR(90.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveOdometry3dTest, GyroAngleReset) {
|
||||
m_odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg},
|
||||
{zero, zero, zero, zero}, Pose3d{});
|
||||
|
||||
SwerveModulePosition position{0.5_m, 0_deg};
|
||||
|
||||
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg},
|
||||
{position, position, position, position});
|
||||
|
||||
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
|
||||
SwerveDriveKinematics<4> kinematics{
|
||||
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
|
||||
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
|
||||
|
||||
SwerveDriveOdometry3d<4> odometry{
|
||||
kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
|
||||
|
||||
SwerveModulePosition fl;
|
||||
SwerveModulePosition fr;
|
||||
SwerveModulePosition bl;
|
||||
SwerveModulePosition br;
|
||||
|
||||
Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
|
||||
Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
|
||||
Pose2d{0_m, 0_m, 45_deg}},
|
||||
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 = 20_ms;
|
||||
units::second_t t = 0_s;
|
||||
|
||||
double maxError = -std::numeric_limits<double>::max();
|
||||
double errorSum = 0;
|
||||
|
||||
while (t < trajectory.TotalTime()) {
|
||||
Trajectory::State groundTruthState = trajectory.Sample(t);
|
||||
|
||||
auto moduleStates = kinematics.ToSwerveModuleStates(
|
||||
{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 = odometry.Update(
|
||||
frc::Rotation3d{groundTruthState.pose.Rotation() +
|
||||
frc::Rotation2d{distribution(generator) * 0.05_rad}},
|
||||
{fl, fr, bl, br});
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation().ToTranslation2d())
|
||||
.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_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
|
||||
SwerveDriveKinematics<4> kinematics{
|
||||
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
|
||||
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
|
||||
|
||||
SwerveDriveOdometry3d<4> odometry{
|
||||
kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
|
||||
|
||||
SwerveModulePosition fl;
|
||||
SwerveModulePosition fr;
|
||||
SwerveModulePosition bl;
|
||||
SwerveModulePosition br;
|
||||
|
||||
Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
|
||||
Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
|
||||
Pose2d{0_m, 0_m, 45_deg}},
|
||||
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 = 20_ms;
|
||||
units::second_t t = 0_s;
|
||||
|
||||
double maxError = -std::numeric_limits<double>::max();
|
||||
double errorSum = 0;
|
||||
|
||||
while (t < trajectory.TotalTime()) {
|
||||
Trajectory::State groundTruthState = trajectory.Sample(t);
|
||||
|
||||
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 = odometry.Update(
|
||||
frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
|
||||
{fl, fr, bl, br});
|
||||
double error = groundTruthState.pose.Translation()
|
||||
.Distance(xhat.Translation().ToTranslation2d())
|
||||
.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);
|
||||
}
|
||||
@@ -77,7 +77,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
|
||||
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
|
||||
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
|
||||
|
||||
SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
|
||||
SwerveDriveOdometry<4> odometry{
|
||||
kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}};
|
||||
|
||||
SwerveModulePosition fl;
|
||||
SwerveModulePosition fr;
|
||||
@@ -141,7 +142,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
|
||||
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
|
||||
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
|
||||
|
||||
SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
|
||||
SwerveDriveOdometry<4> odometry{
|
||||
kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}};
|
||||
|
||||
SwerveModulePosition fl;
|
||||
SwerveModulePosition fr;
|
||||
|
||||
Reference in New Issue
Block a user