15 #include <hal/FRCUsageReporting.h>
16 #include <units/units.h>
18 #include "frc/geometry/Rotation2d.h"
19 #include "frc/geometry/Translation2d.h"
20 #include "frc/kinematics/ChassisSpeeds.h"
21 #include "frc/kinematics/SwerveModuleState.h"
46 template <
size_t NumModules>
60 template <
typename... Wheels>
62 : m_modules{wheel, wheels...} {
63 static_assert(
sizeof...(wheels) >= 1,
64 "A swerve drive requires at least two modules");
66 for (
size_t i = 0; i < NumModules; i++) {
68 m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
69 1, 0, (-m_modules[i].Y()).
template to<double>(),
70 0, 1, (+m_modules[i].X()).
template to<double>();
74 m_forwardKinematics = m_inverseKinematics.householderQr();
76 HAL_Report(HALUsageReporting::kResourceType_Kinematics,
77 HALUsageReporting::kKinematics_SwerveDrive);
110 const ChassisSpeeds& chassisSpeeds,
111 const Translation2d& centerOfRotation = Translation2d());
125 template <
typename... ModuleStates>
142 std::array<SwerveModuleState, NumModules> moduleStates);
157 std::array<SwerveModuleState, NumModules>* moduleStates,
158 units::meters_per_second_t attainableMaxSpeed);
161 Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
162 Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
164 std::array<Translation2d, NumModules> m_modules;
166 Translation2d m_previousCoR;
170 #include "SwerveDriveKinematics.inc"