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] 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

32
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
View File

@@ -13,25 +13,25 @@ using namespace frc;
TEST(Twist3dTest, StraightX) {
const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d{}.Exp(straight);
const auto straightPose = straight.Exp();
Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}};
Transform3d expected{5_m, 0_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
}
TEST(Twist3dTest, StraightY) {
const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d{}.Exp(straight);
const auto straightPose = straight.Exp();
Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}};
Transform3d expected{0_m, 5_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
}
TEST(Twist3dTest, StraightZ) {
const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d{}.Exp(straight);
const auto straightPose = straight.Exp();
Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}};
Transform3d expected{0_m, 0_m, 5_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
}
@@ -41,17 +41,17 @@ TEST(Twist3dTest, QuarterCircle) {
const Twist3d quarterCircle{
5_m / 2.0 * std::numbers::pi, 0_m, 0_m, 0_rad, 0_rad,
units::radian_t{std::numbers::pi / 2.0}};
const auto quarterCirclePose = Pose3d{}.Exp(quarterCircle);
const auto quarterCirclePose = quarterCircle.Exp();
Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
Transform3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
EXPECT_EQ(expected, quarterCirclePose);
}
TEST(Twist3dTest, DiagonalNoDtheta) {
const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto diagonalPose = Pose3d{}.Exp(diagonal);
const auto diagonalPose = diagonal.Exp();
Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}};
Transform3d expected{2_m, 2_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, diagonalPose);
}
@@ -71,7 +71,7 @@ TEST(Twist3dTest, Pose3dLogX) {
const Pose3d end{0_m, 5_m, 5_m, Rotation3d{90_deg, 0_deg, 0_deg}};
const Pose3d start;
const auto twist = start.Log(end);
const auto twist = (end - start).Log();
Twist3d expected{0_m, units::meter_t{5.0 / 2.0 * std::numbers::pi},
0_m, 90_deg,
@@ -79,7 +79,7 @@ TEST(Twist3dTest, Pose3dLogX) {
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
const auto reapplied = start + twist.Exp();
EXPECT_EQ(end, reapplied);
}
@@ -87,14 +87,14 @@ TEST(Twist3dTest, Pose3dLogY) {
const Pose3d end{5_m, 0_m, 5_m, Rotation3d{0_deg, 90_deg, 0_deg}};
const Pose3d start;
const auto twist = start.Log(end);
const auto twist = (end - start).Log();
Twist3d expected{0_m, 0_m, units::meter_t{5.0 / 2.0 * std::numbers::pi},
0_deg, 90_deg, 0_deg};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
const auto reapplied = start + twist.Exp();
EXPECT_EQ(end, reapplied);
}
@@ -102,7 +102,7 @@ TEST(Twist3dTest, Pose3dLogZ) {
const Pose3d end{5_m, 5_m, 0_m, Rotation3d{0_deg, 0_deg, 90_deg}};
const Pose3d start;
const auto twist = start.Log(end);
const auto twist = (end - start).Log();
Twist3d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi},
0_m,
@@ -113,7 +113,7 @@ TEST(Twist3dTest, Pose3dLogZ) {
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
const auto reapplied = start + twist.Exp();
EXPECT_EQ(end, reapplied);
}