Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

30
wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
View File

@@ -39,8 +39,8 @@ public class CoordinateSystem {
R.assignBlock(0, 2, positiveZ.m_axis);
// Require that the change of basis matrix is special orthogonal. This is true
// if the axes used are orthogonal and normalized. The Axis class already
// normalizes itself, so we just need to check for orthogonality.
// if the axes used are orthogonal and normalized. The CoordinateAxis class
// already normalizes itself, so we just need to check for orthogonality.
if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
}
@@ -120,6 +120,32 @@ public class CoordinateSystem {
return m_ned;
}
/**
* Converts the given translation from one coordinate system to another.
*
* @param translation The translation to convert.
* @param from The coordinate system the pose starts in.
* @param to The coordinate system to which to convert.
* @return The given translation in the desired coordinate system.
*/
public static Translation3d convert(
Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
}
/**
* Converts the given rotation from one coordinate system to another.
*
* @param rotation The rotation to convert.
* @param from The coordinate system the rotation starts in.
* @param to The coordinate system to which to convert.
* @return The given rotation in the desired coordinate system.
*/
public static Rotation3d convert(
Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
}
/**
* Converts the given pose from one coordinate system to another.
*

16
wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
View File

@@ -24,8 +24,8 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
R.block<3, 1>(0, 2) = positiveZ.m_axis;
// Require that the change of basis matrix is special orthogonal. This is true
// if the axes used are orthogonal and normalized. The Axis class already
// normalizes itself, so we just need to check for orthogonality.
// if the axes used are orthogonal and normalized. The CoordinateAxis class
// already normalizes itself, so we just need to check for orthogonality.
if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
throw std::domain_error("Coordinate system isn't special orthogonal");
}
@@ -87,6 +87,18 @@ const CoordinateSystem& CoordinateSystem::NED() {
return instance;
}
Translation3d CoordinateSystem::Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return translation.RotateBy(from.m_rotation - to.m_rotation);
}
Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return rotation.RotateBy(from.m_rotation - to.m_rotation);
}
Pose3d CoordinateSystem::Convert(const Pose3d& pose,
const CoordinateSystem& from,
const CoordinateSystem& to) {

25
wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
View File

@@ -10,6 +10,7 @@
#include "frc/geometry/CoordinateAxis.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation3d.h"
namespace frc {
@@ -58,6 +59,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
*/
static const CoordinateSystem& NED();
/**
* Converts the given translation from one coordinate system to another.
*
* @param translation The translation to convert.
* @param from The coordinate system the translation starts in.
* @param to The coordinate system to which to convert.
* @return The given translation in the desired coordinate system.
*/
static Translation3d Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to);
/**
* Converts the given rotation from one coordinate system to another.
*
* @param rotation The rotation to convert.
* @param from The coordinate system the rotation starts in.
* @param to The coordinate system to which to convert.
* @return The given rotation in the desired coordinate system.
*/
static Rotation3d Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to);
/**
* Converts the given pose from one coordinate system to another.
*

137
wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
View File

@@ -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());
}
}

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());
}