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

[wpimath] SwerveDriveKinematics: Add reset method (#5398)

Browse Source 
Adds a reset method where teams can pass in module headings for the kinematics object to use if it gets an all-zero ChassisSpeeds while converting ChassisSpeeds to module states. Also removes internal states array, replacing it with an internal headings array.
This commit is contained in:
Gold856
2023-06-20 01:57:55 -04:00
committed by GitHub
parent 316cd2a453
commit 50b90ceb54
5 changed files with 103 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -69,7 +69,7 @@ class SwerveDriveKinematics
template <std::convertible_to<Translation2d>... ModuleTranslations>
requires(sizeof...(ModuleTranslations) == NumModules)
explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
: m_modules{moduleTranslations...}, m_moduleStates(wpi::empty_array) {
: m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -86,7 +86,7 @@ class SwerveDriveKinematics
explicit SwerveDriveKinematics(
const wpi::array<Translation2d, NumModules>& modules)
: m_modules{modules}, m_moduleStates(wpi::empty_array) {
: m_modules{modules}, m_moduleHeadings(wpi::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -103,6 +103,25 @@ class SwerveDriveKinematics
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
/**
* Reset the internal swerve module headings.
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
*/
template <std::convertible_to<Rotation2d>... ModuleHeadings>
requires(sizeof...(ModuleHeadings) == NumModules)
void ResetHeadings(ModuleHeadings&&... moduleHeadings) {
return this->ResetHeadings(
wpi::array<Rotation2d, NumModules>{moduleHeadings...});
}
/**
* Reset the internal swerve module headings.
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
*/
void ResetHeadings(wpi::array<Rotation2d, NumModules> moduleHeadings);
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
@@ -270,7 +289,7 @@ class SwerveDriveKinematics
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
mutable wpi::array<Rotation2d, NumModules> m_moduleHeadings;
mutable Translation2d m_previousCoR;
};

19
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -17,18 +17,28 @@ template <typename ModuleTranslation, typename... ModuleTranslations>
SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
-> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::ResetHeadings(
wpi::array<Rotation2d, NumModules> moduleHeadings) {
for (size_t i = 0; i < NumModules; i++) {
m_moduleHeadings[i] = moduleHeadings[i];
}
}
template <size_t NumModules>
wpi::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
wpi::array<SwerveModuleState, NumModules> moduleStates(wpi::empty_array);
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
for (size_t i = 0; i < NumModules; i++) {
m_moduleStates[i].speed = 0_mps;
moduleStates[i] = {0_mps, m_moduleHeadings[i]};
}
return m_moduleStates;
return moduleStates;
}
// We have a new center of rotation. We need to compute the matrix again.
@@ -58,10 +68,11 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
m_moduleStates[i] = {speed, rotation};
moduleStates[i] = {speed, rotation};
m_moduleHeadings[i] = rotation;
}
return m_moduleStates;
return moduleStates;
}
template <size_t NumModules>