Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

162
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -4,41 +4,8 @@
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
#include <wpi/timestamp.h>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "wpimath/MathShared.h"
using namespace frc;
DifferentialDrivePoseEstimator::InterpolationRecord
DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
DifferentialDriveKinematics& kinematics, InterpolationRecord endValue,
double i) const {
if (i < 0) {
return *this;
} else if (i > 1) {
return endValue;
} else {
// Find the interpolated left distance.
auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i);
// Find the interpolated right distance.
auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i);
// 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(left - leftDistance, right - rightDistance);
twist.dtheta = (gyro - gyroAngle).Radians();
return {pose.Exp(twist), gyro, left, right};
}
}
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
@@ -52,128 +19,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_kinematics{kinematics},
m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
void DifferentialDrivePoseEstimator::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]));
}
}
}
void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
m_poseBuffer.Clear();
}
Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
return m_odometry.GetPose();
}
void DifferentialDrivePoseEstimator::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().leftDistance,
sample.value().rightDistance, 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().leftDistance, sample.value().rightDistance});
// 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 first_newer_record =
std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
[](const auto& pair, auto t) { return t > pair.first; });
for (auto entry = first_newer_record + 1; entry != internal_buf.end();
entry++) {
UpdateWithTime(entry->first, entry->second.gyroAngle,
entry->second.leftDistance, entry->second.rightDistance);
}
}
Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
leftDistance, rightDistance);
}
Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance) {
m_odometry.Update(gyroAngle, leftDistance, rightDistance);
// fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
// gyroAngle.Radians(),
// leftDistance,
// rightDistance,
// GetEstimatedPosition().X(),
// GetEstimatedPosition().Y(),
// GetEstimatedPosition().Rotation().Radians()
// );
m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
leftDistance, rightDistance});
return GetEstimatedPosition();
}
: PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}

161
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -12,46 +12,6 @@
using namespace frc;
frc::MecanumDrivePoseEstimator::InterpolationRecord
frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
MecanumDriveKinematics& kinematics, InterpolationRecord endValue,
double i) const {
if (i < 0) {
return *this;
} else if (i > 1) {
return endValue;
} else {
// Find the new wheel distance measurements.
MecanumDriveWheelPositions wheels_lerp{
wpi::Lerp(this->wheelPositions.frontLeft,
endValue.wheelPositions.frontLeft, i),
wpi::Lerp(this->wheelPositions.frontRight,
endValue.wheelPositions.frontRight, i),
wpi::Lerp(this->wheelPositions.rearLeft,
endValue.wheelPositions.rearLeft, i),
wpi::Lerp(this->wheelPositions.rearRight,
endValue.wheelPositions.rearRight, i)};
// Find the distance between this measurement and the
// interpolated measurement.
MecanumDriveWheelPositions wheels_delta{
wheels_lerp.frontLeft - this->wheelPositions.frontLeft,
wheels_lerp.frontRight - this->wheelPositions.frontRight,
wheels_lerp.rearLeft - this->wheelPositions.rearLeft,
wheels_lerp.rearRight - this->wheelPositions.rearRight};
// 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};
}
}
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
@@ -64,121 +24,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_kinematics{kinematics},
m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
void frc::MecanumDrivePoseEstimator::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]));
}
}
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_poseBuffer.Clear();
}
Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
return m_odometry.GetPose();
}
void frc::MecanumDrivePoseEstimator::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().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);
}
}
Pose2d frc::MecanumDrivePoseEstimator::Update(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
m_odometry.Update(gyroAngle, wheelPositions);
MecanumDriveWheelPositions internalWheelPositions{
wheelPositions.frontLeft, wheelPositions.frontRight,
wheelPositions.rearLeft, wheelPositions.rearRight};
m_poseBuffer.AddSample(
currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
return GetEstimatedPosition();
}
: PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}

29
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -11,32 +11,9 @@ using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose)
: m_pose(initialPose),
m_prevLeftDistance(leftDistance),
m_prevRightDistance(rightDistance) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
: Odometry<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>(
m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}
const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {
auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
auto deltaRightDistance = rightDistance - m_prevRightDistance;
m_prevLeftDistance = leftDistance;
m_prevRightDistance = rightDistance;
auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
auto angle = gyroAngle + m_gyroOffset;
auto newPose = m_pose.Exp(
{averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}

32
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -11,35 +11,9 @@ using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
: Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}
const Pose2d& MecanumDriveOdometry::Update(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
MecanumDriveWheelPositions wheelDeltas{
wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
wheelPositions.frontRight - m_previousWheelPositions.frontRight,
wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
wheelPositions.rearRight - m_previousWheelPositions.rearRight,
};
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;
}