CTRE Phoenix 6 C++ 24.50.0-alpha-2
SwerveDrivePoseEstimator.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
12#include <array>
13
14namespace ctre {
15namespace phoenix6 {
16namespace swerve {
17namespace impl {
18
19/**
20 * \brief Class for swerve drive odometry. Odometry allows you to track the robot's
21 * position on the field over a course of a match using readings from your
22 * swerve drive encoders and swerve azimuth encoders.
23 *
24 * Teams can use odometry during the autonomous period for complex tasks like
25 * path following. Furthermore, odometry can be used for latency compensation
26 * when using computer-vision systems.
27 */
29public:
30 using WheelSpeeds = std::vector<SwerveModuleState>;
31 using WheelPositions = std::vector<SwerveModulePosition>;
32
33private:
34 SwerveDriveKinematics const &m_kinematics;
35 Pose2d m_pose;
36
37 WheelPositions m_previousWheelPositions;
38 Rotation2d m_previousAngle;
39 Rotation2d m_gyroOffset;
40
41public:
42 /**
43 * \brief Constructs a SwerveDriveOdometry object.
44 *
45 * \param kinematics The swerve drive kinematics for your drivetrain.
46 * \param gyroAngle The angle reported by the gyroscope.
47 * \param modulePositions The wheel positions reported by each module.
48 * \param initialPose The starting position of the robot on the field.
49 */
50 SwerveDriveOdometry(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose = Pose2d{}) :
51 m_kinematics{kinematics},
52 m_pose{std::move(initialPose)},
53 m_previousWheelPositions{std::move(modulePositions)},
54 m_previousAngle{m_pose.Rotation()},
55 m_gyroOffset{m_pose.Rotation() - gyroAngle}
56 {}
57
58 /**
59 * \brief Resets the robot's position on the field.
60 *
61 * The gyroscope angle does not need to be reset here on the user's robot
62 * code. The library automatically takes care of offsetting the gyro angle.
63 *
64 * \param gyroAngle The angle reported by the gyroscope.
65 * \param wheelPositions The current distances measured by each wheel.
66 * \param pose The position on the field that your robot is at.
67 */
68 void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
69 {
70 m_pose = pose;
71 m_previousAngle = m_pose.Rotation();
72 m_gyroOffset = m_pose.Rotation() - gyroAngle;
73 m_previousWheelPositions = std::move(wheelPositions);
74 }
75
76 /**
77 * \brief Returns the position of the robot on the field.
78 *
79 * \returns The pose of the robot.
80 */
81 Pose2d const &Pose() const { return m_pose; }
82
83 /**
84 * \brief Updates the robot's position on the field using forward kinematics and
85 * integration of the pose over time. This method takes in an angle parameter
86 * which is used instead of the angular rate that is calculated from forward
87 * kinematics, in addition to the current distance measurement at each wheel.
88 *
89 * \param gyroAngle The angle reported by the gyroscope.
90 * \param wheelPositions The current distances measured by each wheel.
91 *
92 * \returns The new pose of the robot.
93 */
94 Pose2d const &Update(Rotation2d const &gyroAngle, WheelPositions wheelPositions);
95};
96
97/**
98 * \brief This class wraps Swerve Drive Odometry to fuse latency-compensated
99 * vision measurements with swerve drive encoder distance measurements. It is
100 * intended to be a drop-in for SwerveDriveOdometry.
101 *
102 * Update() should be called every robot loop.
103 *
104 * AddVisionMeasurement() can be called as infrequently as you want; if you
105 * never call it, then this class will behave as regular encoder odometry.
106 */
108public:
109 using WheelSpeeds = std::vector<SwerveModuleState>;
110 using WheelPositions = std::vector<SwerveModulePosition>;
111
112private:
113 struct InterpolationRecord {
114 // The pose observed given the current sensor inputs and the previous pose.
115 Pose2d pose;
116 // The current gyroscope angle.
117 Rotation2d gyroAngle;
118 // The distances traveled by the wheels.
119 WheelPositions wheelPositions;
120
121 /**
122 * Interpolates between two InterpolationRecords.
123 *
124 * \param endValue The end value for the interpolation.
125 * \param i The interpolant (fraction).
126 *
127 * \returns The interpolated state.
128 */
129 InterpolationRecord Interpolate(SwerveDriveKinematics const &kinematics, InterpolationRecord endValue, double i) const;
130
131 bool operator==(InterpolationRecord const &other) const
132 {
133 return pose == other.pose &&
134 gyroAngle == other.gyroAngle &&
135 wheelPositions == other.wheelPositions;
136 }
137 bool operator!=(InterpolationRecord const &other) const
138 {
139 return !(*this == other);
140 }
141 };
142
143 static constexpr units::second_t kBufferDuration = 1.5_s;
144
145 SwerveDriveKinematics const &m_kinematics;
146 SwerveDriveOdometry m_odometry;
147 std::array<double, 3> m_q{};
148
149 struct VisionMatrices;
150 std::unique_ptr<VisionMatrices> m_matrices;
151
153 kBufferDuration, [this](auto const &start, auto const &end, double t) {
154 return start.Interpolate(this->m_kinematics, end, t);
155 }
156 };
157
158public:
159 /**
160 * \brief Constructs a SwerveDrivePoseEstimator with default standard deviations
161 * for the model and vision measurements.
162 *
163 * The default standard deviations of the model states are
164 * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
165 * The default standard deviations of the vision measurements are
166 * 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.
167 *
168 * \param kinematics A correctly-configured kinematics object for your
169 * drivetrain.
170 * \param gyroAngle The current gyro angle.
171 * \param modulePositions The current distance and rotation measurements of
172 * the swerve modules.
173 * \param initialPose The starting pose estimate.
174 */
175 SwerveDrivePoseEstimator(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle,
176 WheelPositions modulePositions, Pose2d initialPose) :
177 SwerveDrivePoseEstimator{kinematics, gyroAngle, std::move(modulePositions), std::move(initialPose),
178 {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}}
179 {}
180
181 /**
182 * \brief Constructs a SwerveDrivePoseEstimator.
183 *
184 * \param kinematics A correctly-configured kinematics object for your
185 * drivetrain.
186 * \param gyroAngle The current gyro angle.
187 * \param modulePositions The current distance and rotation measurements of
188 * the swerve modules.
189 * \param initialPose The starting pose estimate.
190 * \param stateStdDevs Standard deviations of the pose estimate (x position in
191 * meters, y position in meters, and heading in radians). Increase these
192 * numbers to trust your state estimate less.
193 * \param visionMeasurementStdDevs Standard deviations of the vision pose
194 * measurement (x position in meters, y position in meters, and heading in
195 * radians). Increase these numbers to trust the vision pose measurement
196 * less.
197 */
198 SwerveDrivePoseEstimator(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle,
199 WheelPositions modulePositions, Pose2d initialPose,
200 std::array<double, 3> const &stateStdDevs, std::array<double, 3> const &visionMeasurementStdDevs);
201
203
204 /**
205 * \brief Sets the pose estimator's trust in vision measurements. This might be used
206 * to change trust in vision measurements after the autonomous period, or to
207 * change trust as distance to a vision target increases.
208 *
209 * \param visionMeasurementStdDevs Standard deviations of the vision pose
210 * measurement (x position in meters, y position in meters, and heading in
211 * radians). Increase these numbers to trust the vision pose measurement
212 * less.
213 */
214 void SetVisionMeasurementStdDevs(std::array<double, 3> const &visionMeasurementStdDevs);
215
216 /**
217 * \brief Resets the robot's position on the field.
218 *
219 * The gyroscope angle does not need to be reset in the user's robot code.
220 * The library automatically takes care of offsetting the gyro angle.
221 *
222 * \param gyroAngle The current gyro angle.
223 * \param wheelPositions The distances traveled by the encoders.
224 * \param pose The estimated pose of the robot on the field.
225 */
226 void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
227 {
228 m_odometry.ResetPosition(gyroAngle, std::move(wheelPositions), pose);
229 m_poseBuffer.Clear();
230 }
231
232 /**
233 * \brief Gets the estimated robot pose.
234 *
235 * \returns The estimated robot pose in meters.
236 */
237 Pose2d GetEstimatedPosition() const
238 {
239 return m_odometry.Pose();
240 }
241
242 /**
243 * \brief Return the pose at a given timestamp, if the buffer is not empty.
244 *
245 * \param timestamp The pose's timestamp.
246 * \returns The pose at the given timestamp (or std::nullopt if the buffer is
247 * empty).
248 */
249 std::optional<Pose2d> SampleAt(units::second_t timestamp) const
250 {
251 std::optional<InterpolationRecord> const sample = m_poseBuffer.Sample(timestamp);
252 if (sample) {
253 return sample->pose;
254 } else {
255 return std::nullopt;
256 }
257 }
258
259 /**
260 * \brief Adds a vision measurement to the Kalman Filter. This will correct
261 * the odometry pose estimate while still accounting for measurement noise.
262 *
263 * This method can be called as infrequently as you want, as long as you are
264 * calling Update() every loop.
265 *
266 * To promote stability of the pose estimate and make it robust to bad vision
267 * data, we recommend only adding vision measurements that are already within
268 * one meter or so of the current pose estimate.
269 *
270 * \param visionRobotPose The pose of the robot as measured by the vision
271 * camera.
272 * \param timestamp The timestamp of the vision measurement in seconds. Note
273 * that if you don't use your own time source by calling UpdateWithTime(),
274 * then you must use a timestamp with an epoch since system startup (i.e.,
275 * the epoch of this timestamp is the same epoch as
276 * GetCurrentTimeSeconds(). This means that you should use
277 * GetCurrentTimeSeconds() as your time source in this case.
278 */
279 void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp);
280
281 /**
282 * \brief Adds a vision measurement to the Kalman Filter. This will correct
283 * the odometry pose estimate while still accounting for measurement noise.
284 *
285 * This method can be called as infrequently as you want, as long as you are
286 * calling Update() every loop.
287 *
288 * To promote stability of the pose estimate and make it robust to bad vision
289 * data, we recommend only adding vision measurements that are already within
290 * one meter or so of the current pose estimate.
291 *
292 * Note that the vision measurement standard deviations passed into this
293 * method will continue to apply to future measurements until a subsequent
294 * call to SetVisionMeasurementStdDevs() or this method.
295 *
296 * \param visionRobotPose The pose of the robot as measured by the vision
297 * camera.
298 * \param timestamp The timestamp of the vision measurement in seconds. Note
299 * that if you don't use your own time source by calling UpdateWithTime(),
300 * then you must use a timestamp with an epoch since system startup (i.e.,
301 * the epoch of this timestamp is the same epoch as
302 * GetCurrentTimeSeconds(). This means that you should use
303 * GetCurrentTimeSeconds() as your time source in this case.
304 * \param visionMeasurementStdDevs Standard deviations of the vision pose
305 * measurement (x position in meters, y position in meters, and heading in
306 * radians). Increase these numbers to trust the vision pose measurement
307 * less.
308 */
309 void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp,
310 std::array<double, 3> const &visionMeasurementStdDevs)
311 {
312 SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
313 AddVisionMeasurement(visionRobotPose, timestamp);
314 }
315
316 /**
317 * Updates the pose estimator with wheel encoder and gyro information. This
318 * should be called every loop.
319 *
320 * \param gyroAngle The current gyro angle.
321 * \param wheelPositions The distances traveled by the encoders.
322 *
323 * \returns The estimated pose of the robot in meters.
324 */
325 Pose2d Update(Rotation2d const &gyroAngle, WheelPositions const &wheelPositions)
326 {
327 return UpdateWithTime(units::second_t{GetCurrentTimeSeconds()}, gyroAngle, wheelPositions);
328 }
329
330 /**
331 * \brief Updates the pose estimator with wheel encoder and gyro information. This
332 * should be called every loop.
333 *
334 * \param currentTime The time at which this method was called.
335 * \param gyroAngle The current gyro angle.
336 * \param wheelPositions The distances traveled by the encoders.
337 *
338 * \returns The estimated pose of the robot in meters.
339 */
340 Pose2d UpdateWithTime(units::second_t currentTime, Rotation2d const &gyroAngle, WheelPositions const &wheelPositions)
341 {
342 m_odometry.Update(gyroAngle, wheelPositions);
343 m_poseBuffer.AddSample(
344 currentTime,
345 {GetEstimatedPosition(), gyroAngle, wheelPositions}
346 );
347 return GetEstimatedPosition();
348 }
349};
350
351}
352}
353}
354}
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.hpp:235
Class for swerve drive odometry.
Definition: SwerveDrivePoseEstimator.hpp:28
Pose2d const & Pose() const
Returns the position of the robot on the field.
Definition: SwerveDrivePoseEstimator.hpp:81
void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
Resets the robot's position on the field.
Definition: SwerveDrivePoseEstimator.hpp:68
std::vector< SwerveModulePosition > WheelPositions
Definition: SwerveDrivePoseEstimator.hpp:31
Pose2d const & Update(Rotation2d const &gyroAngle, WheelPositions wheelPositions)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
std::vector< SwerveModuleState > WheelSpeeds
Definition: SwerveDrivePoseEstimator.hpp:30
SwerveDriveOdometry(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose=Pose2d{})
Constructs a SwerveDriveOdometry object.
Definition: SwerveDrivePoseEstimator.hpp:50
This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve dr...
Definition: SwerveDrivePoseEstimator.hpp:107
Pose2d UpdateWithTime(units::second_t currentTime, Rotation2d const &gyroAngle, WheelPositions const &wheelPositions)
Updates the pose estimator with wheel encoder and gyro information.
Definition: SwerveDrivePoseEstimator.hpp:340
std::vector< SwerveModulePosition > WheelPositions
Definition: SwerveDrivePoseEstimator.hpp:110
void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
Resets the robot's position on the field.
Definition: SwerveDrivePoseEstimator.hpp:226
void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp)
Adds a vision measurement to the Kalman Filter.
SwerveDrivePoseEstimator(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose, std::array< double, 3 > const &stateStdDevs, std::array< double, 3 > const &visionMeasurementStdDevs)
Constructs a SwerveDrivePoseEstimator.
Pose2d GetEstimatedPosition() const
Gets the estimated robot pose.
Definition: SwerveDrivePoseEstimator.hpp:237
std::vector< SwerveModuleState > WheelSpeeds
Definition: SwerveDrivePoseEstimator.hpp:109
SwerveDrivePoseEstimator(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose)
Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measu...
Definition: SwerveDrivePoseEstimator.hpp:175
std::optional< Pose2d > SampleAt(units::second_t timestamp) const
Return the pose at a given timestamp, if the buffer is not empty.
Definition: SwerveDrivePoseEstimator.hpp:249
void SetVisionMeasurementStdDevs(std::array< double, 3 > const &visionMeasurementStdDevs)
Sets the pose estimator's trust in vision measurements.
Pose2d Update(Rotation2d const &gyroAngle, WheelPositions const &wheelPositions)
Updates the pose estimator with wheel encoder and gyro information.
Definition: SwerveDrivePoseEstimator.hpp:325
void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp, std::array< double, 3 > const &visionMeasurementStdDevs)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivePoseEstimator.hpp:309
CTREXPORT double GetCurrentTimeSeconds()
Get the current timestamp in seconds.
Represents the state of one swerve module.
Definition: StatusCodes.h:18
Definition: span.hpp:401