WPILibC++ 2025.3.1
Loading...
Searching...
No Matches
ChassisSpeeds.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 <wpi/SymbolExports.h>
8
12#include "units/velocity.h"
13
14namespace frc {
15/**
16 * Represents the speed of a robot chassis. Although this struct contains
17 * similar members compared to a Twist2d, they do NOT represent the same thing.
18 * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
19 * reference, a ChassisSpeeds struct represents a robot's velocity.
20 *
21 * A strictly non-holonomic drivetrain, such as a differential drive, should
22 * never have a dy component because it can never move sideways. Holonomic
23 * drivetrains such as swerve and mecanum will often have all three components.
24 */
26 /**
27 * Velocity along the x-axis. (Fwd is +)
28 */
29 units::meters_per_second_t vx = 0_mps;
30
31 /**
32 * Velocity along the y-axis. (Left is +)
33 */
34 units::meters_per_second_t vy = 0_mps;
35
36 /**
37 * Represents the angular velocity of the robot frame. (CCW is +)
38 */
39 units::radians_per_second_t omega = 0_rad_per_s;
40
41 /**
42 * Creates a Twist2d from ChassisSpeeds.
43 *
44 * @param dt The duration of the timestep.
45 *
46 * @return Twist2d.
47 */
48 constexpr Twist2d ToTwist2d(units::second_t dt) const {
49 return Twist2d{vx * dt, vy * dt, omega * dt};
50 }
51
52 /**
53 * Discretizes a continuous-time chassis speed.
54 *
55 * This function converts a continuous-time chassis speed into a discrete-time
56 * one such that when the discrete-time chassis speed is applied for one
57 * timestep, the robot moves as if the velocity components are independent
58 * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
59 * y-axis, and omega * dt around the z-axis).
60 *
61 * This is useful for compensating for translational skew when translating and
62 * rotating a holonomic (swerve or mecanum) drivetrain. However, scaling down
63 * the ChassisSpeeds after discretizing (e.g., when desaturating swerve module
64 * speeds) rotates the direction of net motion in the opposite direction of
65 * rotational velocity, introducing a different translational skew which is
66 * not accounted for by discretization.
67 *
68 * @param vx Forward velocity.
69 * @param vy Sideways velocity.
70 * @param omega Angular velocity.
71 * @param dt The duration of the timestep the speeds should be applied for.
72 *
73 * @return Discretized ChassisSpeeds.
74 */
75 static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx,
76 units::meters_per_second_t vy,
77 units::radians_per_second_t omega,
78 units::second_t dt) {
79 // Construct the desired pose after a timestep, relative to the current
80 // pose. The desired pose has decoupled translation and rotation.
81 Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
82
83 // Find the chassis translation/rotation deltas in the robot frame that move
84 // the robot from its current pose to the desired pose
85 auto twist = Pose2d{}.Log(desiredDeltaPose);
86
87 // Turn the chassis translation/rotation deltas into average velocities
88 return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
89 }
90
91 /**
92 * Discretizes a continuous-time chassis speed.
93 *
94 * This function converts a continuous-time chassis speed into a discrete-time
95 * one such that when the discrete-time chassis speed is applied for one
96 * timestep, the robot moves as if the velocity components are independent
97 * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
98 * y-axis, and omega * dt around the z-axis).
99 *
100 * This is useful for compensating for translational skew when translating and
101 * rotating a holonomic (swerve or mecanum) drivetrain. However, scaling down
102 * the ChassisSpeeds after discretizing (e.g., when desaturating swerve module
103 * speeds) rotates the direction of net motion in the opposite direction of
104 * rotational velocity, introducing a different translational skew which is
105 * not accounted for by discretization.
106 *
107 * @param continuousSpeeds The continuous speeds.
108 * @param dt The duration of the timestep the speeds should be applied for.
109 *
110 * @return Discretized ChassisSpeeds.
111 */
112 static constexpr ChassisSpeeds Discretize(
113 const ChassisSpeeds& continuousSpeeds, units::second_t dt) {
114 return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
115 continuousSpeeds.omega, dt);
116 }
117
118 /**
119 * Converts a user provided field-relative set of speeds into a robot-relative
120 * ChassisSpeeds object.
121 *
122 * @param vx The component of speed in the x direction relative to the field.
123 * Positive x is away from your alliance wall.
124 * @param vy The component of speed in the y direction relative to the field.
125 * Positive y is to your left when standing behind the alliance wall.
126 * @param omega The angular rate of the robot.
127 * @param robotAngle The angle of the robot as measured by a gyroscope. The
128 * robot's angle is considered to be zero when it is facing directly away from
129 * your alliance station wall. Remember that this should be CCW positive.
130 *
131 * @return ChassisSpeeds object representing the speeds in the robot's frame
132 * of reference.
133 */
135 units::meters_per_second_t vx, units::meters_per_second_t vy,
136 units::radians_per_second_t omega, const Rotation2d& robotAngle) {
137 // CW rotation into chassis frame
138 auto rotated =
139 Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
140 .RotateBy(-robotAngle);
141 return {units::meters_per_second_t{rotated.X().value()},
142 units::meters_per_second_t{rotated.Y().value()}, omega};
143 }
144
145 /**
146 * Converts a user provided field-relative ChassisSpeeds object into a
147 * robot-relative ChassisSpeeds object.
148 *
149 * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds
150 * in the field frame of reference. Positive x is away from your alliance
151 * wall. Positive y is to your left when standing behind the alliance wall.
152 * @param robotAngle The angle of the robot as measured by a gyroscope. The
153 * robot's angle is considered to be zero when it is facing directly away
154 * from your alliance station wall. Remember that this should be CCW
155 * positive.
156 * @return ChassisSpeeds object representing the speeds in the robot's frame
157 * of reference.
158 */
160 const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
161 return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
162 fieldRelativeSpeeds.vy,
163 fieldRelativeSpeeds.omega, robotAngle);
164 }
165
166 /**
167 * Converts a user provided robot-relative set of speeds into a field-relative
168 * ChassisSpeeds object.
169 *
170 * @param vx The component of speed in the x direction relative to the robot.
171 * Positive x is towards the robot's front.
172 * @param vy The component of speed in the y direction relative to the robot.
173 * Positive y is towards the robot's left.
174 * @param omega The angular rate of the robot.
175 * @param robotAngle The angle of the robot as measured by a gyroscope. The
176 * robot's angle is considered to be zero when it is facing directly away from
177 * your alliance station wall. Remember that this should be CCW positive.
178 *
179 * @return ChassisSpeeds object representing the speeds in the field's frame
180 * of reference.
181 */
183 units::meters_per_second_t vx, units::meters_per_second_t vy,
184 units::radians_per_second_t omega, const Rotation2d& robotAngle) {
185 // CCW rotation out of chassis frame
186 auto rotated =
187 Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
188 .RotateBy(robotAngle);
189 return {units::meters_per_second_t{rotated.X().value()},
190 units::meters_per_second_t{rotated.Y().value()}, omega};
191 }
192
193 /**
194 * Converts a user provided robot-relative ChassisSpeeds object into a
195 * field-relative ChassisSpeeds object.
196 *
197 * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
198 * in the robot frame of reference. Positive x is the towards robot's
199 * front. Positive y is towards the robot's left.
200 * @param robotAngle The angle of the robot as measured by a gyroscope. The
201 * robot's angle is considered to be zero when it is facing directly away
202 * from your alliance station wall. Remember that this should be CCW
203 * positive.
204 * @return ChassisSpeeds object representing the speeds in the field's frame
205 * of reference.
206 */
208 const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
209 return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
210 robotRelativeSpeeds.vy,
211 robotRelativeSpeeds.omega, robotAngle);
212 }
213
214 /**
215 * Adds two ChassisSpeeds and returns the sum.
216 *
217 * <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
218 * = ChassisSpeeds{3.0, 2.0, 2.0}
219 *
220 * @param other The ChassisSpeeds to add.
221 *
222 * @return The sum of the ChassisSpeeds.
223 */
224 constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
225 return {vx + other.vx, vy + other.vy, omega + other.omega};
226 }
227
228 /**
229 * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
230 * returns the difference.
231 *
232 * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
233 * = ChassisSpeeds{4.0, 2.0, 1.0}
234 *
235 * @param other The ChassisSpeeds to subtract.
236 *
237 * @return The difference between the two ChassisSpeeds.
238 */
239 constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
240 return *this + -other;
241 }
242
243 /**
244 * Returns the inverse of the current ChassisSpeeds.
245 * This is equivalent to negating all components of the ChassisSpeeds.
246 *
247 * @return The inverse of the current ChassisSpeeds.
248 */
249 constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
250
251 /**
252 * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
253 *
254 * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
255 * = ChassisSpeeds{4.0, 5.0, 1.0}
256 *
257 * @param scalar The scalar to multiply by.
258 *
259 * @return The scaled ChassisSpeeds.
260 */
261 constexpr ChassisSpeeds operator*(double scalar) const {
262 return {scalar * vx, scalar * vy, scalar * omega};
263 }
264
265 /**
266 * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
267 *
268 * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
269 * = ChassisSpeeds{1.0, 1.25, 0.5}
270 *
271 * @param scalar The scalar to divide by.
272 *
273 * @return The scaled ChassisSpeeds.
274 */
275 constexpr ChassisSpeeds operator/(double scalar) const {
276 return operator*(1.0 / scalar);
277 }
278
279 /**
280 * Compares the ChassisSpeeds with another ChassisSpeed.
281 *
282 * @param other The other ChassisSpeeds.
283 *
284 * @return The result of the comparison. Is true if they are the same.
285 */
286 constexpr bool operator==(const ChassisSpeeds& other) const = default;
287};
288} // namespace frc
289
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr Twist2d Log(const Pose2d &end) const
Returns a Twist2d that maps this pose to the end pose.
Definition Pose2d.h:346
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
Represents a translation in 2D space.
Definition Translation2d.h:29
constexpr Translation2d RotateBy(const Rotation2d &other) const
Applies a rotation to the translation in 2D space.
Definition Translation2d.h:135
Definition CAN.h:11
Represents the speed of a robot chassis.
Definition ChassisSpeeds.h:25
units::meters_per_second_t vx
Velocity along the x-axis.
Definition ChassisSpeeds.h:29
units::meters_per_second_t vy
Velocity along the y-axis.
Definition ChassisSpeeds.h:34
static constexpr ChassisSpeeds Discretize(const ChassisSpeeds &continuousSpeeds, units::second_t dt)
Discretizes a continuous-time chassis speed.
Definition ChassisSpeeds.h:112
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition ChassisSpeeds.h:39
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(const ChassisSpeeds &robotRelativeSpeeds, const Rotation2d &robotAngle)
Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds obje...
Definition ChassisSpeeds.h:207
constexpr Twist2d ToTwist2d(units::second_t dt) const
Creates a Twist2d from ChassisSpeeds.
Definition ChassisSpeeds.h:48
constexpr ChassisSpeeds operator+(const ChassisSpeeds &other) const
Adds two ChassisSpeeds and returns the sum.
Definition ChassisSpeeds.h:224
static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, units::second_t dt)
Discretizes a continuous-time chassis speed.
Definition ChassisSpeeds.h:75
constexpr ChassisSpeeds operator-(const ChassisSpeeds &other) const
Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
Definition ChassisSpeeds.h:239
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d &robotAngle)
Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object.
Definition ChassisSpeeds.h:134
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d &robotAngle)
Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds object.
Definition ChassisSpeeds.h:182
constexpr ChassisSpeeds operator*(double scalar) const
Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
Definition ChassisSpeeds.h:261
constexpr ChassisSpeeds operator/(double scalar) const
Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
Definition ChassisSpeeds.h:275
constexpr ChassisSpeeds operator-() const
Returns the inverse of the current ChassisSpeeds.
Definition ChassisSpeeds.h:249
constexpr bool operator==(const ChassisSpeeds &other) const =default
Compares the ChassisSpeeds with another ChassisSpeed.
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(const ChassisSpeeds &fieldRelativeSpeeds, const Rotation2d &robotAngle)
Converts a user provided field-relative ChassisSpeeds object into a robot-relative ChassisSpeeds obje...
Definition ChassisSpeeds.h:159
A change in distance along a 2D arc since the last pose update.
Definition Twist2d.h:22
Type representing an arbitrary unit.
Definition base.h:888