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

[wpimath] Add affine transformation constructors and getters to geometry API (#7430)

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 15:49:17 -08:00
committed by GitHub
parent f772bb141d
commit e222efaa01
32 changed files with 615 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -53,6 +53,21 @@ class WPILIB_DLLEXPORT Pose2d {
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with the specified affine transformation matrix.
*
* @param matrix The affine transformation matrix.
* @throws std::domain_error if the affine transformation matrix is invalid.
*/
constexpr explicit Pose2d(const Eigen::Matrix3d& matrix)
: m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
{matrix(1, 0), matrix(1, 1)}}} {
if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
throw std::domain_error("Affine transformation matrix is invalid");
}
}
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
@@ -202,6 +217,17 @@ class WPILIB_DLLEXPORT Pose2d {
*/
constexpr Twist2d Log(const Pose2d& end) const;
/**
* Returns an affine transformation matrix representation of this pose.
*/
constexpr Eigen::Matrix3d ToMatrix() const {
auto vec = m_translation.ToVector();
auto mat = m_rotation.ToMatrix();
return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
{mat(1, 0), mat(1, 1), vec(1)},
{0.0, 0.0, 1.0}};
}
/**
* Returns the nearest Pose2d from a collection of poses
* @param poses The collection of poses.