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

[wpimath] Add CoordinateSystem.convert() translation and rotation overloads (#4227)

Browse Source
This commit is contained in:
Tyler Veness
2022-05-18 20:41:15 -07:00
committed by GitHub
parent 3fada4e0b4
commit 0d9956273c
5 changed files with 190 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
View File

@@ -8,70 +8,66 @@
using namespace frc;
void CheckConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
const CoordinateSystem& coordFrom,
const CoordinateSystem& coordTo) {
// "from" to "to"
EXPECT_EQ(
poseTo.Translation(),
CoordinateSystem::Convert(poseFrom.Translation(), coordFrom, coordTo));
EXPECT_EQ(poseTo.Rotation(),
CoordinateSystem::Convert(poseFrom.Rotation(), coordFrom, coordTo));
EXPECT_EQ(poseTo, CoordinateSystem::Convert(poseFrom, coordFrom, coordTo));
// "to" to "from"
EXPECT_EQ(
poseFrom.Translation(),
CoordinateSystem::Convert(poseTo.Translation(), coordTo, coordFrom));
EXPECT_EQ(poseFrom.Rotation(),
CoordinateSystem::Convert(poseTo.Rotation(), coordTo, coordFrom));
EXPECT_EQ(poseFrom, CoordinateSystem::Convert(poseTo, coordTo, coordFrom));
}
TEST(CoordinateSystemTest, EDNtoNWU) {
Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}};
Pose3d nwu1{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}};
// No rotation from EDN to NWU
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
EXPECT_EQ(nwu1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(),
CoordinateSystem::NWU()));
EXPECT_EQ(edn1, CoordinateSystem::Convert(nwu1, CoordinateSystem::NWU(),
CoordinateSystem::EDN()));
// 45° roll from EDN to NWU
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
Pose3d{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}};
Pose3d nwu2{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}};
// 45° pitch from EDN to NWU
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
EXPECT_EQ(nwu2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(),
CoordinateSystem::NWU()));
EXPECT_EQ(edn2, CoordinateSystem::Convert(nwu2, CoordinateSystem::NWU(),
CoordinateSystem::EDN()));
Pose3d edn3{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}};
Pose3d nwu3{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}};
EXPECT_EQ(nwu3, CoordinateSystem::Convert(edn3, CoordinateSystem::EDN(),
CoordinateSystem::NWU()));
EXPECT_EQ(edn3, CoordinateSystem::Convert(nwu3, CoordinateSystem::NWU(),
CoordinateSystem::EDN()));
Pose3d edn4{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}};
Pose3d nwu4{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}};
EXPECT_EQ(nwu4, CoordinateSystem::Convert(edn4, CoordinateSystem::EDN(),
CoordinateSystem::NWU()));
EXPECT_EQ(edn4, CoordinateSystem::Convert(nwu4, CoordinateSystem::NWU(),
CoordinateSystem::EDN()));
// 45° yaw from EDN to NWU
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
}
TEST(CoordinateSystemTest, EDNtoNED) {
Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}};
Pose3d ned1{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}};
// No rotation from EDN to NED
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
EXPECT_EQ(ned1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(),
CoordinateSystem::NED()));
EXPECT_EQ(edn1, CoordinateSystem::Convert(ned1, CoordinateSystem::NED(),
CoordinateSystem::EDN()));
// 45° roll from EDN to NED
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
Pose3d{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}};
Pose3d ned2{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}};
// 45° pitch from EDN to NED
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
EXPECT_EQ(ned2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(),
CoordinateSystem::NED()));
EXPECT_EQ(edn2, CoordinateSystem::Convert(ned2, CoordinateSystem::NED(),
CoordinateSystem::EDN()));
Pose3d edn3{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}};
Pose3d ned3{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}};
EXPECT_EQ(ned3, CoordinateSystem::Convert(edn3, CoordinateSystem::EDN(),
CoordinateSystem::NED()));
EXPECT_EQ(edn3, CoordinateSystem::Convert(ned3, CoordinateSystem::NED(),
CoordinateSystem::EDN()));
Pose3d edn4{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}};
Pose3d ned4{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}};
EXPECT_EQ(ned4, CoordinateSystem::Convert(edn4, CoordinateSystem::EDN(),
CoordinateSystem::NED()));
EXPECT_EQ(edn4, CoordinateSystem::Convert(ned4, CoordinateSystem::NED(),
CoordinateSystem::EDN()));
// 45° yaw from EDN to NED
CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
}