Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace Pose2/3d.exp(Twist2/3d) with Pose2/3d.plus(Twist2/3d.exp()) (#8188)

Browse Source 
This better matches math notation.
This commit is contained in:
ninjadrknss
2025-08-30 11:37:09 -07:00
committed by GitHub
parent 55e52bc2b7
commit 96004f9bb5
51 changed files with 643 additions and 682 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -263,7 +263,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = visionSample.value().Log(visionRobotPose);
auto twist = (visionRobotPose - visionSample.value()).Log();
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
@@ -278,7 +278,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
units::radian_t{k_times_twist(2)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample};
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)

5
wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
View File

@@ -272,7 +272,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = visionSample.value().Log(visionRobotPose);
auto twist = (visionRobotPose - visionSample.value()).Log();
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
@@ -289,7 +289,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample};
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)

81
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -15,7 +15,6 @@
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "units/length.h"
namespace frc {
@@ -197,39 +196,6 @@ class WPILIB_DLLEXPORT Pose2d {
return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* See https://file.tavsys.net/control/controls-engineering-in-frc.pdf section
* 10.2 "Pose exponential" for a derivation.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}.
*
* @return The new pose of the robot.
*/
constexpr Pose2d Exp(const Twist2d& twist) const;
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
*
* @return The twist that maps this to end.
*/
constexpr Twist2d Log(const Pose2d& end) const;
/**
* Returns an affine transformation matrix representation of this pose.
*/
@@ -328,51 +294,4 @@ constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
return {transform.Translation(), transform.Rotation()};
}
constexpr Pose2d Pose2d::Exp(const Twist2d& twist) const {
const auto dx = twist.dx;
const auto dy = twist.dy;
const auto dtheta = twist.dtheta.value();
const auto sinTheta = gcem::sin(dtheta);
const auto cosTheta = gcem::cos(dtheta);
double s, c;
if (gcem::abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
Rotation2d{cosTheta, sinTheta}};
return *this + transform;
}
constexpr Twist2d Pose2d::Log(const Pose2d& end) const {
const auto transform = end.RelativeTo(*this);
const auto dtheta = transform.Rotation().Radians().value();
const auto halfDtheta = dtheta / 2.0;
const auto cosMinusOne = transform.Rotation().Cos() - 1;
double halfThetaByTanOfHalfDtheta;
if (gcem::abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta =
-(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
}
const Translation2d translationPart =
transform.Translation().RotateBy(
{halfThetaByTanOfHalfDtheta, -halfDtheta}) *
gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
}
} // namespace frc

198
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -19,7 +19,6 @@
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation3d.h"
#include "frc/geometry/Twist3d.h"
namespace frc {
@@ -223,37 +222,6 @@ class WPILIB_DLLEXPORT Pose3d {
return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
* 0.5_deg}}.
*
* @return The new pose of the robot.
*/
constexpr Pose3d Exp(const Twist3d& twist) const;
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
*
* @return The twist that maps this to end.
*/
constexpr Twist3d Log(const Pose3d& end) const;
/**
* Returns an affine transformation matrix representation of this pose.
*/
@@ -336,56 +304,10 @@ void from_json(const wpi::json& json, Pose3d& pose);
} // namespace frc
#include "frc/geometry/proto/Pose3dProto.h"
#include "frc/geometry/struct/Pose3dStruct.h"
#include "frc/geometry/Transform3d.h"
namespace frc {
namespace detail {
/**
* Applies the hat operator to a rotation vector.
*
* It takes a rotation vector and returns the corresponding matrix
* representation of the Lie algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
/**
* Applies the hat operator to a rotation vector.
*
* It takes a rotation vector and returns the corresponding matrix
* representation of the Lie algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
constexpr Eigen::Matrix3d RotationVectorToMatrix(
const Eigen::Vector3d& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
} // namespace detail
constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
const auto pose = this->RelativeTo(other);
return Transform3d{pose.Translation(), pose.Rotation()};
@@ -401,121 +323,7 @@ constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
return {transform.Translation(), transform.Rotation()};
}
constexpr Pose3d Pose3d::Exp(const Twist3d& twist) const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
auto impl = [this]<typename Matrix3d, typename Vector3d>(
const Twist3d& twist) -> Pose3d {
Vector3d u{{twist.dx.value(), twist.dy.value(), twist.dz.value()}};
Vector3d rvec{{twist.rx.value(), twist.ry.value(), twist.rz.value()}};
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();
double thetaSq = theta * theta;
double A;
double B;
double C;
if (gcem::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/3! - θ²/5! + θ⁴/7!
// sources:
// A:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
// B:
// 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
// C:
// 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
A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
} else {
// A = std::sin(θ)/θ
// B = (1 - std::cos(θ)) / θ²
// C = (1 - A) / θ²
A = gcem::sin(theta) / theta;
B = (1 - gcem::cos(theta)) / thetaSq;
C = (1 - A) / thetaSq;
}
Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq;
Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq;
Vector3d translation_component = V * u;
const Transform3d transform{
Translation3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)}},
Rotation3d{R}};
return *this + transform;
};
if (std::is_constant_evaluated()) {
return impl.template operator()<ct_matrix3d, ct_vector3d>(twist);
} else {
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(twist);
}
}
constexpr Twist3d Pose3d::Log(const Pose3d& end) const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
auto impl = [this]<typename Matrix3d, typename Vector3d>(
const Pose3d& end) -> Twist3d {
const auto transform = end.RelativeTo(*this);
Vector3d u{
{transform.X().value(), transform.Y().value(), transform.Z().value()}};
Vector3d rvec = transform.Rotation().ToVector();
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();
double thetaSq = theta * theta;
double C;
if (gcem::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
// sources:
// A:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
// B:
// 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
// C:
// 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
C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
} else {
// A = std::sin(θ)/θ
// B = (1 - std::cos(θ)) / θ²
// C = (1 - A/(2*B)) / θ²
double A = gcem::sin(theta) / theta;
double B = (1 - gcem::cos(theta)) / thetaSq;
C = (1 - A / (2 * B)) / thetaSq;
}
Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
Vector3d translation_component = V_inv * u;
return Twist3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)},
units::radian_t{rvec(0)},
units::radian_t{rvec(1)},
units::radian_t{rvec(2)}};
};
if (std::is_constant_evaluated()) {
return impl.template operator()<ct_matrix3d, ct_vector3d>(end);
} else {
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(end);
}
}
} // namespace frc
#include "frc/geometry/proto/Pose3dProto.h"
#include "frc/geometry/struct/Pose3dStruct.h"

31
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -14,6 +14,7 @@
namespace frc {
class Pose2d;
struct Twist2d;
/**
* Represents a transformation for a Pose2d in the pose's frame.
@@ -109,6 +110,14 @@ class WPILIB_DLLEXPORT Transform2d {
*/
constexpr const Rotation2d& Rotation() const { return m_rotation; }
/**
* Returns a Twist2d of the current transform (pose delta). If b is the output
* of {@code a.Log()}, then {@code b.Exp()} would yield a.
*
* @return The twist that maps the current transform.
*/
constexpr Twist2d Log() const;
/**
* Invert the transformation. This is useful for undoing a transformation.
*
@@ -163,6 +172,7 @@ class WPILIB_DLLEXPORT Transform2d {
} // namespace frc
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Twist2d.h"
namespace frc {
@@ -180,6 +190,27 @@ constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}
constexpr Twist2d Transform2d::Log() const {
const auto dtheta = m_rotation.Radians().value();
const auto halfDtheta = dtheta / 2.0;
const auto cosMinusOne = m_rotation.Cos() - 1;
double halfThetaByTanOfHalfDtheta;
if (gcem::abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * m_rotation.Sin()) / cosMinusOne;
}
const Translation2d translationPart =
m_translation.RotateBy({halfThetaByTanOfHalfDtheta, -halfDtheta}) *
gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
}
} // namespace frc
#include "frc/geometry/proto/Transform2dProto.h"

64
wpimath/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -14,6 +14,7 @@
namespace frc {
class Pose3d;
struct Twist3d;
/**
* Represents a transformation for a Pose3d in the pose's frame.
@@ -134,6 +135,14 @@ class WPILIB_DLLEXPORT Transform3d {
*/
constexpr const Rotation3d& Rotation() const { return m_rotation; }
/**
* Returns a Twist3d of the current transform (pose delta). If b is the output
* of {@code a.Log()}, then {@code b.Exp()} would yield a.
*
* @return The twist that maps the current transform.
*/
constexpr Twist3d Log() const;
/**
* Invert the transformation. This is useful for undoing a transformation.
*
@@ -188,6 +197,8 @@ class WPILIB_DLLEXPORT Transform3d {
} // namespace frc
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Twist3d.h"
#include "frc/geometry/detail/RotationVectorToMatrix.h"
namespace frc {
@@ -205,6 +216,59 @@ constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
constexpr Twist3d Transform3d::Log() const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
auto impl = [this]<typename Matrix3d, typename Vector3d>() -> Twist3d {
Vector3d u{{m_translation.X().value(), m_translation.Y().value(),
m_translation.Z().value()}};
Vector3d rvec = m_rotation.ToVector();
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();
double thetaSq = theta * theta;
double C;
if (gcem::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
// sources:
// A:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
// B:
// 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
// C:
// 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
C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
} else {
// A = std::sin(θ)/θ
// B = (1 - std::cos(θ)) / θ²
// C = (1 - A/(2*B)) / θ²
double A = gcem::sin(theta) / theta;
double B = (1 - gcem::cos(theta)) / thetaSq;
C = (1 - A / (2 * B)) / thetaSq;
}
Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
Vector3d translation_component = V_inv * u;
return Twist3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)},
units::radian_t{rvec(0)},
units::radian_t{rvec(1)},
units::radian_t{rvec(2)}};
};
if (std::is_constant_evaluated()) {
return impl.template operator()<ct_matrix3d, ct_vector3d>();
}
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>();
}
} // namespace frc
#include "frc/geometry/proto/Transform3dProto.h"

44
wpimath/src/main/native/include/frc/geometry/Twist2d.h
View File

@@ -12,6 +12,8 @@
namespace frc {
class Transform2d;
/**
* A change in distance along a 2D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose2ds from a Twist2d and
@@ -35,6 +37,24 @@ struct WPILIB_DLLEXPORT Twist2d {
*/
units::radian_t dtheta = 0_rad;
/**
* Obtain a new Transform2d from a (constant curvature) velocity.
*
* See "https://file.tavsys.net/control/controls-engineering-in-frc.pdf"
* Controls Engineering in the FIRST Robotics Competition</a> section 10.2
* "Pose exponential" for a derivation.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs Exp() on the twist, the user will
* receive the pose delta.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @return The pose delta of the robot.
*/
constexpr Transform2d Exp() const;
/**
* Checks equality between this Twist2d and another object.
*
@@ -60,5 +80,29 @@ struct WPILIB_DLLEXPORT Twist2d {
} // namespace frc
#include "frc/geometry/Transform2d.h"
namespace frc {
constexpr Transform2d Twist2d::Exp() const {
const auto theta = dtheta.value();
const auto sinTheta = gcem::sin(theta);
const auto cosTheta = gcem::cos(theta);
double s, c;
if (gcem::abs(theta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * theta * theta;
c = 0.5 * theta;
} else {
s = sinTheta / theta;
c = (1 - cosTheta) / theta;
}
return Transform2d(Translation2d{dx * s - dy * c, dx * c + dy * s},
Rotation2d{cosTheta, sinTheta});
}
} // namespace frc
#include "frc/geometry/proto/Twist2dProto.h"
#include "frc/geometry/struct/Twist2dStruct.h"

85
wpimath/src/main/native/include/frc/geometry/Twist3d.h
View File

@@ -12,6 +12,8 @@
namespace frc {
class Transform3d;
/**
* A change in distance along a 3D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose3ds from a Twist3d and
@@ -50,6 +52,24 @@ struct WPILIB_DLLEXPORT Twist3d {
*/
units::radian_t rz = 0_rad;
/**
* Obtain a new Transform3d from a (constant curvature) velocity.
*
* See "https://file.tavsys.net/control/controls-engineering-in-frc.pdf"
* Controls Engineering in the FIRST Robotics Competition section 10.2
* "Pose exponential" for a derivation.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs Exp() on the twist, the user will
* receive the pose delta.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @return The pose delta of the robot.
*/
constexpr Transform3d Exp() const;
/**
* Checks equality between this Twist3d and another object.
*
@@ -79,5 +99,70 @@ struct WPILIB_DLLEXPORT Twist3d {
} // namespace frc
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/detail/RotationVectorToMatrix.h"
namespace frc {
constexpr Transform3d Twist3d::Exp() const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
auto impl = [this]<typename Matrix3d, typename Vector3d>() -> Transform3d {
Vector3d u{{dx.value(), dy.value(), dz.value()}};
Vector3d rvec{{rx.value(), ry.value(), rz.value()}};
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();
double thetaSq = theta * theta;
double A;
double B;
double C;
if (gcem::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/3! - θ²/5! + θ⁴/7!
// sources:
// A:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
// B:
// 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
// C:
// 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
A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
} else {
// A = std::sin(θ)/θ
// B = (1 - std::cos(θ)) / θ²
// C = (1 - A) / θ²
A = gcem::sin(theta) / theta;
B = (1 - gcem::cos(theta)) / thetaSq;
C = (1 - A) / thetaSq;
}
Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq;
Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq;
Vector3d translation_component = V * u;
const Transform3d transform{
Translation3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)}},
Rotation3d{R}};
return transform;
};
if (std::is_constant_evaluated()) {
return impl.template operator()<ct_matrix3d, ct_vector3d>();
}
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>();
}
} // namespace frc
#include "frc/geometry/proto/Twist3dProto.h"
#include "frc/geometry/struct/Twist3dStruct.h"

26
wpimath/src/main/native/include/frc/geometry/detail/RotationVectorToMatrix.h Normal file
View File

@@ -0,0 +1,26 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <Eigen/Core>
#include "frc/ct_matrix.h"
namespace frc::detail {
constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) {
return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
constexpr Eigen::Matrix3d RotationVectorToMatrix(
const Eigen::Vector3d& rotation) {
return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
} // namespace frc::detail

4
wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
View File

@@ -166,9 +166,9 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
} else if (t >= 1) {
return end;
} else {
Twist2d twist = start.Log(end);
Twist2d twist = (end - start).Log();
Twist2d scaledTwist = twist * t;
return start.Exp(scaledTwist);
return start + scaledTwist.Exp();
}
}) {}

4
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -71,11 +71,11 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
constexpr ChassisSpeeds Discretize(units::second_t dt) const {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
// Find the chassis translation/rotation deltas in the robot frame that move
// the robot from its current pose to the desired pose
auto twist = Pose2d{}.Log(desiredDeltaPose);
auto twist = desiredTransform.Log();
// Turn the chassis translation/rotation deltas into average velocities
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};

2
wpimath/src/main/native/include/frc/kinematics/Odometry.h
View File

@@ -122,7 +122,7 @@ class WPILIB_DLLEXPORT Odometry {
m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(twist);
auto newPose = m_pose + twist.Exp();
m_previousAngle = angle;
m_previousWheelPositions = wheelPositions;

2
wpimath/src/main/native/include/frc/kinematics/Odometry3d.h
View File

@@ -131,7 +131,7 @@ class WPILIB_DLLEXPORT Odometry3d {
units::radian_t{angle_difference(1)},
units::radian_t{angle_difference(2)}};
auto newPose = m_pose.Exp(twist);
auto newPose = m_pose + twist.Exp();
m_previousWheelPositions = wheelPositions;
m_previousAngle = angle;

2
wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
View File

@@ -106,7 +106,7 @@ class WPILIB_DLLEXPORT SplineParameterizer {
throw MalformedSplineException(kMalformedSplineExceptionMsg);
}
const auto twist = start.value().first.Log(end.value().first);
const auto twist = (end.value().first - start.value().first).Log();
if (units::math::abs(twist.dy) > kMaxDy ||
units::math::abs(twist.dx) > kMaxDx ||