WPILibC++  2020.3.2
SwerveDriveKinematics.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 <array>
11 #include <cstddef>
12 
13 #include <Eigen/Core>
14 #include <Eigen/QR>
15 #include <hal/FRCUsageReporting.h>
16 #include <units/units.h>
17 
18 #include "frc/geometry/Rotation2d.h"
19 #include "frc/geometry/Translation2d.h"
20 #include "frc/kinematics/ChassisSpeeds.h"
21 #include "frc/kinematics/SwerveModuleState.h"
22 
23 namespace frc {
46 template <size_t NumModules>
48  public:
60  template <typename... Wheels>
61  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
62  : m_modules{wheel, wheels...} {
63  static_assert(sizeof...(wheels) >= 1,
64  "A swerve drive requires at least two modules");
65 
66  for (size_t i = 0; i < NumModules; i++) {
67  // clang-format off
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>();
71  // clang-format on
72  }
73 
74  m_forwardKinematics = m_inverseKinematics.householderQr();
75 
76  HAL_Report(HALUsageReporting::kResourceType_Kinematics,
77  HALUsageReporting::kKinematics_SwerveDrive);
78  }
79 
81 
109  std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
110  const ChassisSpeeds& chassisSpeeds,
111  const Translation2d& centerOfRotation = Translation2d());
112 
125  template <typename... ModuleStates>
126  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
127 
141  ChassisSpeeds ToChassisSpeeds(
142  std::array<SwerveModuleState, NumModules> moduleStates);
143 
156  static void NormalizeWheelSpeeds(
157  std::array<SwerveModuleState, NumModules>* moduleStates,
158  units::meters_per_second_t attainableMaxSpeed);
159 
160  private:
161  Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
162  Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
163  m_forwardKinematics;
164  std::array<Translation2d, NumModules> m_modules;
165 
166  Translation2d m_previousCoR;
167 };
168 } // namespace frc
169 
170 #include "SwerveDriveKinematics.inc"
frc::SwerveDriveKinematics::ToSwerveModuleStates
std::array< SwerveModuleState, NumModules > ToSwerveModuleStates(const ChassisSpeeds &chassisSpeeds, const Translation2d &centerOfRotation=Translation2d())
Performs inverse kinematics to return the module states from a desired chassis velocity.
Definition: SwerveDriveKinematics.inc:20
frc::Translation2d
Represents a translation in 2d space.
Definition: Translation2d.h:28
frc::SwerveDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:47
frc::SwerveDriveKinematics::ToChassisSpeeds
ChassisSpeeds ToChassisSpeeds(ModuleStates &&... wheelStates)
Performs forward kinematics to return the resulting chassis state from the given module states.
Definition: SwerveDriveKinematics.inc:59
frc::SwerveDriveKinematics::SwerveDriveKinematics
SwerveDriveKinematics(Translation2d wheel, Wheels &&... wheels)
Constructs a swerve drive kinematics object.
Definition: SwerveDriveKinematics.h:61
frc::SwerveDriveKinematics::NormalizeWheelSpeeds
static void NormalizeWheelSpeeds(std::array< SwerveModuleState, NumModules > *moduleStates, units::meters_per_second_t attainableMaxSpeed)
Normalizes the wheel speeds using some max attainable speed.
Definition: SwerveDriveKinematics.inc:92
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16