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

[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

Browse Source
This commit is contained in:
Joseph Eng
2023-06-19 17:10:39 -07:00
committed by GitHub
parent 5c2addda0f
commit 25ad5017a9
41 changed files with 1742 additions and 2177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

160
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -7,13 +7,11 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
@@ -32,7 +30,9 @@ namespace frc {
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
: public PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions> {
public:
/**
* Constructs a DifferentialDrivePoseEstimator with default standard
@@ -79,19 +79,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
*
@@ -101,71 +88,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose);
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose.
*/
Pose2d GetEstimatedPosition() const;
/**
* Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
units::meter_t rightDistance, const Pose2d& pose) {
PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::
ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
/**
@@ -179,7 +105,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* @return The estimated pose of the robot.
*/
Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
units::meter_t rightDistance) {
return PoseEstimator<
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::Update(gyroAngle,
{leftDistance, rightDistance});
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
@@ -195,63 +126,16 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance);
units::meter_t rightDistance) {
return PoseEstimator<
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle,
{leftDistance,
rightDistance});
}
private:
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyro angle.
Rotation2d gyroAngle;
// The distance traveled by the left encoder.
units::meter_t leftDistance;
// The distance traveled by the right encoder.
units::meter_t rightDistance;
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics,
InterpolationRecord endValue,
double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
DifferentialDriveKinematics& m_kinematics;
DifferentialDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
DifferentialDriveOdometry m_odometryImpl;
};
} // namespace frc

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

@@ -10,6 +10,7 @@
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,7 +33,9 @@ namespace frc {
* never call it, then this class will behave mostly like regular encoder
* odometry.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
: public PoseEstimator<MecanumDriveWheelSpeeds,
MecanumDriveWheelPositions> {
public:
/**
* Constructs a MecanumDrivePoseEstimator with default standard deviations
@@ -76,174 +79,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances measured at each wheel.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& pose);
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
* Add a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime()
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp().) This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @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 MecanumDriveWheelPositions& wheelPositions);
private:
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyroscope angle.
Rotation2d gyroAngle;
// The distances measured at each wheel.
MecanumDriveWheelPositions wheelPositions;
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics,
InterpolationRecord endValue,
double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
MecanumDriveKinematics& m_kinematics;
MecanumDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
MecanumDriveOdometry m_odometryImpl;
};
} // namespace frc

228
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h Normal file
View File

@@ -0,0 +1,228 @@
// 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 <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/Odometry.h"
#include "frc/kinematics/WheelPositions.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
/**
* This class wraps odometry to fuse latency-compensated
* vision measurements with encoder measurements. Robot code should not use this
* directly- Instead, use the particular type for your drivetrain (e.g.,
* DifferentialDrivePoseEstimator). It will correct for noisy vision
* measurements and encoder drift. It is intended to be an easy drop-in for
* Odometry.
*
* Update() should be called every robot loop.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
*/
template <typename WheelSpeeds, WheelPositions WheelPositions>
class WPILIB_DLLEXPORT PoseEstimator {
public:
/**
* Constructs a PoseEstimator.
*
* @param kinematics A correctly-configured kinematics object for your
* drivetrain.
* @param odometry A correctly-configured odometry object for your drivetrain.
* @param stateStdDevs Standard deviations of the pose estimate (x position in
* meters, y position in meters, and heading in radians). Increase these
* numbers to trust your state estimate less.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances traveled by the encoders.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose);
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
* Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances traveled by the encoders.
*
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances traveled by the encoders.
*
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
private:
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyroscope angle.
Rotation2d gyroAngle;
// The distances traveled by the wheels.
WheelPositions wheelPositions;
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
InterpolationRecord endValue, double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
Odometry<WheelSpeeds, WheelPositions>& m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};
} // namespace frc
#include "frc/estimator/PoseEstimator.inc"

170
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc Normal file
View File

@@ -0,0 +1,170 @@
// 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 "frc/estimator/PoseEstimator.h"
namespace frc {
template <typename WheelSpeeds, WheelPositions WheelPositions>
PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_kinematics(kinematics), m_odometry(odometry) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_poseBuffer.Clear();
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {
return m_odometry.GetPose();
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (!m_poseBuffer.GetInternalBuffer().empty() &&
m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);
if (!sample.has_value()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose
auto twist = sample.value().pose.Log(visionRobotPose);
// Step 3: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Vectord<3> k_times_twist =
m_visionK *
Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
// Step 4: Convert back to Twist2d
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.ResetPosition(sample.value().gyroAngle,
sample.value().wheelPositions,
sample.value().pose.Exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the
// same timestamp
m_poseBuffer.AddSample(timestamp,
{GetEstimatedPosition(), sample.value().gyroAngle,
sample.value().wheelPositions});
// Step 7: Replay odometry inputs between sample time and latest recorded
// sample to update the pose buffer and correct odometry.
auto internal_buf = m_poseBuffer.GetInternalBuffer();
auto upper_bound =
std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
[](const auto& pair, auto t) { return t > pair.first; });
for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
UpdateWithTime(entry->first, entry->second.gyroAngle,
entry->second.wheelPositions);
}
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
m_odometry.Update(gyroAngle, wheelPositions);
// Copy?
WheelPositions internalWheelPositions = wheelPositions;
m_poseBuffer.AddSample(
currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
return GetEstimatedPosition();
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
InterpolationRecord endValue, double i) const {
if (i < 0) {
return *this;
} else if (i > 1) {
return endValue;
} else {
// Find the new wheel distance measurements.
WheelPositions wheels_lerp =
this->wheelPositions.Interpolate(endValue.wheelPositions, i);
// Find the distance between this measurement and the
// interpolated measurement.
WheelPositions wheels_delta = wheels_lerp - this->wheelPositions;
// Find the new gyro angle.
auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
// Create a twist to represent this changed based on the interpolated
// sensor inputs.
auto twist = kinematics.ToTwist2d(wheels_delta);
twist.dtheta = (gyro - gyroAngle).Radians();
return {pose.Exp(twist), gyro, wheels_lerp};
}
}
} // namespace frc

291
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -6,19 +6,16 @@
#include <cmath>
#include <fmt/format.h>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/timestamp.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
#include "frc/kinematics/SwerveDriveWheelPositions.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -34,7 +31,9 @@ namespace frc {
* odometry.
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator {
class SwerveDrivePoseEstimator
: public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -84,14 +83,10 @@ class SwerveDrivePoseEstimator {
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_kinematics{kinematics},
m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
: PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
/**
* Resets the robot's position on the field.
@@ -108,157 +103,11 @@ class SwerveDrivePoseEstimator {
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
m_poseBuffer.Clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
/**
* Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime()
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp().) This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (!m_poseBuffer.GetInternalBuffer().empty() &&
m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);
if (!sample.has_value()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose
auto twist = sample.value().pose.Log(visionRobotPose);
// Step 3: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
frc::Vectord<3> k_times_twist =
m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(),
twist.dtheta.value()};
// Step 4: Convert back to Twist2d
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.ResetPosition(sample.value().gyroAngle,
sample.value().modulePositions,
sample.value().pose.Exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the
// same timestamp
m_poseBuffer.AddSample(timestamp,
{GetEstimatedPosition(), sample.value().gyroAngle,
sample.value().modulePositions});
// Step 7: Replay odometry inputs between sample time and latest recorded
// sample to update the pose buffer and correct odometry.
auto internal_buf = m_poseBuffer.GetInternalBuffer();
auto upper_bound = std::lower_bound(
internal_buf.begin(), internal_buf.end(), timestamp,
[](const auto& pair, auto t) { return t > pair.first; });
for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
UpdateWithTime(entry->first, entry->second.gyroAngle,
entry->second.modulePositions);
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
* To promote stability of the pose estimate and make it robust to bad vision
* data, we recommend only adding vision measurements that are already within
* one meter or so of the current pose estimate.
*
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
PoseEstimator<
SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
{modulePositions},
pose);
}
/**
@@ -273,8 +122,10 @@ class SwerveDrivePoseEstimator {
Pose2d Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
modulePositions);
return PoseEstimator<
SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
{modulePositions});
}
/**
@@ -290,111 +141,13 @@ class SwerveDrivePoseEstimator {
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
m_odometry.Update(gyroAngle, modulePositions);
wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
internalModulePositions[i].distance = modulePositions[i].distance;
internalModulePositions[i].angle = modulePositions[i].angle;
}
m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
internalModulePositions});
return GetEstimatedPosition();
return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::
UpdateWithTime(currentTime, gyroAngle, {modulePositions});
}
private:
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyroscope angle.
Rotation2d gyroAngle;
// The distances traveled and rotations measured at each module.
wpi::array<SwerveModulePosition, NumModules> modulePositions;
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(
SwerveDriveKinematics<NumModules>& kinematics,
InterpolationRecord endValue, double i) const {
if (i < 0) {
return *this;
} else if (i > 1) {
return endValue;
} else {
// Find the new module distances.
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wpi::empty_array};
// Find the distance between this measurement and the
// interpolated measurement.
wpi::array<SwerveModulePosition, NumModules> modulesDelta{
wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
modulePositions[i].distance =
wpi::Lerp(this->modulePositions[i].distance,
endValue.modulePositions[i].distance, i);
modulePositions[i].angle =
wpi::Lerp(this->modulePositions[i].angle,
endValue.modulePositions[i].angle, i);
modulesDelta[i].distance =
modulePositions[i].distance - this->modulePositions[i].distance;
modulesDelta[i].angle = modulePositions[i].angle;
}
// Find the new gyro angle.
auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
// Create a twist to represent this changed based on the interpolated
// sensor inputs.
auto twist = kinematics.ToTwist2d(modulesDelta);
twist.dtheta = (gyro - gyroAngle).Radians();
return {pose.Exp(twist), gyro, modulePositions};
}
}
};
static constexpr units::second_t kBufferDuration = 1.5_s;
SwerveDriveKinematics<NumModules>& m_kinematics;
SwerveDriveOdometry<NumModules> m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
SwerveDriveOdometry<NumModules> m_odometryImpl;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

15
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
View File

@@ -8,7 +8,9 @@
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelPositions.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "frc/kinematics/Kinematics.h"
#include "units/angle.h"
#include "units/length.h"
#include "wpimath/MathShared.h"
@@ -22,7 +24,9 @@ namespace frc {
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
*/
class WPILIB_DLLEXPORT DifferentialDriveKinematics {
class WPILIB_DLLEXPORT DifferentialDriveKinematics
: public Kinematics<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions> {
public:
/**
* Constructs a differential drive kinematics object.
@@ -46,7 +50,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
* @return The chassis speed.
*/
constexpr ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
}
@@ -60,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
* @return The left and right velocities.
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const {
const ChassisSpeeds& chassisSpeeds) const override {
return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
@@ -79,6 +83,11 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
(rightDistance - leftDistance) / trackWidth * 1_rad};
}
Twist2d ToTwist2d(
const DifferentialDriveWheelPositions& wheelDeltas) const override {
return ToTwist2d(wheelDeltas.left, wheelDeltas.right);
}
units::meter_t trackWidth;
};
} // namespace frc

40
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -7,6 +7,10 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveWheelPositions.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "frc/kinematics/Odometry.h"
#include "units/length.h"
namespace frc {
@@ -22,7 +26,9 @@ namespace frc {
* It is important that you reset your encoders to zero before using this class.
* Any subsequent pose resets also require the encoders to be reset to zero.
*/
class WPILIB_DLLEXPORT DifferentialDriveOdometry {
class WPILIB_DLLEXPORT DifferentialDriveOdometry
: public Odometry<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions> {
public:
/**
* Constructs a DifferentialDriveOdometry object.
@@ -56,20 +62,13 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_prevLeftDistance = leftDistance;
m_prevRightDistance = rightDistance;
Odometry<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
{leftDistance,
rightDistance},
pose);
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot position on the field using distance measurements from
* encoders. This method is more numerically accurate than using velocities to
@@ -82,15 +81,14 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
units::meter_t rightDistance) {
return Odometry<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::Update(gyroAngle,
{leftDistance,
rightDistance});
}
private:
Pose2d m_pose;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
units::meter_t m_prevLeftDistance = 0_m;
units::meter_t m_prevRightDistance = 0_m;
DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
};
} // namespace frc

56
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h Normal file
View File

@@ -0,0 +1,56 @@
// 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/MathExtras.h>
#include <wpi/SymbolExports.h>
#include "units/length.h"
namespace frc {
/**
* Represents the wheel positions for a differential drive drivetrain.
*/
struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
/**
* Distance driven by the left side.
*/
units::meter_t left = 0_m;
/**
* Distance driven by the right side.
*/
units::meter_t right = 0_m;
/**
* Checks equality between this DifferentialDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const DifferentialDriveWheelPositions& other) const = default;
/**
* Checks inequality between this DifferentialDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
DifferentialDriveWheelPositions operator-(
const DifferentialDriveWheelPositions& other) const {
return {left - other.left, right - other.right};
}
DifferentialDriveWheelPositions Interpolate(
const DifferentialDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(left, endValue.left, t),
wpi::Lerp(right, endValue.right, t)};
}
};
} // namespace frc

60
wpimath/src/main/native/include/frc/kinematics/Kinematics.h Normal file
View File

@@ -0,0 +1,60 @@
// 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 "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds. Robot code should not use this directly-
* Instead, use the particular type for your drivetrain (e.g.,
* DifferentialDriveKinematics).
*
* Inverse kinematics converts a desired chassis speed into wheel speeds whereas
* forward kinematics converts wheel speeds into chassis speed.
*/
template <typename WheelSpeeds, typename WheelPositions>
class WPILIB_DLLEXPORT Kinematics {
public:
/**
* Performs forward kinematics to return the resulting chassis speed from the
* wheel speeds. This method is often used for odometry -- determining the
* robot's position on the field using data from the real-world speed of each
* wheel on the robot.
*
* @param wheelSpeeds The speeds of the wheels.
* @return The chassis speed.
*/
virtual ChassisSpeeds ToChassisSpeeds(
const WheelSpeeds& wheelSpeeds) const = 0;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
*/
virtual WheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const = 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 in the robot's movement.
*/
virtual Twist2d ToTwist2d(const WheelPositions& wheelDeltas) const = 0;
};
} // namespace frc

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

@@ -11,6 +11,7 @@
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "wpimath/MathShared.h"
@@ -39,7 +40,8 @@ namespace frc {
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
class WPILIB_DLLEXPORT MecanumDriveKinematics {
class WPILIB_DLLEXPORT MecanumDriveKinematics
: public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
public:
/**
* Constructs a mecanum drive kinematics object.
@@ -101,7 +103,12 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d{}) const;
const Translation2d& centerOfRotation) const;
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return ToWheelSpeeds(chassisSpeeds, {});
}
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -114,7 +121,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
/**
* Performs forward kinematics to return the resulting Twist2d from the
@@ -126,7 +133,8 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
*
* @return The resulting chassis speed.
*/
Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
Twist2d ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const override;
private:
mutable Matrixd<4, 3> m_inverseKinematics;

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

@@ -9,7 +9,9 @@
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "frc/kinematics/Odometry.h"
#include "units/time.h"
namespace frc {
@@ -23,7 +25,8 @@ namespace frc {
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
class WPILIB_DLLEXPORT MecanumDriveOdometry {
class WPILIB_DLLEXPORT MecanumDriveOdometry
: public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
public:
/**
* Constructs a MecanumDriveOdometry object.
@@ -38,52 +41,8 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
const MecanumDriveWheelPositions& wheelPositions,
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 gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_previousWheelPositions = wheelPositions;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
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 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 wheelPositions The current distances measured by each wheel.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions);
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
MecanumDriveWheelPositions m_previousWheelPositions;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
MecanumDriveKinematics m_kinematicsImpl;
};
} // namespace frc

17
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
View File

@@ -4,13 +4,14 @@
#pragma once
#include <wpi/MathExtras.h>
#include <wpi/SymbolExports.h>
#include "units/length.h"
namespace frc {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
* Represents the wheel positions for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
/**
@@ -49,5 +50,19 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
* @return Whether the two objects are not equal.
*/
bool operator!=(const MecanumDriveWheelPositions& other) const = default;
MecanumDriveWheelPositions operator-(
const MecanumDriveWheelPositions& other) const {
return {frontLeft - other.frontLeft, frontRight - other.frontRight,
rearLeft - other.rearLeft, rearRight - other.rearRight};
}
MecanumDriveWheelPositions Interpolate(
const MecanumDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(frontLeft, endValue.frontLeft, t),
wpi::Lerp(frontRight, endValue.frontRight, t),
wpi::Lerp(rearLeft, endValue.rearLeft, t),
wpi::Lerp(rearRight, endValue.rearRight, t)};
}
};
} // namespace frc

88
wpimath/src/main/native/include/frc/kinematics/Odometry.h Normal file
View File

@@ -0,0 +1,88 @@
// 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 "frc/geometry/Pose2d.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/WheelPositions.h"
namespace frc {
/**
* Class for odometry. Robot code should not use this directly- Instead, use the
* particular type for your drivetrain (e.g., DifferentialDriveOdometry).
* Odometry allows you to track the robot's position on the field over a course
* of a match using readings from your wheel encoders.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
template <typename WheelSpeeds, WheelPositions WheelPositions>
class WPILIB_DLLEXPORT Odometry {
public:
/**
* Constructs an Odometry object.
*
* @param kinematics The 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 Odometry(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions,
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 gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_previousWheelPositions = wheelPositions;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
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 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 wheelPositions The current distances measured by each wheel.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
private:
const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
Pose2d m_pose;
WheelPositions m_previousWheelPositions;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
} // namespace frc
#include "Odometry.inc"

40
wpimath/src/main/native/include/frc/kinematics/Odometry.inc Normal file
View File

@@ -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.
#pragma once
#include "frc/kinematics/Odometry.h"
namespace frc {
template <typename WheelSpeeds, WheelPositions WheelPositions>
Odometry<WheelSpeeds, WheelPositions>::Odometry(
const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto wheelDeltas = wheelPositions - m_previousWheelPositions;
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;
}
} // namespace frc

24
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -16,12 +16,18 @@
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/SwerveDriveWheelPositions.h"
#include "frc/kinematics/SwerveModulePosition.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "units/velocity.h"
#include "wpimath/MathShared.h"
namespace frc {
template <size_t NumModules>
using SwerveDriveWheelSpeeds = wpi::array<SwerveModuleState, NumModules>;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
@@ -45,7 +51,9 @@ namespace frc {
* the robot on the field using encoders and a gyro.
*/
template <size_t NumModules>
class SwerveDriveKinematics {
class SwerveDriveKinematics
: public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a swerve drive kinematics object. This takes in a variable
@@ -130,6 +138,11 @@ class SwerveDriveKinematics {
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d{}) const;
SwerveDriveWheelSpeeds<NumModules> ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return ToSwerveModuleStates(chassisSpeeds);
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
@@ -162,8 +175,8 @@ class SwerveDriveKinematics {
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
const wpi::array<SwerveModuleState, NumModules>& moduleStates) const;
ChassisSpeeds ToChassisSpeeds(const wpi::array<SwerveModuleState, NumModules>&
moduleStates) const override;
/**
* Performs forward kinematics to return the resulting Twist2d from the
@@ -201,6 +214,11 @@ class SwerveDriveKinematics {
Twist2d ToTwist2d(
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
Twist2d ToTwist2d(
const SwerveDriveWheelPositions<NumModules>& wheelDeltas) const override {
return ToTwist2d(wheelDeltas.positions);
}
/**
* Renormalizes the wheel speeds if any individual speed is above the
* specified maximum.

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

@@ -11,8 +11,11 @@
#include <wpi/SymbolExports.h>
#include <wpi/timestamp.h>
#include "Odometry.h"
#include "SwerveDriveKinematics.h"
#include "SwerveDriveWheelPositions.h"
#include "SwerveModulePosition.h"
#include "SwerveModuleState.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
@@ -28,7 +31,9 @@ namespace frc {
* when using computer-vision systems.
*/
template <size_t NumModules>
class SwerveDriveOdometry {
class SwerveDriveOdometry
: public Odometry<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a SwerveDriveOdometry object.
@@ -56,13 +61,13 @@ class SwerveDriveOdometry {
void ResetPosition(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose);
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
const Pose2d& pose) {
Odometry<
SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
{modulePositions},
pose);
}
/**
* Updates the robot's position on the field using forward kinematics and
@@ -79,17 +84,15 @@ class SwerveDriveOdometry {
*/
const Pose2d& Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
return Odometry<
SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
{modulePositions});
}
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
wpi::empty_array};
SwerveDriveKinematics<NumModules> m_kinematicsImpl;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

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

@@ -13,58 +13,11 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
for (size_t i = 0; i < NumModules; i++) {
m_previousModulePositions[i] = {modulePositions[i].distance,
modulePositions[i].angle};
}
: Odometry<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>(
m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
template <size_t NumModules>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
for (size_t i = 0; i < NumModules; i++) {
m_previousModulePositions[i].distance = modulePositions[i].distance;
}
}
template <size_t NumModules>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
auto moduleDeltas =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t index = 0; index < NumModules; index++) {
auto lastPosition = m_previousModulePositions[index];
auto currentPosition = modulePositions[index];
moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
currentPosition.angle};
m_previousModulePositions[index].distance = modulePositions[index].distance;
}
auto angle = gyroAngle + m_gyroOffset;
auto twist = m_kinematics.ToTwist2d(moduleDeltas);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}
} // namespace frc

61
wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h Normal file
View File

@@ -0,0 +1,61 @@
// 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/MathExtras.h>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/kinematics/SwerveModulePosition.h"
namespace frc {
/**
* Represents the wheel positions for a swerve drive drivetrain.
*/
template <size_t NumModules>
struct WPILIB_DLLEXPORT SwerveDriveWheelPositions {
/**
* The distances driven by the wheels.
*/
wpi::array<SwerveModulePosition, NumModules> positions;
/**
* Checks equality between this SwerveDriveWheelPositions and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveDriveWheelPositions& other) const = default;
/**
* Checks inequality between this SwerveDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const SwerveDriveWheelPositions& other) const = default;
SwerveDriveWheelPositions<NumModules> operator-(
const SwerveDriveWheelPositions<NumModules>& other) const {
auto result =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t i = 0; i < NumModules; i++) {
result[i] = positions[i] - other.positions[i];
}
return {result};
}
SwerveDriveWheelPositions<NumModules> Interpolate(
const SwerveDriveWheelPositions<NumModules>& endValue, double t) const {
auto result =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t i = 0; i < NumModules; i++) {
result[i] = positions[i].Interpolate(endValue.positions[i], t);
}
return {result};
}
};
} // namespace frc

11
wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <wpi/MathExtras.h>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
@@ -33,5 +34,15 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModulePosition& other) const;
SwerveModulePosition operator-(const SwerveModulePosition& other) const {
return {distance - other.distance, angle};
}
SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
double t) const {
return {wpi::Lerp(distance, endValue.distance, t),
wpi::Lerp(angle, endValue.angle, t)};
}
};
} // namespace frc

16
wpimath/src/main/native/include/frc/kinematics/WheelPositions.h Normal file
View File

@@ -0,0 +1,16 @@
// 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 <concepts>
namespace frc {
template <typename T>
concept WheelPositions =
std::copy_constructible<T> && requires(T a, T b, double t) {
{ a - b } -> std::convertible_to<T>;
{ a.Interpolate(b, t) } -> std::convertible_to<T>;
};
} // namespace frc