WPILibC++ 2024.3.2
SwerveDriveKinematics.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <concepts>
8#include <cstddef>
9
10#include <Eigen/QR>
11#include <wpi/SymbolExports.h>
12#include <wpi/array.h>
13
14#include "frc/EigenCore.h"
23#include "units/velocity.h"
24#include "wpimath/MathShared.h"
25
26namespace frc {
27
28template <size_t NumModules>
30
31/**
32 * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
33 * into individual module states (speed and angle).
34 *
35 * The inverse kinematics (converting from a desired chassis velocity to
36 * individual module states) uses the relative locations of the modules with
37 * respect to the center of rotation. The center of rotation for inverse
38 * kinematics is also variable. This means that you can set your set your center
39 * of rotation in a corner of the robot to perform special evasion maneuvers.
40 *
41 * Forward kinematics (converting an array of module states into the overall
42 * chassis motion) is performs the exact opposite of what inverse kinematics
43 * does. Since this is an overdetermined system (more equations than variables),
44 * we use a least-squares approximation.
45 *
46 * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
47 * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
48 * multiply by [moduleStates] to get our chassis speeds.
49 *
50 * Forward kinematics is also used for odometry -- determining the position of
51 * the robot on the field using encoders and a gyro.
52 */
53template <size_t NumModules>
55 : public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
56 SwerveDriveWheelPositions<NumModules>> {
57 public:
58 /**
59 * Constructs a swerve drive kinematics object. This takes in a variable
60 * number of module locations as Translation2ds. The order in which you pass
61 * in the module locations is the same order that you will receive the module
62 * states when performing inverse kinematics. It is also expected that you
63 * pass in the module states in the same order when calling the forward
64 * kinematics methods.
65 *
66 * @param moduleTranslations The locations of the modules relative to the
67 * physical center of the robot.
68 */
69 template <std::convertible_to<Translation2d>... ModuleTranslations>
70 requires(sizeof...(ModuleTranslations) == NumModules)
71 explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
73 for (size_t i = 0; i < NumModules; i++) {
74 // clang-format off
75 m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
76 1, 0, (-m_modules[i].Y()).value(),
77 0, 1, (+m_modules[i].X()).value();
78 // clang-format on
79 }
80
81 m_forwardKinematics = m_inverseKinematics.householderQr();
82
85 }
86
90 for (size_t i = 0; i < NumModules; i++) {
91 // clang-format off
92 m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
93 1, 0, (-m_modules[i].Y()).value(),
94 0, 1, (+m_modules[i].X()).value();
95 // clang-format on
96 }
97
98 m_forwardKinematics = m_inverseKinematics.householderQr();
99
102 }
103
105
106 /**
107 * Reset the internal swerve module headings.
108 * @param moduleHeadings The swerve module headings. The order of the module
109 * headings should be same as passed into the constructor of this class.
110 */
116 }
117
118 /**
119 * Reset the internal swerve module headings.
120 * @param moduleHeadings The swerve module headings. The order of the module
121 * headings should be same as passed into the constructor of this class.
122 */
124
125 /**
126 * Performs inverse kinematics to return the module states from a desired
127 * chassis velocity. This method is often used to convert joystick values into
128 * module speeds and angles.
129 *
130 * This function also supports variable centers of rotation. During normal
131 * operations, the center of rotation is usually the same as the physical
132 * center of the robot; therefore, the argument is defaulted to that use case.
133 * However, if you wish to change the center of rotation for evasive
134 * maneuvers, vision alignment, or for any other use case, you can do so.
135 *
136 * In the case that the desired chassis speeds are zero (i.e. the robot will
137 * be stationary), the previously calculated module angle will be maintained.
138 *
139 * @param chassisSpeeds The desired chassis speed.
140 * @param centerOfRotation The center of rotation. For example, if you set the
141 * center of rotation at one corner of the robot and provide a chassis speed
142 * that only has a dtheta component, the robot will rotate around that corner.
143 *
144 * @return An array containing the module states. Use caution because these
145 * module states are not normalized. Sometimes, a user input may cause one of
146 * the module speeds to go above the attainable max velocity. Use the
147 * DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
148 * units::meters_per_second_t) function to rectify this issue. In addition,
149 * you can leverage the power of C++17 to directly assign the module states to
150 * variables:
151 *
152 * @code{.cpp}
153 * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
154 * @endcode
155 */
157 const ChassisSpeeds& chassisSpeeds,
158 const Translation2d& centerOfRotation = Translation2d{}) const;
159
161 const ChassisSpeeds& chassisSpeeds) const override {
163 }
164
165 /**
166 * Performs forward kinematics to return the resulting chassis state from the
167 * given module states. This method is often used for odometry -- determining
168 * the robot's position on the field using data from the real-world speed and
169 * angle of each module on the robot.
170 *
171 * @param moduleStates The state of the modules (as a SwerveModuleState type)
172 * as measured from respective encoders and gyros. The order of the swerve
173 * module states should be same as passed into the constructor of this class.
174 *
175 * @return The resulting chassis speed.
176 */
177 template <std::convertible_to<SwerveModuleState>... ModuleStates>
178 requires(sizeof...(ModuleStates) == NumModules)
179 ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
180 return this->ToChassisSpeeds(
182 }
183
184 /**
185 * Performs forward kinematics to return the resulting chassis state from the
186 * given module states. This method is often used for odometry -- determining
187 * the robot's position on the field using data from the real-world speed and
188 * angle of each module on the robot.
189 *
190 * @param moduleStates The state of the modules as an wpi::array of type
191 * SwerveModuleState, NumModules long as measured from respective encoders
192 * and gyros. The order of the swerve module states should be same as passed
193 * into the constructor of this class.
194 *
195 * @return The resulting chassis speed.
196 */
198 moduleStates) const override;
199
200 /**
201 * Performs forward kinematics to return the resulting Twist2d from the
202 * given module position deltas. This method is often used for odometry --
203 * determining the robot's position on the field using data from the
204 * real-world position delta and angle of each module on the robot.
205 *
206 * @param moduleDeltas The latest change in position of the modules (as a
207 * SwerveModulePosition type) as measured from respective encoders and gyros.
208 * The order of the swerve module states should be same as passed into the
209 * constructor of this class.
210 *
211 * @return The resulting Twist2d.
212 */
213 template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
214 requires(sizeof...(ModuleDeltas) == NumModules)
215 Twist2d ToTwist2d(ModuleDeltas&&... moduleDeltas) const {
216 return this->ToTwist2d(
218 }
219
220 /**
221 * Performs forward kinematics to return the resulting Twist2d from the
222 * given module position deltas. This method is often used for odometry --
223 * determining the robot's position on the field using data from the
224 * real-world position delta and angle of each module on the robot.
225 *
226 * @param moduleDeltas The latest change in position of the modules (as a
227 * SwerveModulePosition type) as measured from respective encoders and gyros.
228 * The order of the swerve module states should be same as passed into the
229 * constructor of this class.
230 *
231 * @return The resulting Twist2d.
232 */
235
238 const SwerveDriveWheelPositions<NumModules>& end) const override {
239 auto result =
241 for (size_t i = 0; i < NumModules; i++) {
242 auto startModule = start.positions[i];
243 auto endModule = end.positions[i];
244 result[i] = {endModule.distance - startModule.distance, endModule.angle};
245 }
247 }
248
249 /**
250 * Renormalizes the wheel speeds if any individual speed is above the
251 * specified maximum.
252 *
253 * Sometimes, after inverse kinematics, the requested speed
254 * from one or more modules may be above the max attainable speed for the
255 * driving motor on that module. To fix this issue, one can reduce all the
256 * wheel speeds to make sure that all requested module speeds are at-or-below
257 * the absolute threshold, while maintaining the ratio of speeds between
258 * modules.
259 *
260 * @param moduleStates Reference to array of module states. The array will be
261 * mutated with the normalized speeds!
262 * @param attainableMaxSpeed The absolute max speed that a module can reach.
263 */
264 static void DesaturateWheelSpeeds(
266 units::meters_per_second_t attainableMaxSpeed);
267
268 /**
269 * Renormalizes the wheel speeds if any individual speed is above the
270 * specified maximum, as well as getting rid of joystick saturation at edges
271 * of joystick.
272 *
273 * Sometimes, after inverse kinematics, the requested speed
274 * from one or more modules may be above the max attainable speed for the
275 * driving motor on that module. To fix this issue, one can reduce all the
276 * wheel speeds to make sure that all requested module speeds are at-or-below
277 * the absolute threshold, while maintaining the ratio of speeds between
278 * modules.
279 *
280 * @param moduleStates Reference to array of module states. The array will be
281 * mutated with the normalized speeds!
282 * @param desiredChassisSpeed The desired speed of the robot
283 * @param attainableMaxModuleSpeed The absolute max speed a module can reach
284 * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
285 * can reach while translating
286 * @param attainableMaxRobotRotationSpeed The absolute max speed the robot can
287 * reach while rotating
288 */
289 static void DesaturateWheelSpeeds(
291 ChassisSpeeds desiredChassisSpeed,
292 units::meters_per_second_t attainableMaxModuleSpeed,
293 units::meters_per_second_t attainableMaxRobotTranslationSpeed,
295
296 private:
297 mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
298 Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
301
302 mutable Translation2d m_previousCoR;
303};
304
305extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
306 SwerveDriveKinematics<4>;
307
308} // namespace frc
309
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: Kinematics.h:23
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:56
wpi::array< SwerveModuleState, NumModules > ToSwerveModuleStates(const ChassisSpeeds &chassisSpeeds, const Translation2d &centerOfRotation=Translation2d{}) const
Performs inverse kinematics to return the module states from a desired chassis velocity.
Definition: SwerveDriveKinematics.inc:30
SwerveDriveKinematics(const SwerveDriveKinematics &)=default
Reset the internal swerve module headings.
Definition: SwerveDriveKinematics.h:113
SwerveDriveWheelSpeeds< NumModules > ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds) const override
Performs inverse kinematics to return the wheel speeds from a desired chassis velocity.
Definition: SwerveDriveKinematics.h:160
SwerveDriveKinematics(ModuleTranslations &&... moduleTranslations)
Constructs a swerve drive kinematics object.
Definition: SwerveDriveKinematics.h:71
Twist2d ToTwist2d(const SwerveDriveWheelPositions< NumModules > &start, const SwerveDriveWheelPositions< NumModules > &end) const override
Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
Definition: SwerveDriveKinematics.h:236
Twist2d ToTwist2d(ModuleDeltas &&... moduleDeltas) const
Performs forward kinematics to return the resulting Twist2d from the given module position deltas.
Definition: SwerveDriveKinematics.h:215
SwerveDriveKinematics(const wpi::array< Translation2d, NumModules > &modules)
Definition: SwerveDriveKinematics.h:87
ChassisSpeeds ToChassisSpeeds(ModuleStates &&... moduleStates) const
Performs forward kinematics to return the resulting chassis state from the given module states.
Definition: SwerveDriveKinematics.h:179
static void DesaturateWheelSpeeds(wpi::array< SwerveModuleState, NumModules > *moduleStates, units::meters_per_second_t attainableMaxSpeed)
Renormalizes the wheel speeds if any individual speed is above the specified maximum.
Definition: SwerveDriveKinematics.inc:118
Represents a translation in 2D space.
Definition: Translation2d.h:27
This class is a wrapper around std::array that does compile time size checking.
Definition: array.h:26
static void ReportUsage(MathUsageId id, int count)
Definition: MathShared.h:73
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21