42 : m_translation{
std::move(translation)},
43 m_rotation{
std::move(rotation)} {}
54 constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
56 : m_translation{x, y, z}, m_rotation{
std::move(rotation)} {}
64 constexpr explicit Pose3d(
const Eigen::Matrix4d& matrix)
65 : m_translation{
Eigen::Vector3d{
66 {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
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");
85 : m_translation{pose.X(), pose.Y(), 0_m},
86 m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
99 return TransformBy(other);
127 constexpr units::meter_t
X()
const {
return m_translation.X(); }
134 constexpr units::meter_t
Y()
const {
return m_translation.Y(); }
141 constexpr units::meter_t
Z()
const {
return m_translation.Z(); }
169 return *
this * (1.0 /
scalar);
181 return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
245 auto vec = m_translation.ToVector();
246 auto mat = m_rotation.ToMatrix();
247 return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
248 {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
249 {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
250 {0.0, 0.0, 0.0, 1.0}};
257 return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
296 return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
297 {rotation(2), 0.0, -rotation(0)},
298 {-rotation(1), rotation(0), 0.0}};
311 const Eigen::Vector3d& rotation) {
316 return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
317 {rotation(2), 0.0, -rotation(0)},
318 {-rotation(1), rotation(0), 0.0}};
325 return Transform3d{pose.Translation(), pose.Rotation()};
329 return {m_translation + (other.Translation().
RotateBy(m_rotation)),
330 other.Rotation() + m_rotation};
335 return {transform.Translation(), transform.Rotation()};
341 auto impl = [
this]<
typename Matrix3d,
typename Vector3d>(
343 Vector3d u{{twist.
dx.value(), twist.
dy.value(), twist.
dz.value()}};
344 Vector3d rvec{{twist.
rx.value(), twist.
ry.value(), twist.
rz.value()}};
346 Matrix3d omegaSq = omega * omega;
347 double theta = rvec.norm();
348 double thetaSq = theta * theta;
365 A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
366 B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
367 C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
374 C = (1 - A) / thetaSq;
377 Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq;
378 Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq;
380 Vector3d translation_component = V * u;
384 units::meter_t{translation_component(1)},
385 units::meter_t{translation_component(2)}},
388 return *
this + transform;
391 if (std::is_constant_evaluated()) {
394 return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(twist);
401 auto impl = [
this]<
typename Matrix3d,
typename Vector3d>(
403 const auto transform = end.RelativeTo(*
this);
406 {transform.X().value(), transform.Y().value(), transform.Z().value()}};
407 Vector3d rvec = transform.Rotation().ToVector();
409 Matrix3d omegaSq = omega * omega;
410 double theta = rvec.norm();
411 double thetaSq = theta * theta;
426 C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
432 double B = (1 -
gcem::cos(theta)) / thetaSq;
433 C = (1 - A / (2 * B)) / thetaSq;
436 Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
438 Vector3d translation_component = V_inv * u;
440 return Twist3d{units::meter_t{translation_component(0)},
441 units::meter_t{translation_component(1)},
442 units::meter_t{translation_component(2)},
443 units::radian_t{rvec(0)},
444 units::radian_t{rvec(1)},
445 units::radian_t{rvec(2)}};
448 if (std::is_constant_evaluated()) {
451 return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(end);
#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:328
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:398
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:244
constexpr Pose2d ToPose2d() const
Returns a Pose2d representing this Pose3d projected into the X-Y plane.
Definition Pose3d.h:256
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:333
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:323
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:338
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 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:291
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