WPILibC++  2020.3.2
MecanumDriveKinematics.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <Eigen/Core>
11 #include <Eigen/QR>
12 #include <hal/FRCUsageReporting.h>
13 
14 #include "frc/geometry/Translation2d.h"
15 #include "frc/kinematics/ChassisSpeeds.h"
16 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
17 
18 namespace frc {
19 
43  public:
56  explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
57  Translation2d frontRightWheel,
58  Translation2d rearLeftWheel,
59  Translation2d rearRightWheel)
60  : m_frontLeftWheel{frontLeftWheel},
61  m_frontRightWheel{frontRightWheel},
62  m_rearLeftWheel{rearLeftWheel},
63  m_rearRightWheel{rearRightWheel} {
64  SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
65  rearRightWheel);
66  m_forwardKinematics = m_inverseKinematics.householderQr();
67  HAL_Report(HALUsageReporting::kResourceType_Kinematics,
68  HALUsageReporting::kKinematics_MecanumDrive);
69  }
70 
72 
102  MecanumDriveWheelSpeeds ToWheelSpeeds(
103  const ChassisSpeeds& chassisSpeeds,
104  const Translation2d& centerOfRotation = Translation2d());
105 
116  ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds);
117 
118  private:
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;
125 
126  Translation2d m_previousCoR;
127 
140  void SetInverseKinematics(Translation2d fl, Translation2d fr,
141  Translation2d rl, Translation2d rr);
142 };
143 
144 } // namespace frc
frc::Translation2d
Represents a translation in 2d space.
Definition: Translation2d.h:28
frc::MecanumDriveKinematics::MecanumDriveKinematics
MecanumDriveKinematics(Translation2d frontLeftWheel, Translation2d frontRightWheel, Translation2d rearLeftWheel, Translation2d rearRightWheel)
Constructs a mecanum drive kinematics object.
Definition: MecanumDriveKinematics.h:56
frc::MecanumDriveKinematics::ToWheelSpeeds
MecanumDriveWheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds, const Translation2d &centerOfRotation=Translation2d())
Performs inverse kinematics to return the wheel speeds from a desired chassis velocity.
frc::MecanumDriveKinematics::ToChassisSpeeds
ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds &wheelSpeeds)
Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::MecanumDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:42