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

[wpilib] Add pose estimators (#2867)

Browse Source 
Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes.

Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Declan Freeman-Gleason
2020-11-28 17:35:35 -05:00
committed by GitHub
parent 3413bfc06a
commit bc8f338771
58 changed files with 4958 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -76,6 +76,23 @@ class SwerveDriveKinematics {
wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
}
explicit SwerveDriveKinematics(
const std::array<Translation2d, NumModules>& wheels)
: m_modules{wheels} {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
1, 0, (-m_modules[i].Y()).template to<double>(),
0, 1, (+m_modules[i].X()).template to<double>();
// clang-format on
}
m_forwardKinematics = m_inverseKinematics.householderQr();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
}
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
/**