CTRE Phoenix 6 C++ 24.50.0-alpha-2
SwerveDriveKinematics.hpp
Go to the documentation of this file.
1/*
2 * Copyright (C) Cross The Road Electronics.  All rights reserved.
3 * License information can be found in CTRE_LICENSE.txt
4 * For support and suggestions contact support@ctr-electronics.com or file
5 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
6 */
7#pragma once
8
10#include <units/angular_velocity.h>
11#include <units/velocity.h>
12#include <memory>
13#include <vector>
14
15/**
16 * \brief Represents the state of one swerve module.
17 */
18namespace ctre {
19namespace phoenix6 {
20namespace swerve {
21namespace impl {
22
24 /**
25 * \brief Speed of the wheel of the module.
26 */
27 units::meters_per_second_t speed = 0_mps;
28 /**
29 * \brief Angle of the module.
30 */
31 Rotation2d angle = 0_rad;
32
33 /**
34 * \brief Minimize the change in heading the desired swerve module state would
35 * require by potentially reversing the direction the wheel spins. If this is
36 * used with the PIDController class's continuous input functionality, the
37 * furthest a wheel will ever rotate is 90 degrees.
38 *
39 * \param desiredState The desired state.
40 * \param currentAngle The current module angle.
41 */
42 static SwerveModuleState Optimize(SwerveModuleState const &desiredState, Rotation2d currentAngle)
43 {
44 auto const delta = desiredState.angle - currentAngle;
45 if (units::math::abs(delta.Degrees()) > 90_deg) {
46 return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
47 } else {
48 return desiredState;
49 }
50 }
51
52 bool operator==(SwerveModuleState const &other) const
53 {
54 return units::math::abs(speed - other.speed) < 1E-9_mps &&
55 angle == other.angle;
56 }
57 bool operator!=(SwerveModuleState const &other) const { return !(*this == other); }
58};
59
60/**
61 * \brief Represents the position of one swerve module.
62 */
64 /**
65 * \brief Distance the wheel of a module has traveled
66 */
67 units::meter_t distance = 0_m;
68 /**
69 * \brief Angle of the module.
70 */
71 Rotation2d angle;
72
73 SwerveModulePosition Interpolate(SwerveModulePosition const &endValue, double t) const
74 {
75 return {Lerp(distance, endValue.distance, t), Lerp(angle, endValue.angle, t)};
76 }
77
78 bool operator==(SwerveModulePosition const &other) const
79 {
80 return units::math::abs(distance - other.distance) < 1E-9_m &&
81 angle == other.angle;
82 }
83 bool operator!=(SwerveModulePosition const &other) const { return !(*this == other); }
84};
85
86/**
87 * \brief Represents the speed of a robot chassis. Although this struct contains
88 * similar members compared to a Twist2d, they do NOT represent the same thing.
89 * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
90 * reference, a ChassisSpeeds struct represents a robot's velocity.
91 *
92 * A strictly non-holonomic drivetrain, such as a differential drive, should
93 * never have a dy component because it can never move sideways. Holonomic
94 * drivetrains such as swerve and mecanum will often have all three components.
95 */
97 /**
98 * \brief Velocity along the x-axis. (Fwd is +)
99 */
100 units::meters_per_second_t vx = 0_mps;
101 /**
102 * \brief Velocity along the y-axis. (Left is +)
103 */
104 units::meters_per_second_t vy = 0_mps;
105 /**
106 * \brief Represents the angular velocity of the robot frame. (CCW is +)
107 */
108 units::radians_per_second_t omega = 0_rad_per_s;
109
110 /**
111 * \brief Creates a Twist2d from ChassisSpeeds.
112 *
113 * \param dt The duration of the timestep.
114 *
115 * \returns Twist2d.
116 */
117 constexpr Twist2d ToTwist2d(units::second_t dt) const
118 {
119 return {vx * dt, vy * dt, omega * dt};
120 }
121
122 /**
123 * \brief Disretizes a continuous-time chassis speed.
124 *
125 * This function converts a continuous-time chassis speed into a discrete-time
126 * one such that when the discrete-time chassis speed is applied for one
127 * timestep, the robot moves as if the velocity components are independent
128 * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
129 * y-axis, and omega * dt around the z-axis).
130 *
131 * This is useful for compensating for translational skew when translating and
132 * rotating a swerve drivetrain.
133 *
134 * \param continuousSpeeds The continuous speeds.
135 * \param dt The duration of the timestep the speeds should be applied for.
136 *
137 * \returns Discretized ChassisSpeeds.
138 */
139 static ChassisSpeeds Discretize(ChassisSpeeds const &continuousSpeeds, units::second_t dt)
140 {
141 Pose2d const desiredDeltaPose{continuousSpeeds.vx * dt, continuousSpeeds.vy * dt, continuousSpeeds.omega * dt};
142 auto const twist = Pose2d{}.Log(desiredDeltaPose);
143 return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
144 }
145
146 /**
147 * \brief Converts a user provided field-relative ChassisSpeeds object into a
148 * robot-relative ChassisSpeeds object.
149 *
150 * \param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds
151 * in the field frame of reference. Positive x is away from your alliance
152 * wall. Positive y is to your left when standing behind the alliance wall.
153 * \param robotAngle The angle of the robot as measured by a gyroscope. The
154 * robot's angle is considered to be zero when it is facing directly away
155 * from your alliance station wall. Remember that this should be CCW
156 * positive.
157 * \returns ChassisSpeeds object representing the speeds in the robot's frame
158 * of reference.
159 */
160 static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
161 {
162 auto const rotated = Translation2d{fieldRelativeSpeeds.vx * 1_s, fieldRelativeSpeeds.vy * 1_s}
163 .RotateBy(-robotAngle);
164 return {rotated.X() / 1_s, rotated.Y() / 1_s, fieldRelativeSpeeds.omega};
165 }
166
167 /**
168 * \brief Converts a user provided robot-relative ChassisSpeeds object into a
169 * field-relative ChassisSpeeds object.
170 *
171 * \param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
172 * in the robot frame of reference. Positive x is the towards robot's
173 * front. Positive y is towards the robot's left.
174 * \param robotAngle The angle of the robot as measured by a gyroscope. The
175 * robot's angle is considered to be zero when it is facing directly away
176 * from your alliance station wall. Remember that this should be CCW
177 * positive.
178 * \returns ChassisSpeeds object representing the speeds in the field's frame
179 * of reference.
180 */
181 static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
182 {
183 auto const rotated = Translation2d{fieldRelativeSpeeds.vx * 1_s, fieldRelativeSpeeds.vy * 1_s}
184 .RotateBy(robotAngle);
185 return {rotated.X() / 1_s, rotated.Y() / 1_s, fieldRelativeSpeeds.omega};
186 }
187
188 constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
189 constexpr ChassisSpeeds operator+(ChassisSpeeds const &other) const
190 {
191 return {vx + other.vx, vy + other.vy, omega + other.omega};
192 }
193 constexpr ChassisSpeeds operator-(ChassisSpeeds const &other) const
194 {
195 return *this + -other;
196 }
197 constexpr ChassisSpeeds operator*(double scalar) const
198 {
199 return {scalar * vx, scalar * vy, scalar * omega};
200 }
201 constexpr ChassisSpeeds operator/(double scalar) const
202 {
203 return *this * (1.0 / scalar);
204 }
205
206 bool operator==(ChassisSpeeds const &other) const
207 {
208 return vx == other.vx && vy == other.vy && omega == other.omega;
209 }
210 bool operator!=(ChassisSpeeds const &other) const { return !(*this == other); }
211};
212
213/**
214 * \brief Helper class that converts a chassis velocity (dx, dy, and dtheta components)
215 * into individual module states (speed and angle).
216 *
217 * The inverse kinematics (converting from a desired chassis velocity to
218 * individual module states) uses the relative locations of the modules with
219 * respect to the center of rotation. The center of rotation for inverse
220 * kinematics is also variable. This means that you can set your set your center
221 * of rotation in a corner of the robot to perform special evasion maneuvers.
222 *
223 * Forward kinematics (converting an array of module states into the overall
224 * chassis motion) is performs the exact opposite of what inverse kinematics
225 * does. Since this is an overdetermined system (more equations than variables),
226 * we use a least-squares approximation.
227 *
228 * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
229 * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
230 * multiply by [moduleStates] to get our chassis speeds.
231 *
232 * Forward kinematics is also used for odometry -- determining the position of
233 * the robot on the field using encoders and a gyro.
234 */
236 size_t m_numModules;
237 std::vector<Translation2d> m_moduleLocations;
238
239 struct KinematicsMatrices;
240 std::unique_ptr<KinematicsMatrices> m_matrices;
241
242 std::vector<Rotation2d> m_lastModuleHeading;
243 Translation2d m_lastCOR{};
244
245public:
246 using WheelSpeeds = std::vector<SwerveModuleState>;
247 using WheelPositions = std::vector<SwerveModulePosition>;
248
249 /**
250 * \brief Constructs a swerve drive kinematics object. This takes in a variable
251 * number of module locations as Translation2ds. The order in which you pass
252 * in the module locations is the same order that you will receive the module
253 * states when performing inverse kinematics. It is also expected that you
254 * pass in the module states in the same order when calling the forward
255 * kinematics methods.
256 *
257 * \param moduleTranslations The locations of the modules relative to the
258 * physical center of the robot.
259 */
260 SwerveDriveKinematics(std::vector<Translation2d> moduleLocations);
261
265
268
269 /**
270 * \brief Reset the internal swerve module headings.
271 *
272 * \param moduleHeadings The swerve module headings. The order of the module
273 * headings should be same as passed into the constructor of this class.
274 */
275 void ResetHeadings(std::vector<Rotation2d> const &moduleHeadings)
276 {
277 for (size_t i = 0; i < m_numModules && i < moduleHeadings.size(); ++i) {
278 m_lastModuleHeading[i] = moduleHeadings[i];
279 }
280 }
281
282 /**
283 * \brief Performs inverse kinematics to return the module states from a desired
284 * chassis velocity. This method is often used to convert joystick values into
285 * module speeds and angles.
286 *
287 * This function also supports variable centers of rotation. During normal
288 * operations, the center of rotation is usually the same as the physical
289 * center of the robot; therefore, the argument is defaulted to that use case.
290 * However, if you wish to change the center of rotation for evasive
291 * maneuvers, vision alignment, or for any other use case, you can do so.
292 *
293 * In the case that the desired chassis speeds are zero (i.e. the robot will
294 * be stationary), the previously calculated module angle will be maintained.
295 *
296 * \param chassisSpeeds The desired chassis speed.
297 * \param centerOfRotation The center of rotation. For example, if you set the
298 * center of rotation at one corner of the robot and provide a chassis speed
299 * that only has a dtheta component, the robot will rotate around that corner.
300 *
301 * \returns A vector containing the module states. Use caution because these
302 * module states are not normalized. Sometimes, a user input may cause one of
303 * the module speeds to go above the attainable max velocity. Use the
304 * DesaturateWheelSpeeds(WheelSpeeds*, units::meters_per_second_t) function to
305 * rectify this issue.
306 */
307 WheelSpeeds ToSwerveModuleStates(ChassisSpeeds const &speeds, Translation2d const &centerOfRotation = Translation2d{});
308
309 /**
310 * \brief Performs forward kinematics to return the resulting chassis state from the
311 * given module states. This method is often used for odometry -- determining
312 * the robot's position on the field using data from the real-world speed and
313 * angle of each module on the robot.
314 *
315 * \param moduleStates The state of the modules as a std::vector of type
316 * SwerveModuleState as measured from respective encoders and gyros. The
317 * order of the swerve module states should be same as passed into the
318 * constructor of this class.
319 *
320 * \returns The resulting chassis speed.
321 */
322 ChassisSpeeds ToChassisSpeeds(WheelSpeeds const &moduleStates) const;
323
324 /**
325 * \brief Performs forward kinematics to return the resulting Twist2d from the
326 * given module position deltas. This method is often used for odometry --
327 * determining the robot's position on the field using data from the
328 * real-world position delta and angle of each module on the robot.
329 *
330 * \param moduleDeltas The latest change in position of the modules (as a
331 * SwerveModulePosition type) as measured from respective encoders and gyros.
332 * The order of the swerve module states should be same as passed into the
333 * constructor of this class.
334 *
335 * \returns The resulting Twist2d.
336 */
337 Twist2d ToTwist2d(WheelPositions const &moduleDeltas) const;
338
339 /**
340 * \brief Performs forward kinematics to return the resulting Twist2d from the given
341 * change in wheel positions. This method is often used for odometry --
342 * determining the robot's position on the field using changes in the distance
343 * driven by each wheel on the robot.
344 *
345 * \param start The starting distances driven by the wheels.
346 * \param end The ending distances driven by the wheels.
347 *
348 * \returns The resulting Twist2d in the robot's movement.
349 */
350 Twist2d ToTwist2d(WheelPositions const &start, WheelPositions const &end) const
351 {
352 WheelPositions result(m_numModules);
353 for (size_t i = 0; i < m_numModules && i < std::min(start.size(), end.size()); ++i) {
354 result[i] = {end[i].distance - start[i].distance, end[i].angle};
355 }
356 return ToTwist2d(result);
357 }
358
359 /**
360 * \brief Performs interpolation between two values.
361 *
362 * \param start The value to start at.
363 * \param end The value to end at.
364 * \param t How far between the two values to interpolate. This should be
365 * bounded to [0, 1].
366 * \returns The interpolated value.
367 */
368 WheelPositions Interpolate(WheelPositions const &start, WheelPositions const &end, double t) const
369 {
370 WheelPositions result(m_numModules);
371 for (size_t i = 0; i < m_numModules && i < std::min(start.size(), end.size()); ++i) {
372 result[i] = start[i].Interpolate(end[i], t);
373 }
374 return result;
375 }
376
377 /**
378 * \brief Renormalizes the wheel speeds if any individual speed is above the
379 * specified maximum.
380 *
381 * Sometimes, after inverse kinematics, the requested speed
382 * from one or more modules may be above the max attainable speed for the
383 * driving motor on that module. To fix this issue, one can reduce all the
384 * wheel speeds to make sure that all requested module speeds are at-or-below
385 * the absolute threshold, while maintaining the ratio of speeds between
386 * modules.
387 *
388 * \param moduleStates Reference to vector of module states. The vector will be
389 * mutated with the normalized speeds!
390 * \param attainableMaxSpeed The absolute max speed that a module can reach.
391 */
392 static void DesaturateWheelSpeeds(WheelSpeeds *moduleStates, units::meters_per_second_t attainableMaxSpeed);
393};
394
395}
396}
397}
398}
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.hpp:235
WheelSpeeds ToSwerveModuleStates(ChassisSpeeds const &speeds, Translation2d const &centerOfRotation=Translation2d{})
Performs inverse kinematics to return the module states from a desired chassis velocity.
SwerveDriveKinematics(std::vector< Translation2d > moduleLocations)
Constructs a swerve drive kinematics object.
Twist2d ToTwist2d(WheelPositions const &start, WheelPositions const &end) const
Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
Definition: SwerveDriveKinematics.hpp:350
std::vector< SwerveModulePosition > WheelPositions
Definition: SwerveDriveKinematics.hpp:247
SwerveDriveKinematics & operator=(SwerveDriveKinematics const &)
static void DesaturateWheelSpeeds(WheelSpeeds *moduleStates, units::meters_per_second_t attainableMaxSpeed)
Renormalizes the wheel speeds if any individual speed is above the specified maximum.
ChassisSpeeds ToChassisSpeeds(WheelSpeeds const &moduleStates) const
Performs forward kinematics to return the resulting chassis state from the given module states.
WheelPositions Interpolate(WheelPositions const &start, WheelPositions const &end, double t) const
Performs interpolation between two values.
Definition: SwerveDriveKinematics.hpp:368
SwerveDriveKinematics(SwerveDriveKinematics const &)
SwerveDriveKinematics & operator=(SwerveDriveKinematics &&)
void ResetHeadings(std::vector< Rotation2d > const &moduleHeadings)
Reset the internal swerve module headings.
Definition: SwerveDriveKinematics.hpp:275
Twist2d ToTwist2d(WheelPositions const &moduleDeltas) const
Performs forward kinematics to return the resulting Twist2d from the given module position deltas.
std::vector< SwerveModuleState > WheelSpeeds
Definition: SwerveDriveKinematics.hpp:246
Represents the state of one swerve module.
Definition: StatusCodes.h:18
Represents the speed of a robot chassis.
Definition: SwerveDriveKinematics.hpp:96
constexpr ChassisSpeeds operator-(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:193
constexpr ChassisSpeeds operator*(double scalar) const
Definition: SwerveDriveKinematics.hpp:197
constexpr ChassisSpeeds operator-() const
Definition: SwerveDriveKinematics.hpp:188
static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
Converts a user provided field-relative ChassisSpeeds object into a robot-relative ChassisSpeeds obje...
Definition: SwerveDriveKinematics.hpp:160
bool operator!=(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:210
constexpr Twist2d ToTwist2d(units::second_t dt) const
Creates a Twist2d from ChassisSpeeds.
Definition: SwerveDriveKinematics.hpp:117
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition: SwerveDriveKinematics.hpp:108
units::meters_per_second_t vx
Velocity along the x-axis.
Definition: SwerveDriveKinematics.hpp:100
units::meters_per_second_t vy
Velocity along the y-axis.
Definition: SwerveDriveKinematics.hpp:104
constexpr ChassisSpeeds operator+(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:189
static ChassisSpeeds Discretize(ChassisSpeeds const &continuousSpeeds, units::second_t dt)
Disretizes a continuous-time chassis speed.
Definition: SwerveDriveKinematics.hpp:139
constexpr ChassisSpeeds operator/(double scalar) const
Definition: SwerveDriveKinematics.hpp:201
bool operator==(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:206
static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds obje...
Definition: SwerveDriveKinematics.hpp:181
Represents the position of one swerve module.
Definition: SwerveDriveKinematics.hpp:63
bool operator!=(SwerveModulePosition const &other) const
Definition: SwerveDriveKinematics.hpp:83
bool operator==(SwerveModulePosition const &other) const
Definition: SwerveDriveKinematics.hpp:78
Rotation2d angle
Angle of the module.
Definition: SwerveDriveKinematics.hpp:71
units::meter_t distance
Distance the wheel of a module has traveled.
Definition: SwerveDriveKinematics.hpp:67
SwerveModulePosition Interpolate(SwerveModulePosition const &endValue, double t) const
Definition: SwerveDriveKinematics.hpp:73
Definition: SwerveDriveKinematics.hpp:23
bool operator==(SwerveModuleState const &other) const
Definition: SwerveDriveKinematics.hpp:52
units::meters_per_second_t speed
Speed of the wheel of the module.
Definition: SwerveDriveKinematics.hpp:27
Rotation2d angle
Angle of the module.
Definition: SwerveDriveKinematics.hpp:31
bool operator!=(SwerveModuleState const &other) const
Definition: SwerveDriveKinematics.hpp:57
static SwerveModuleState Optimize(SwerveModuleState const &desiredState, Rotation2d currentAngle)
Minimize the change in heading the desired swerve module state would require by potentially reversing...
Definition: SwerveDriveKinematics.hpp:42