Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rework function signatures for Pose Estimation / Odometry (#4642)

Browse Source 
Forcing the use of SwerveModulePostition[] and wpi::array<SwerveModulePosition, len> in place of variadic function signatures.
This commit is contained in:
Jordan McMichael
2022-11-17 13:36:33 -05:00
committed by GitHub
parent 0401597d3b
commit ce4c45df13
9 changed files with 84 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -252,7 +252,7 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
* @param modulePositions The current distance measurements and rotations of the swerve modules.
*/
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, SwerveModulePosition... modulePositions) {
Pose2d poseMeters, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -65,7 +65,7 @@ public class SwerveDriveOdometry {
public SwerveDriveOdometry(
SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition... modulePositions) {
SwerveModulePosition[] modulePositions) {
this(kinematics, gyroAngle, modulePositions, new Pose2d());
}
@@ -82,7 +82,7 @@ public class SwerveDriveOdometry {
* @param modulePositions The wheel positions reported by each module.
*/
public void resetPosition(
Pose2d pose, Rotation2d gyroAngle, SwerveModulePosition... modulePositions) {
Pose2d pose, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
if (modulePositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
@@ -120,7 +120,7 @@ public class SwerveDriveOdometry {
* in the same order in which you instantiated your SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition... modulePositions) {
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
if (modulePositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "

33
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -43,20 +43,6 @@ class SwerveDriveOdometry {
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The wheel positions reported by each module.
*/
template <typename... ModulePositions>
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* Resets the robot's position on the field.
*
@@ -77,25 +63,6 @@ class SwerveDriveOdometry {
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* 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 gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current position of all swerve modules. Please
* provide the positions in the same order in which you instantiated your
* SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModulePositions>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* 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

28
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View File

@@ -22,20 +22,6 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
template <size_t NumModules>
template <typename... ModulePositions>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions) {
static_assert(sizeof...(wheelPositions) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wheelPositions...};
this->ResetPosition(pose, gyroAngle, modulePositions);
}
template <size_t NumModules>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
@@ -46,20 +32,6 @@ void SwerveDriveOdometry<NumModules>::ResetPosition(
m_previousModulePositions = modulePositions;
}
template <size_t NumModules>
template <typename... ModulePositions>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle, ModulePositions&&... wheelPositions) {
static_assert(sizeof...(wheelPositions) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wheelPositions...};
return this->Update(gyroAngle, modulePositions);
}
template <size_t NumModules>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle,

41
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
View File

@@ -28,7 +28,8 @@ class SwerveDriveOdometryTest {
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(m_kinematics, new Rotation2d(), zero, zero, zero, zero);
new SwerveDriveOdometry(
m_kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
@Test
void testTwoIterations() {
@@ -42,10 +43,12 @@ class SwerveDriveOdometryTest {
m_odometry.update(
new Rotation2d(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition());
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
var pose = m_odometry.update(new Rotation2d(), wheelDeltas);
assertAll(
@@ -70,7 +73,7 @@ class SwerveDriveOdometryTest {
};
final var zero = new SwerveModulePosition();
m_odometry.update(new Rotation2d(), zero, zero, zero, zero);
m_odometry.update(new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelDeltas);
assertAll(
@@ -84,11 +87,13 @@ class SwerveDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(
new Pose2d(new Translation2d(), fieldAngle), gyro, zero, zero, zero, zero);
new Pose2d(new Translation2d(), fieldAngle),
gyro,
new SwerveModulePosition[] {zero, zero, zero, zero});
var delta = new SwerveModulePosition();
m_odometry.update(gyro, delta, delta, delta, delta);
m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
var pose = m_odometry.update(gyro, delta, delta, delta, delta);
var pose = m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
assertAll(
() -> assertEquals(1.0, pose.getX(), 0.1),
@@ -104,7 +109,9 @@ class SwerveDriveOdometryTest {
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1));
var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero);
var odometry =
new SwerveDriveOdometry(
kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
SwerveModulePosition fl = new SwerveModulePosition();
SwerveModulePosition fr = new SwerveModulePosition();
@@ -159,10 +166,7 @@ class SwerveDriveOdometryTest {
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
fl,
fr,
bl,
br);
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -187,7 +191,9 @@ class SwerveDriveOdometryTest {
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1));
var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero);
var odometry =
new SwerveDriveOdometry(
kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
SwerveModulePosition fl = new SwerveModulePosition();
SwerveModulePosition fr = new SwerveModulePosition();
@@ -232,7 +238,10 @@ class SwerveDriveOdometryTest {
bl.angle = groundTruthState.poseMeters.getRotation();
br.angle = groundTruthState.poseMeters.getRotation();
var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), fl, fr, bl, br);
var xHat =
odometry.update(
new Rotation2d(rand.nextGaussian() * 0.05),
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());

20
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -32,11 +32,12 @@ class SwerveDriveOdometryTest : public ::testing::Test {
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModulePosition position{0.5_m, 0_deg};
m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
m_odometry.Update(0_deg, zero, zero, zero, zero);
m_odometry.Update(0_deg, {zero, zero, zero, zero});
auto pose = m_odometry.Update(0_deg, position, position, position, position);
auto pose =
m_odometry.Update(0_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -49,8 +50,8 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
SwerveModulePosition bl{18.85_m, -90_deg};
SwerveModulePosition br{42.15_m, -26.565_deg};
m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
auto pose = m_odometry.Update(90_deg, fl, fr, bl, br);
m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -58,11 +59,12 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d{}, 90_deg, zero, zero, zero, zero);
m_odometry.ResetPosition(Pose2d{}, 90_deg, {zero, zero, zero, zero});
SwerveModulePosition position{0.5_m, 0_deg};
auto pose = m_odometry.Update(90_deg, position, position, position, position);
auto pose =
m_odometry.Update(90_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -116,7 +118,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
fl, fr, bl, br);
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -178,7 +180,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
br.angle = groundTruthState.pose.Rotation();
auto xhat = odometry.Update(
frc::Rotation2d{distribution(generator) * 0.05_rad}, fl, fr, bl, br);
frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();