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

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

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 21:29:02 -08:00
committed by GitHub
parent c81bd0c909
commit 62a6a77bbf
32 changed files with 611 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -206,9 +206,12 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
* @param pose The pose that is being represented.
*
* @return The vector.
* @deprecated Create the vector manually instead. If you were using this as an
* intermediate step for constructing affine transformations, use
* Pose2d.ToMatrix() instead.
*/
WPILIB_DLLEXPORT
constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
[[deprecated("Use Pose2d.ToMatrix() instead.")]]
WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector3d{{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()}};
@@ -220,9 +223,12 @@ constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
* @param pose The pose that is being represented.
*
* @return The vector.
* @deprecated Create the vector manually instead. If you were using this as an
* intermediate step for constructing affine transformations, use
* Pose2d.ToMatrix() instead.
*/
WPILIB_DLLEXPORT
constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
[[deprecated("Use Pose2d.ToMatrix() instead.")]]
WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector4d{{pose.Translation().X().value(),
pose.Translation().Y().value(), pose.Rotation().Cos(),
pose.Rotation().Sin()}};
@@ -311,9 +317,12 @@ bool IsDetectable(const Matrixd<States, States>& A,
* @param pose The pose that is being represented.
*
* @return The vector.
* @deprecated Create the vector manually instead. If you were using this as an
* intermediate step for constructing affine transformations, use
* Pose2d.ToMatrix() instead.
*/
WPILIB_DLLEXPORT
constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
[[deprecated("Use Pose2d.ToMatrix() instead.")]]
WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
return Eigen::Vector3d{
{pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}};
}

19
wpimath/src/main/native/include/frc/ct_matrix.h
View File

@@ -308,6 +308,23 @@ class ct_matrix {
(*this)(0) * rhs(1) - rhs(0) * (*this)(1)}};
}
/**
* Constexpr version of Eigen's 2x2 matrix determinant member function.
*
* @return Determinant of matrix.
*/
constexpr Scalar determinant() const
requires(Rows == 2 && Cols == 2)
{
// |a b|
// |c d| = ad - bc
Scalar a = (*this)(0, 0);
Scalar b = (*this)(0, 1);
Scalar c = (*this)(1, 0);
Scalar d = (*this)(1, 1);
return a * d - b * c;
}
/**
* Constexpr version of Eigen's 3x3 matrix determinant member function.
*
@@ -364,7 +381,9 @@ using ct_vector = ct_matrix<Scalar, Rows, 1>;
template <typename Scalar, int Cols>
using ct_row_vector = ct_matrix<Scalar, 1, Cols>;
using ct_matrix2d = ct_matrix<double, 2, 2>;
using ct_matrix3d = ct_matrix<double, 3, 3>;
using ct_vector2d = ct_vector<double, 2>;
using ct_vector3d = ct_vector<double, 3>;
} // namespace frc

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.

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

@@ -4,6 +4,7 @@
#pragma once
#include <stdexcept>
#include <type_traits>
#include <utility>
@@ -54,6 +55,25 @@ class WPILIB_DLLEXPORT Pose3d {
Rotation3d rotation)
: m_translation{x, y, z}, 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 Pose3d(const Eigen::Matrix4d& matrix)
: m_translation{Eigen::Vector3d{
{matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
m_rotation{
Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
{matrix(1, 0), matrix(1, 1), matrix(1, 2)},
{matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
matrix(3, 3) != 1.0) {
throw std::domain_error("Affine transformation matrix is invalid");
}
}
/**
* Constructs a 3D pose from a 2D pose in the X-Y plane.
*
@@ -218,6 +238,18 @@ class WPILIB_DLLEXPORT Pose3d {
*/
constexpr Twist3d Log(const Pose3d& end) const;
/**
* Returns an affine transformation matrix representation of this pose.
*/
constexpr Eigen::Matrix4d ToMatrix() const {
auto vec = m_translation.ToVector();
auto mat = m_rotation.ToMatrix();
return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
{mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
{mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
{0.0, 0.0, 0.0, 1.0}};
}
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/

48
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -5,12 +5,16 @@
#pragma once
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include <Eigen/LU>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include "frc/ct_matrix.h"
#include "units/angle.h"
#include "wpimath/MathShared.h"
@@ -59,6 +63,41 @@ class WPILIB_DLLEXPORT Rotation2d {
}
}
/**
* Constructs a Rotation2d from a rotation matrix.
*
* @param rotationMatrix The rotation matrix.
* @throws std::domain_error if the rotation matrix isn't special orthogonal.
*/
constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) {
auto impl =
[]<typename Matrix2d>(const Matrix2d& R) -> std::pair<double, double> {
// Require that the rotation matrix is special orthogonal. This is true if
// the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if ((R * R.transpose() - Matrix2d::Identity()).norm() > 1e-9) {
throw std::domain_error("Rotation matrix isn't orthogonal");
}
if (gcem::abs(R.determinant() - 1.0) > 1e-9) {
throw std::domain_error(
"Rotation matrix is orthogonal but not special orthogonal");
}
// R = [cosθ −sinθ]
// [sinθ cosθ]
return {R(0, 0), R(1, 0)};
};
if (std::is_constant_evaluated()) {
auto cossin = impl(ct_matrix2d{rotationMatrix});
m_cos = std::get<0>(cossin);
m_sin = std::get<1>(cossin);
} else {
auto cossin = impl(rotationMatrix);
m_cos = std::get<0>(cossin);
m_sin = std::get<1>(cossin);
}
}
/**
* Adds two rotations together, with the result being bounded between -π and
* π.
@@ -147,6 +186,15 @@ class WPILIB_DLLEXPORT Rotation2d {
Cos() * other.Sin() + Sin() * other.Cos()};
}
/**
* Returns matrix representation of this rotation.
*/
constexpr Eigen::Matrix2d ToMatrix() const {
// R = [cosθ −sinθ]
// [sinθ cosθ]
return Eigen::Matrix2d{{m_cos, -m_sin}, {m_sin, m_cos}};
}
/**
* Returns the radian value of the rotation constrained within [-π, π].
*

18
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -403,6 +403,24 @@ class WPILIB_DLLEXPORT Rotation3d {
return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
}
/**
* Returns rotation matrix representation of this rotation.
*/
constexpr Eigen::Matrix3d ToMatrix() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z),
2.0 * (x * z + w * y)},
{2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z),
2.0 * (y * z - w * x)},
{2.0 * (x * z - w * y), 2.0 * (y * z + w * x),
1.0 - 2.0 * (x * x + y * y)}};
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.

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

@@ -49,6 +49,21 @@ class WPILIB_DLLEXPORT Transform2d {
constexpr Transform2d(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 Transform2d(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");
}
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
@@ -75,6 +90,18 @@ class WPILIB_DLLEXPORT Transform2d {
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns an affine transformation matrix representation of this
* transformation.
*/
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 rotational component of the transformation.
*

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

@@ -51,6 +51,25 @@ class WPILIB_DLLEXPORT Transform3d {
Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
/**
* Constructs a transform 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 Transform3d(const Eigen::Matrix4d& matrix)
: m_translation{Eigen::Vector3d{
{matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
m_rotation{
Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
{matrix(1, 0), matrix(1, 1), matrix(1, 2)},
{matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
matrix(3, 3) != 1.0) {
throw std::domain_error("Affine transformation matrix is invalid");
}
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
@@ -95,6 +114,19 @@ class WPILIB_DLLEXPORT Transform3d {
*/
constexpr units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns an affine transformation matrix representation of this
* transformation.
*/
constexpr Eigen::Matrix4d ToMatrix() const {
auto vec = m_translation.ToVector();
auto mat = m_rotation.ToMatrix();
return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
{mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
{mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
{0.0, 0.0, 0.0, 1.0}};
}
/**
* Returns the rotational component of the transformation.
*

12
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -54,13 +54,13 @@ class WPILIB_DLLEXPORT Translation2d {
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
/**
* Constructs a Translation2d from the provided translation vector's X and Y
* components. The values are assumed to be in meters.
* Constructs a Translation2d from a 2D translation vector. The values are
* assumed to be in meters.
*
* @param vector The translation vector to represent.
* @param vector The translation vector.
*/
constexpr explicit Translation2d(const Eigen::Vector2d& vector)
: m_x{units::meter_t{vector(0)}}, m_y{units::meter_t{vector(1)}} {}
: m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
/**
* Calculates the distance between two translations in 2D space.
@@ -90,9 +90,9 @@ class WPILIB_DLLEXPORT Translation2d {
constexpr units::meter_t Y() const { return m_y; }
/**
* Returns a vector representation of this translation.
* Returns a 2D translation vector representation of this translation.
*
* @return A Vector representation of this translation.
* @return A 2D translation vector representation of this translation.
*/
constexpr Eigen::Vector2d ToVector() const {
return Eigen::Vector2d{{m_x.value(), m_y.value()}};

10
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -56,10 +56,10 @@ class WPILIB_DLLEXPORT Translation3d {
}
/**
* Constructs a Translation3d from the provided translation vector's X, Y, and
* Z components. The values are assumed to be in meters.
* Constructs a Translation3d from a 3D translation vector. The values are
* assumed to be in meters.
*
* @param vector The translation vector to represent.
* @param vector The translation vector.
*/
constexpr explicit Translation3d(const Eigen::Vector3d& vector)
: m_x{units::meter_t{vector.x()}},
@@ -114,9 +114,9 @@ class WPILIB_DLLEXPORT Translation3d {
constexpr units::meter_t Z() const { return m_z; }
/**
* Returns a vector representation of this translation.
* Returns a 3D translation vector representation of this translation.
*
* @return A Vector representation of this translation.
* @return A 3D translation vector representation of this translation.
*/
constexpr Eigen::Vector3d ToVector() const {
return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};