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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,26 +18,21 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_observer(
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return u; },
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create vision correction mechanism.
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
return x;
},
u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
};
@@ -76,7 +71,7 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
m_visionCorrect(Vectord<3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -102,11 +97,10 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(),
omega.value()};
Vectord<3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(), omega.value()};
Eigen::Vector<double, 1> localY{angle.Radians().value()};
Vectord<1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());