12 #include <hal/FRCUsageReporting.h>
14 #include "frc/geometry/Translation2d.h"
15 #include "frc/kinematics/ChassisSpeeds.h"
16 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
60 : m_frontLeftWheel{frontLeftWheel},
61 m_frontRightWheel{frontRightWheel},
62 m_rearLeftWheel{rearLeftWheel},
63 m_rearRightWheel{rearRightWheel} {
64 SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
66 m_forwardKinematics = m_inverseKinematics.householderQr();
67 HAL_Report(HALUsageReporting::kResourceType_Kinematics,
68 HALUsageReporting::kKinematics_MecanumDrive);
103 const ChassisSpeeds& chassisSpeeds,
104 const Translation2d& centerOfRotation = Translation2d());
116 ChassisSpeeds
ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds);
119 Eigen::Matrix<double, 4, 3> m_inverseKinematics;
120 Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
121 Translation2d m_frontLeftWheel;
122 Translation2d m_frontRightWheel;
123 Translation2d m_rearLeftWheel;
124 Translation2d m_rearRightWheel;
126 Translation2d m_previousCoR;
140 void SetInverseKinematics(Translation2d fl, Translation2d fr,
141 Translation2d rl, Translation2d rr);