ChoreoLib
Choreo support library.
Loading...
Searching...
No Matches
SwerveSample.h
1// Copyright (c) Choreo contributors
2
3#pragma once
4
5#include <algorithm>
6#include <array>
7#include <type_traits>
8
9#include <frc/kinematics/ChassisSpeeds.h>
10#include <units/acceleration.h>
11#include <units/angle.h>
12#include <units/angular_acceleration.h>
13#include <units/angular_velocity.h>
14#include <units/force.h>
15#include <units/length.h>
16#include <units/time.h>
17#include <units/velocity.h>
18#include <wpi/MathExtras.h>
19#include <wpi/json_fwd.h>
20
21#include "choreo/util/AllianceFlipperUtil.h"
22
23namespace choreo {
24
29 public:
33 constexpr SwerveSample() = default;
34
54 constexpr SwerveSample(units::second_t timestamp, units::meter_t x,
55 units::meter_t y, units::radian_t heading,
56 units::meters_per_second_t vx,
57 units::meters_per_second_t vy,
58 units::radians_per_second_t omega,
59 units::meters_per_second_squared_t ax,
60 units::meters_per_second_squared_t ay,
61 units::radians_per_second_squared_t alpha,
62 std::array<units::newton_t, 4> moduleForcesX,
63 std::array<units::newton_t, 4> moduleForcesY)
65 x{x},
66 y{y},
68 vx{vx},
69 vy{vy},
70 omega{omega},
71 ax{ax},
72 ay{ay},
73 alpha{alpha},
76
82 constexpr units::second_t GetTimestamp() const { return timestamp; }
83
89 constexpr frc::Pose2d GetPose() const {
90 return frc::Pose2d{x, y, frc::Rotation2d{heading}};
91 }
92
98 constexpr frc::ChassisSpeeds GetChassisSpeeds() const {
99 return frc::ChassisSpeeds{vx, vy, omega};
100 }
101
108 template <int Year = util::kDefaultYear>
109 constexpr SwerveSample Flipped() const {
110 constexpr auto flipper = choreo::util::GetFlipperForYear<Year>();
111 if constexpr (flipper.isMirrored) {
112 return SwerveSample{timestamp,
113 flipper.FlipX(x),
114 flipper.FlipY(y),
115 flipper.FlipHeading(heading),
116 -vx,
117 vy,
118 -omega,
119 -ax,
120 ay,
121 -alpha,
122 // FL, FR, BL, BR
123 // Mirrored
124 // -FR, -FL, -BR, -BL
127 // FL, FR, BL, BR
128 // Mirrored
129 // -FR, -FL, -BR, -BL
131 moduleForcesY[2]}};
132 } else {
133 return SwerveSample{timestamp,
134 flipper.FlipX(x),
135 flipper.FlipY(y),
136 flipper.FlipHeading(heading),
137 -vx,
138 -vy,
139 omega,
140 -ax,
141 -ay,
142 alpha,
146 -moduleForcesY[2], -moduleForcesY[3]}};
147 }
148 }
149
156 constexpr SwerveSample OffsetBy(units::second_t timeStampOffset) const {
157 return SwerveSample{timestamp + timeStampOffset,
158 x,
159 y,
160 heading,
161 vx,
162 vy,
163 omega,
164 ax,
165 ay,
166 alpha,
169 }
170
178 constexpr SwerveSample Interpolate(const SwerveSample& endValue,
179 units::second_t t) const {
180 units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp);
181 frc::Pose2d interpolatedPose =
182 GetPose().Exp(GetPose().Log(endValue.GetPose()) * scale.value());
183
184 std::array<units::newton_t, 4> interpolatedForcesX;
185 std::array<units::newton_t, 4> interpolatedForcesY;
186 for (int i = 0; i < 4; i++) {
187 interpolatedForcesX[i] =
188 wpi::Lerp(moduleForcesX[i], endValue.moduleForcesX[i], scale.value());
189 interpolatedForcesY[i] =
190 wpi::Lerp(moduleForcesY[i], endValue.moduleForcesY[i], scale.value());
191 }
192
193 return SwerveSample{wpi::Lerp(timestamp, endValue.timestamp, scale),
194 interpolatedPose.X(),
195 interpolatedPose.Y(),
196 interpolatedPose.Rotation().Radians(),
197 wpi::Lerp(vx, endValue.vx, scale),
198 wpi::Lerp(vy, endValue.vy, scale),
199 wpi::Lerp(omega, endValue.omega, scale),
200 wpi::Lerp(ax, endValue.ax, scale),
201 wpi::Lerp(ay, endValue.ay, scale),
202 wpi::Lerp(alpha, endValue.alpha, scale),
203 interpolatedForcesX,
204 interpolatedForcesY};
205 }
206
213 constexpr bool operator==(const SwerveSample& other) const {
214 constexpr double epsilon = 1e-6;
215
216 auto compare_units = [epsilon](const auto& a, const auto& b) {
217 using UnitType =
218 std::remove_const_t<std::remove_reference_t<decltype(a)>>;
219 return units::math::abs(a - b) < UnitType(epsilon);
220 };
221
222 auto compare_arrays = [&compare_units](const auto& arr1, const auto& arr2) {
223 return std::equal(arr1.begin(), arr1.end(), arr2.begin(), compare_units);
224 };
225
226 return compare_units(timestamp, other.timestamp) &&
227 compare_units(x, other.x) && compare_units(y, other.y) &&
228 compare_units(heading, other.heading) &&
229 compare_units(vx, other.vx) && compare_units(vy, other.vy) &&
230 compare_units(omega, other.omega) && compare_units(ax, other.ax) &&
231 compare_units(ay, other.ay) && compare_units(alpha, other.alpha) &&
232 compare_arrays(moduleForcesX, other.moduleForcesX) &&
233 compare_arrays(moduleForcesY, other.moduleForcesY);
234 }
235
237 units::second_t timestamp = 0_s;
238
240 units::meter_t x = 0_m;
241
243 units::meter_t y = 0_m;
244
246 units::radian_t heading = 0_rad;
247
249 units::meters_per_second_t vx = 0_mps;
250
252 units::meters_per_second_t vy = 0_mps;
253
255 units::radians_per_second_t omega = 0_rad_per_s;
256
258 units::meters_per_second_squared_t ax = 0_mps_sq;
259
261 units::meters_per_second_squared_t ay = 0_mps_sq;
262
264 units::radians_per_second_squared_t alpha = 0_rad_per_s_sq;
265
268 std::array<units::newton_t, 4> moduleForcesX{0_N, 0_N, 0_N, 0_N};
269
272 std::array<units::newton_t, 4> moduleForcesY{0_N, 0_N, 0_N, 0_N};
273};
274
275void to_json(wpi::json& json, const SwerveSample& trajectorySample);
276void from_json(const wpi::json& json, SwerveSample& trajectorySample);
277
278} // namespace choreo
279
280#include "choreo/trajectory/struct/SwerveSampleStruct.h"
Definition SwerveSample.h:28
units::meter_t y
The Y position of the sample relative to the blue alliance wall origin.
Definition SwerveSample.h:243
units::meters_per_second_t vy
The velocity of the sample in the Y direction.
Definition SwerveSample.h:252
constexpr SwerveSample Interpolate(const SwerveSample &endValue, units::second_t t) const
Definition SwerveSample.h:178
constexpr units::second_t GetTimestamp() const
Definition SwerveSample.h:82
units::meters_per_second_t vx
The velocity of the sample in the X direction.
Definition SwerveSample.h:249
units::radians_per_second_t omega
The angular velocity of the sample.
Definition SwerveSample.h:255
units::meters_per_second_squared_t ax
The acceleration of the in the X direction.
Definition SwerveSample.h:258
units::meter_t x
The X position of the sample relative to the blue alliance wall origin.
Definition SwerveSample.h:240
constexpr SwerveSample OffsetBy(units::second_t timeStampOffset) const
Definition SwerveSample.h:156
constexpr bool operator==(const SwerveSample &other) const
Definition SwerveSample.h:213
constexpr frc::ChassisSpeeds GetChassisSpeeds() const
Definition SwerveSample.h:98
constexpr frc::Pose2d GetPose() const
Definition SwerveSample.h:89
constexpr SwerveSample()=default
units::radian_t heading
The heading of the sample, with 0 being in the +X direction.
Definition SwerveSample.h:246
units::second_t timestamp
The timestamp of this sample relative to the beginning of the trajectory.
Definition SwerveSample.h:237
constexpr SwerveSample Flipped() const
Definition SwerveSample.h:109
units::meters_per_second_squared_t ay
The acceleration of the in the Y direction.
Definition SwerveSample.h:261
units::radians_per_second_squared_t alpha
The angular acceleration of the sample.
Definition SwerveSample.h:264
std::array< units::newton_t, 4 > moduleForcesY
Definition SwerveSample.h:272
std::array< units::newton_t, 4 > moduleForcesX
Definition SwerveSample.h:268
constexpr SwerveSample(units::second_t timestamp, units::meter_t x, units::meter_t y, units::radian_t heading, units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, units::meters_per_second_squared_t ax, units::meters_per_second_squared_t ay, units::radians_per_second_squared_t alpha, std::array< units::newton_t, 4 > moduleForcesX, std::array< units::newton_t, 4 > moduleForcesY)
Definition SwerveSample.h:54