mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
[wpimath] Add CoordinateSystem.convert() translation and rotation overloads (#4227)
This commit is contained in:
Tyler Veness
@@ -10,119 +10,124 @@ import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class CoordinateSystemTest {
|
||||
private void checkConvert(
|
||||
Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
|
||||
// "from" to "to"
|
||||
assertEquals(
|
||||
poseTo.getTranslation(),
|
||||
CoordinateSystem.convert(poseFrom.getTranslation(), coordFrom, coordTo));
|
||||
assertEquals(
|
||||
poseTo.getRotation(), CoordinateSystem.convert(poseFrom.getRotation(), coordFrom, coordTo));
|
||||
assertEquals(poseTo, CoordinateSystem.convert(poseFrom, coordFrom, coordTo));
|
||||
|
||||
// "to" to "from"
|
||||
assertEquals(
|
||||
poseFrom.getTranslation(),
|
||||
CoordinateSystem.convert(poseTo.getTranslation(), coordTo, coordFrom));
|
||||
assertEquals(
|
||||
poseFrom.getRotation(), CoordinateSystem.convert(poseTo.getRotation(), coordTo, coordFrom));
|
||||
assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
|
||||
}
|
||||
|
||||
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
|
||||
@Test
|
||||
void testEDNtoNWU() {
|
||||
var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d());
|
||||
var nwu1 =
|
||||
// No rotation from EDN to NWU
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
-1.0,
|
||||
-2.0,
|
||||
new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(90.0)));
|
||||
new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NWU());
|
||||
|
||||
assertEquals(
|
||||
nwu1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
|
||||
assertEquals(
|
||||
edn1, CoordinateSystem.convert(nwu1, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
|
||||
|
||||
var edn2 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0));
|
||||
var nwu2 =
|
||||
// 45° roll from EDN to NWU
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
-1.0,
|
||||
-2.0,
|
||||
new Rotation3d(-Units.degreesToRadians(45.0), 0.0, -Units.degreesToRadians(90.0)));
|
||||
new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NWU());
|
||||
|
||||
assertEquals(
|
||||
nwu2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
|
||||
assertEquals(
|
||||
edn2, CoordinateSystem.convert(nwu2, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
|
||||
|
||||
var edn3 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0));
|
||||
var nwu3 =
|
||||
// 45° pitch from EDN to NWU
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
-1.0,
|
||||
-2.0,
|
||||
new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(135.0)));
|
||||
new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NWU());
|
||||
|
||||
assertEquals(
|
||||
nwu3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
|
||||
assertEquals(
|
||||
edn3, CoordinateSystem.convert(nwu3, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
|
||||
|
||||
var edn4 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0)));
|
||||
var nwu4 =
|
||||
// 45° yaw from EDN to NWU
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
-1.0,
|
||||
-2.0,
|
||||
new Rotation3d(
|
||||
-Units.degreesToRadians(90.0),
|
||||
Units.degreesToRadians(-90.0),
|
||||
Units.degreesToRadians(45.0),
|
||||
-Units.degreesToRadians(90.0)));
|
||||
|
||||
assertEquals(
|
||||
nwu4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
|
||||
assertEquals(
|
||||
edn4, CoordinateSystem.convert(nwu4, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
|
||||
Units.degreesToRadians(-90.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NWU());
|
||||
}
|
||||
|
||||
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
|
||||
@Test
|
||||
void testEDNtoNED() {
|
||||
var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d());
|
||||
var ned1 =
|
||||
// No rotation from EDN to NED
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
1.0,
|
||||
2.0,
|
||||
new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0)));
|
||||
new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NED());
|
||||
|
||||
assertEquals(
|
||||
ned1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NED()));
|
||||
assertEquals(
|
||||
edn1, CoordinateSystem.convert(ned1, CoordinateSystem.NED(), CoordinateSystem.EDN()));
|
||||
|
||||
var edn2 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0));
|
||||
var ned2 =
|
||||
// 45° roll from EDN to NED
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
1.0,
|
||||
2.0,
|
||||
new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0)));
|
||||
new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NED());
|
||||
|
||||
assertEquals(
|
||||
ned2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NED()));
|
||||
assertEquals(
|
||||
edn2, CoordinateSystem.convert(ned2, CoordinateSystem.NED(), CoordinateSystem.EDN()));
|
||||
|
||||
var edn3 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0));
|
||||
var ned3 =
|
||||
// 45° pitch from EDN to NED
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
1.0,
|
||||
2.0,
|
||||
new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0)));
|
||||
new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NED());
|
||||
|
||||
assertEquals(
|
||||
ned3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NED()));
|
||||
assertEquals(
|
||||
edn3, CoordinateSystem.convert(ned3, CoordinateSystem.NED(), CoordinateSystem.EDN()));
|
||||
|
||||
var edn4 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0)));
|
||||
var ned4 =
|
||||
// 45° yaw from EDN to NED
|
||||
checkConvert(
|
||||
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
|
||||
new Pose3d(
|
||||
3.0,
|
||||
1.0,
|
||||
2.0,
|
||||
new Rotation3d(
|
||||
Units.degreesToRadians(90.0),
|
||||
-Units.degreesToRadians(45.0),
|
||||
Units.degreesToRadians(90.0)));
|
||||
|
||||
assertEquals(
|
||||
ned4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NED()));
|
||||
assertEquals(
|
||||
edn4, CoordinateSystem.convert(ned4, CoordinateSystem.NED(), CoordinateSystem.EDN()));
|
||||
Units.degreesToRadians(-45.0),
|
||||
Units.degreesToRadians(90.0))),
|
||||
CoordinateSystem.EDN(),
|
||||
CoordinateSystem.NED());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user