WPILibC++ 2025.3.1
Loading...
Searching...
No Matches
Pose3d.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 <stdexcept>
8#include <type_traits>
9#include <utility>
10
11#include <Eigen/Core>
12#include <wpi/SymbolExports.h>
13#include <wpi/json_fwd.h>
14
15#include "frc/ct_matrix.h"
16#include "frc/geometry/Pose2d.h"
20
21namespace frc {
22
23class Transform3d;
24
25/**
26 * Represents a 3D pose containing translational and rotational elements.
27 */
29 public:
30 /**
31 * Constructs a pose at the origin facing toward the positive X axis.
32 */
33 constexpr Pose3d() = default;
34
35 /**
36 * Constructs a pose with the specified translation and rotation.
37 *
38 * @param translation The translational component of the pose.
39 * @param rotation The rotational component of the pose.
40 */
41 constexpr Pose3d(Translation3d translation, Rotation3d rotation)
42 : m_translation{std::move(translation)},
43 m_rotation{std::move(rotation)} {}
44
45 /**
46 * Constructs a pose with x, y, and z translations instead of a separate
47 * Translation3d.
48 *
49 * @param x The x component of the translational component of the pose.
50 * @param y The y component of the translational component of the pose.
51 * @param z The z component of the translational component of the pose.
52 * @param rotation The rotational component of the pose.
53 */
54 constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
55 Rotation3d rotation)
56 : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
57
58 /**
59 * Constructs a pose with the specified affine transformation matrix.
60 *
61 * @param matrix The affine transformation matrix.
62 * @throws std::domain_error if the affine transformation matrix is invalid.
63 */
64 constexpr explicit Pose3d(const Eigen::Matrix4d& matrix)
65 : m_translation{Eigen::Vector3d{
66 {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
67 m_rotation{
68 Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
69 {matrix(1, 0), matrix(1, 1), matrix(1, 2)},
70 {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
71 if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
72 matrix(3, 3) != 1.0) {
73 throw std::domain_error("Affine transformation matrix is invalid");
74 }
75 }
76
77 /**
78 * Constructs a 3D pose from a 2D pose in the X-Y plane.
79 *
80 * @param pose The 2D pose.
81 * @see Rotation3d(Rotation2d)
82 * @see Translation3d(Translation2d)
83 */
84 constexpr explicit Pose3d(const Pose2d& pose)
85 : m_translation{pose.X(), pose.Y(), 0_m},
86 m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
87
88 /**
89 * Transforms the pose by the given transformation and returns the new
90 * transformed pose. The transform is applied relative to the pose's frame.
91 * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
92 * applied relative to the global frame and around the origin.
93 *
94 * @param other The transform to transform the pose by.
95 *
96 * @return The transformed pose.
97 */
98 constexpr Pose3d operator+(const Transform3d& other) const {
99 return TransformBy(other);
100 }
101
102 /**
103 * Returns the Transform3d that maps the one pose to another.
104 *
105 * @param other The initial pose of the transformation.
106 * @return The transform that maps the other pose to the current pose.
107 */
108 constexpr Transform3d operator-(const Pose3d& other) const;
109
110 /**
111 * Checks equality between this Pose3d and another object.
112 */
113 constexpr bool operator==(const Pose3d&) const = default;
114
115 /**
116 * Returns the underlying translation.
117 *
118 * @return Reference to the translational component of the pose.
119 */
120 constexpr const Translation3d& Translation() const { return m_translation; }
121
122 /**
123 * Returns the X component of the pose's translation.
124 *
125 * @return The x component of the pose's translation.
126 */
127 constexpr units::meter_t X() const { return m_translation.X(); }
128
129 /**
130 * Returns the Y component of the pose's translation.
131 *
132 * @return The y component of the pose's translation.
133 */
134 constexpr units::meter_t Y() const { return m_translation.Y(); }
135
136 /**
137 * Returns the Z component of the pose's translation.
138 *
139 * @return The z component of the pose's translation.
140 */
141 constexpr units::meter_t Z() const { return m_translation.Z(); }
142
143 /**
144 * Returns the underlying rotation.
145 *
146 * @return Reference to the rotational component of the pose.
147 */
148 constexpr const Rotation3d& Rotation() const { return m_rotation; }
149
150 /**
151 * Multiplies the current pose by a scalar.
152 *
153 * @param scalar The scalar.
154 *
155 * @return The new scaled Pose2d.
156 */
157 constexpr Pose3d operator*(double scalar) const {
158 return Pose3d{m_translation * scalar, m_rotation * scalar};
159 }
160
161 /**
162 * Divides the current pose by a scalar.
163 *
164 * @param scalar The scalar.
165 *
166 * @return The new scaled Pose2d.
167 */
168 constexpr Pose3d operator/(double scalar) const {
169 return *this * (1.0 / scalar);
170 }
171
172 /**
173 * Rotates the pose around the origin and returns the new pose.
174 *
175 * @param other The rotation to transform the pose by, which is applied
176 * extrinsically (from the global frame).
177 *
178 * @return The rotated pose.
179 */
180 constexpr Pose3d RotateBy(const Rotation3d& other) const {
181 return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
182 }
183
184 /**
185 * Transforms the pose by the given transformation and returns the new
186 * transformed pose. The transform is applied relative to the pose's frame.
187 * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
188 * applied relative to the global frame and around the origin.
189 *
190 * @param other The transform to transform the pose by.
191 *
192 * @return The transformed pose.
193 */
194 constexpr Pose3d TransformBy(const Transform3d& other) const;
195
196 /**
197 * Returns the current pose relative to the given pose.
198 *
199 * This function can often be used for trajectory tracking or pose
200 * stabilization algorithms to get the error between the reference and the
201 * current pose.
202 *
203 * @param other The pose that is the origin of the new coordinate frame that
204 * the current pose will be converted into.
205 *
206 * @return The current pose relative to the new origin pose.
207 */
208 constexpr Pose3d RelativeTo(const Pose3d& other) const;
209
210 /**
211 * Rotates the current pose around a point in 3D space.
212 *
213 * @param point The point in 3D space to rotate around.
214 * @param rot The rotation to rotate the pose by.
215 *
216 * @return The new rotated pose.
217 */
218 constexpr Pose3d RotateAround(const Translation3d& point,
219 const Rotation3d& rot) const {
220 return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
221 }
222
223 /**
224 * Obtain a new Pose3d from a (constant curvature) velocity.
225 *
226 * The twist is a change in pose in the robot's coordinate frame since the
227 * previous pose update. When the user runs exp() on the previous known
228 * field-relative pose with the argument being the twist, the user will
229 * receive the new field-relative pose.
230 *
231 * "Exp" represents the pose exponential, which is solving a differential
232 * equation moving the pose forward in time.
233 *
234 * @param twist The change in pose in the robot's coordinate frame since the
235 * previous pose update. For example, if a non-holonomic robot moves forward
236 * 0.01 meters and changes angle by 0.5 degrees since the previous pose
237 * update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
238 * 0.5_deg}}.
239 *
240 * @return The new pose of the robot.
241 */
242 constexpr Pose3d Exp(const Twist3d& twist) const;
243
244 /**
245 * Returns a Twist3d that maps this pose to the end pose. If c is the output
246 * of a.Log(b), then a.Exp(c) would yield b.
247 *
248 * @param end The end pose for the transformation.
249 *
250 * @return The twist that maps this to end.
251 */
252 constexpr Twist3d Log(const Pose3d& end) const;
253
254 /**
255 * Returns an affine transformation matrix representation of this pose.
256 */
257 constexpr Eigen::Matrix4d ToMatrix() const {
258 auto vec = m_translation.ToVector();
259 auto mat = m_rotation.ToMatrix();
260 return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
261 {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
262 {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
263 {0.0, 0.0, 0.0, 1.0}};
264 }
265
266 /**
267 * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
268 */
269 constexpr Pose2d ToPose2d() const {
270 return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
271 }
272
273 private:
274 Translation3d m_translation;
275 Rotation3d m_rotation;
276};
277
279void to_json(wpi::json& json, const Pose3d& pose);
280
282void from_json(const wpi::json& json, Pose3d& pose);
283
284} // namespace frc
285
288
290
291namespace frc {
292
293namespace detail {
294
295/**
296 * Applies the hat operator to a rotation vector.
297 *
298 * It takes a rotation vector and returns the corresponding matrix
299 * representation of the Lie algebra element (a 3x3 rotation matrix).
300 *
301 * @param rotation The rotation vector.
302 * @return The rotation vector as a 3x3 rotation matrix.
303 */
305 // Given a rotation vector <a, b, c>,
306 // [ 0 -c b]
307 // Omega = [ c 0 -a]
308 // [-b a 0]
309 return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
310 {rotation(2), 0.0, -rotation(0)},
311 {-rotation(1), rotation(0), 0.0}};
312}
313
314/**
315 * Applies the hat operator to a rotation vector.
316 *
317 * It takes a rotation vector and returns the corresponding matrix
318 * representation of the Lie algebra element (a 3x3 rotation matrix).
319 *
320 * @param rotation The rotation vector.
321 * @return The rotation vector as a 3x3 rotation matrix.
322 */
323constexpr Eigen::Matrix3d RotationVectorToMatrix(
324 const Eigen::Vector3d& rotation) {
325 // Given a rotation vector <a, b, c>,
326 // [ 0 -c b]
327 // Omega = [ c 0 -a]
328 // [-b a 0]
329 return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
330 {rotation(2), 0.0, -rotation(0)},
331 {-rotation(1), rotation(0), 0.0}};
332}
333
334} // namespace detail
335
336constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
337 const auto pose = this->RelativeTo(other);
338 return Transform3d{pose.Translation(), pose.Rotation()};
339}
340
341constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
342 return {m_translation + (other.Translation().RotateBy(m_rotation)),
343 other.Rotation() + m_rotation};
344}
345
346constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
347 const Transform3d transform{other, *this};
348 return {transform.Translation(), transform.Rotation()};
349}
350
351constexpr Pose3d Pose3d::Exp(const Twist3d& twist) const {
352 // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
353
354 auto impl = [this]<typename Matrix3d, typename Vector3d>(
355 const Twist3d& twist) -> Pose3d {
356 Vector3d u{{twist.dx.value(), twist.dy.value(), twist.dz.value()}};
357 Vector3d rvec{{twist.rx.value(), twist.ry.value(), twist.rz.value()}};
358 Matrix3d omega = detail::RotationVectorToMatrix(rvec);
359 Matrix3d omegaSq = omega * omega;
360 double theta = rvec.norm();
361 double thetaSq = theta * theta;
362
363 double A;
364 double B;
365 double C;
366 if (gcem::abs(theta) < 1E-7) {
367 // Taylor Expansions around θ = 0
368 // A = 1/1! - θ²/3! + θ⁴/5!
369 // B = 1/2! - θ²/4! + θ⁴/6!
370 // C = 1/3! - θ²/5! + θ⁴/7!
371 // sources:
372 // A:
373 // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
374 // B:
375 // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
376 // C:
377 // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
378 A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
379 B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
380 C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
381 } else {
382 // A = std::sin(θ)/θ
383 // B = (1 - std::cos(θ)) / θ²
384 // C = (1 - A) / θ²
385 A = gcem::sin(theta) / theta;
386 B = (1 - gcem::cos(theta)) / thetaSq;
387 C = (1 - A) / thetaSq;
388 }
389
390 Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq;
391 Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq;
392
393 Vector3d translation_component = V * u;
394
395 const Transform3d transform{
396 Translation3d{units::meter_t{translation_component(0)},
397 units::meter_t{translation_component(1)},
398 units::meter_t{translation_component(2)}},
399 Rotation3d{R}};
400
401 return *this + transform;
402 };
403
404 if (std::is_constant_evaluated()) {
405 return impl.template operator()<ct_matrix3d, ct_vector3d>(twist);
406 } else {
407 return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(twist);
408 }
409}
410
411constexpr Twist3d Pose3d::Log(const Pose3d& end) const {
412 // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
413
414 auto impl = [this]<typename Matrix3d, typename Vector3d>(
415 const Pose3d& end) -> Twist3d {
416 const auto transform = end.RelativeTo(*this);
417
418 Vector3d u{
419 {transform.X().value(), transform.Y().value(), transform.Z().value()}};
420 Vector3d rvec = transform.Rotation().ToVector();
421 Matrix3d omega = detail::RotationVectorToMatrix(rvec);
422 Matrix3d omegaSq = omega * omega;
423 double theta = rvec.norm();
424 double thetaSq = theta * theta;
425
426 double C;
427 if (gcem::abs(theta) < 1E-7) {
428 // Taylor Expansions around θ = 0
429 // A = 1/1! - θ²/3! + θ⁴/5!
430 // B = 1/2! - θ²/4! + θ⁴/6!
431 // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
432 // sources:
433 // A:
434 // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
435 // B:
436 // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
437 // C:
438 // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
439 C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
440 } else {
441 // A = std::sin(θ)/θ
442 // B = (1 - std::cos(θ)) / θ²
443 // C = (1 - A/(2*B)) / θ²
444 double A = gcem::sin(theta) / theta;
445 double B = (1 - gcem::cos(theta)) / thetaSq;
446 C = (1 - A / (2 * B)) / thetaSq;
447 }
448
449 Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
450
451 Vector3d translation_component = V_inv * u;
452
453 return Twist3d{units::meter_t{translation_component(0)},
454 units::meter_t{translation_component(1)},
455 units::meter_t{translation_component(2)},
456 units::radian_t{rvec(0)},
457 units::radian_t{rvec(1)},
458 units::radian_t{rvec(2)}};
459 };
460
461 if (std::is_constant_evaluated()) {
462 return impl.template operator()<ct_matrix3d, ct_vector3d>(end);
463 } else {
464 return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(end);
465 }
466}
467
468} // namespace frc
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
namespace for Niels Lohmann
Definition json.h:99
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
Represents a 3D pose containing translational and rotational elements.
Definition Pose3d.h:28
constexpr Pose3d(const Pose2d &pose)
Constructs a 3D pose from a 2D pose in the X-Y plane.
Definition Pose3d.h:84
constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, Rotation3d rotation)
Constructs a pose with x, y, and z translations instead of a separate Translation3d.
Definition Pose3d.h:54
constexpr Pose3d TransformBy(const Transform3d &other) const
Transforms the pose by the given transformation and returns the new transformed pose.
Definition Pose3d.h:341
constexpr Pose3d RotateBy(const Rotation3d &other) const
Rotates the pose around the origin and returns the new pose.
Definition Pose3d.h:180
constexpr const Translation3d & Translation() const
Returns the underlying translation.
Definition Pose3d.h:120
constexpr Pose3d(Translation3d translation, Rotation3d rotation)
Constructs a pose with the specified translation and rotation.
Definition Pose3d.h:41
constexpr Twist3d Log(const Pose3d &end) const
Returns a Twist3d that maps this pose to the end pose.
Definition Pose3d.h:411
constexpr units::meter_t Z() const
Returns the Z component of the pose's translation.
Definition Pose3d.h:141
constexpr Pose3d operator/(double scalar) const
Divides the current pose by a scalar.
Definition Pose3d.h:168
constexpr Eigen::Matrix4d ToMatrix() const
Returns an affine transformation matrix representation of this pose.
Definition Pose3d.h:257
constexpr Pose3d RotateAround(const Translation3d &point, const Rotation3d &rot) const
Rotates the current pose around a point in 3D space.
Definition Pose3d.h:218
constexpr Pose2d ToPose2d() const
Returns a Pose2d representing this Pose3d projected into the X-Y plane.
Definition Pose3d.h:269
constexpr const Rotation3d & Rotation() const
Returns the underlying rotation.
Definition Pose3d.h:148
constexpr units::meter_t Y() const
Returns the Y component of the pose's translation.
Definition Pose3d.h:134
constexpr Pose3d operator*(double scalar) const
Multiplies the current pose by a scalar.
Definition Pose3d.h:157
constexpr Pose3d RelativeTo(const Pose3d &other) const
Returns the current pose relative to the given pose.
Definition Pose3d.h:346
constexpr Pose3d(const Eigen::Matrix4d &matrix)
Constructs a pose with the specified affine transformation matrix.
Definition Pose3d.h:64
constexpr bool operator==(const Pose3d &) const =default
Checks equality between this Pose3d and another object.
constexpr Transform3d operator-(const Pose3d &other) const
Returns the Transform3d that maps the one pose to another.
Definition Pose3d.h:336
constexpr Pose3d operator+(const Transform3d &other) const
Transforms the pose by the given transformation and returns the new transformed pose.
Definition Pose3d.h:98
constexpr Pose3d()=default
Constructs a pose at the origin facing toward the positive X axis.
constexpr Pose3d Exp(const Twist3d &twist) const
Obtain a new Pose3d from a (constant curvature) velocity.
Definition Pose3d.h:351
constexpr units::meter_t X() const
Returns the X component of the pose's translation.
Definition Pose3d.h:127
A rotation in a 3D coordinate frame represented by a quaternion.
Definition Rotation3d.h:29
Represents a transformation for a Pose3d in the pose's frame.
Definition Transform3d.h:21
Represents a translation in 3D space.
Definition Translation3d.h:26
Compile-time wrapper for Eigen::Matrix.
Definition ct_matrix.h:26
Definition Variable.hpp:739
detail namespace with internal helper functions
Definition input_adapters.h:32
constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d &rotation)
Applies the hat operator to a rotation vector.
Definition Pose3d.h:304
Definition CAN.h:11
WPILIB_DLLEXPORT void to_json(wpi::json &json, const Rotation3d &rotation)
ct_matrix< double, 3, 3 > ct_matrix3d
Definition ct_matrix.h:385
constexpr T abs(const T x) noexcept
Compile-time absolute value function.
Definition abs.hpp:40
constexpr return_t< T > cos(const T x) noexcept
Compile-time cosine function.
Definition cos.hpp:83
constexpr return_t< T > sin(const T x) noexcept
Compile-time sine function.
Definition sin.hpp:85
Implement std::hash so that hash_code can be used in STL containers.
Definition PointerIntPair.h:280
A change in distance along a 3D arc since the last pose update.
Definition Twist3d.h:22
units::meter_t dx
Linear "dx" component.
Definition Twist3d.h:26
units::radian_t ry
Rotation vector y component.
Definition Twist3d.h:46
units::meter_t dy
Linear "dy" component.
Definition Twist3d.h:31
units::radian_t rz
Rotation vector z component.
Definition Twist3d.h:51
units::meter_t dz
Linear "dz" component.
Definition Twist3d.h:36
units::radian_t rx
Rotation vector x component.
Definition Twist3d.h:41
Type representing an arbitrary unit.
Definition base.h:888