Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add CoordinateSystem conversion for Transform3d (#4443)

Browse Source 
I also refactored Pose3d's conversion implementation to use the
Translation3d and Rotation3d conversions, thereby giving Translation3d
and Rotation3d test coverage. No changes were made to the expected
values of the Pose3d conversion tests.

The expected values of the Transform3d conversion tests were copied from
the Pose3d conversion tests without modification.
This commit is contained in:
Tyler Veness
2022-10-01 21:09:04 -07:00
committed by GitHub
parent 38bb23eb18
commit b53b3526a2
5 changed files with 287 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -110,6 +110,21 @@ public class CoordinateSystem {
* @return The given pose in the desired coordinate system.
*/
public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
return pose.relativeTo(new Pose3d(new Translation3d(), to.m_rotation.minus(from.m_rotation)));
return new Pose3d(
convert(pose.getTranslation(), from, to), convert(pose.getRotation(), from, to));
}
/**
* Converts the given transform from one coordinate system to another.
*
* @param transform The transform to convert.
* @param from The coordinate system the transform starts in.
* @param to The coordinate system to which to convert.
* @return The given transform in the desired coordinate system.
*/
public static Transform3d convert(
Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
return new Transform3d(
convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
}
}

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

@@ -61,6 +61,13 @@ Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
Pose3d CoordinateSystem::Convert(const Pose3d& pose,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return pose.RelativeTo(
Pose3d{Translation3d{}, to.m_rotation - from.m_rotation});
return Pose3d{Convert(pose.Translation(), from, to),
Convert(pose.Rotation(), from, to)};
}
Transform3d CoordinateSystem::Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return Transform3d{Convert(transform.Translation(), from, to),
Convert(transform.Rotation(), from, to)};
}

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

@@ -94,6 +94,18 @@ class WPILIB_DLLEXPORT CoordinateSystem {
static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from,
const CoordinateSystem& to);
/**
* Converts the given transform from one coordinate system to another.
*
* @param transform The transform to convert.
* @param from The coordinate system the transform starts in.
* @param to The coordinate system to which to convert.
* @return The given transform in the desired coordinate system.
*/
static Transform3d Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to);
private:
// Rotation from this coordinate system to NWU coordinate system
Rotation3d m_rotation;

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

@@ -10,7 +10,7 @@ import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
class CoordinateSystemTest {
private void checkConvert(
private void checkPose3dConvert(
Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
// "from" to "to"
assertEquals(
@@ -29,10 +29,34 @@ class CoordinateSystemTest {
assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
}
private void checkTransform3dConvert(
Transform3d transformFrom,
Transform3d transformTo,
CoordinateSystem coordFrom,
CoordinateSystem coordTo) {
// "from" to "to"
assertEquals(
transformTo.getTranslation(),
CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
assertEquals(
transformTo.getRotation(),
CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo));
assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
// "to" to "from"
assertEquals(
transformFrom.getTranslation(),
CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
assertEquals(
transformFrom.getRotation(),
CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom));
assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
}
@Test
void testEDNtoNWU() {
void testPose3dEDNtoNWU() {
// No rotation from EDN to NWU
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
new Pose3d(
3.0,
@@ -43,7 +67,7 @@ class CoordinateSystemTest {
CoordinateSystem.NWU());
// 45° roll from EDN to NWU
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
new Pose3d(
3.0,
@@ -54,7 +78,7 @@ class CoordinateSystemTest {
CoordinateSystem.NWU());
// 45° pitch from EDN to NWU
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
new Pose3d(
3.0,
@@ -65,7 +89,7 @@ class CoordinateSystemTest {
CoordinateSystem.NWU());
// 45° yaw from EDN to NWU
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
new Pose3d(
3.0,
@@ -80,9 +104,9 @@ class CoordinateSystemTest {
}
@Test
void testEDNtoNED() {
void testPose3dEDNtoNED() {
// No rotation from EDN to NED
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
new Pose3d(
3.0,
@@ -93,7 +117,7 @@ class CoordinateSystemTest {
CoordinateSystem.NED());
// 45° roll from EDN to NED
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
new Pose3d(
3.0,
@@ -104,7 +128,7 @@ class CoordinateSystemTest {
CoordinateSystem.NED());
// 45° pitch from EDN to NED
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
new Pose3d(
3.0,
@@ -115,7 +139,7 @@ class CoordinateSystemTest {
CoordinateSystem.NED());
// 45° yaw from EDN to NED
checkConvert(
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
new Pose3d(
3.0,
@@ -128,4 +152,100 @@ class CoordinateSystemTest {
CoordinateSystem.EDN(),
CoordinateSystem.NED());
}
@Test
void testTransform3dEDNtoNWU() {
// No rotation from EDN to NWU
checkTransform3dConvert(
new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
// 45° roll from EDN to NWU
checkTransform3dConvert(
new Transform3d(
new Translation3d(1.0, 2.0, 3.0),
new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
// 45° pitch from EDN to NWU
checkTransform3dConvert(
new Transform3d(
new Translation3d(1.0, 2.0, 3.0),
new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
// 45° yaw from EDN to NWU
checkTransform3dConvert(
new Transform3d(
new Translation3d(1.0, 2.0, 3.0),
new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
new Transform3d(
new Translation3d(3.0, -1.0, -2.0),
new Rotation3d(
Units.degreesToRadians(-90.0),
Units.degreesToRadians(45.0),
Units.degreesToRadians(-90.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
}
@Test
void testTransform3dEDNtoNED() {
// No rotation from EDN to NED
checkTransform3dConvert(
new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
// 45° roll from EDN to NED
checkTransform3dConvert(
new Transform3d(
new Translation3d(1.0, 2.0, 3.0),
new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
// 45° pitch from EDN to NED
checkTransform3dConvert(
new Transform3d(
new Translation3d(1.0, 2.0, 3.0),
new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
// 45° yaw from EDN to NED
checkTransform3dConvert(
new Transform3d(
new Translation3d(1.0, 2.0, 3.0),
new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
new Transform3d(
new Translation3d(3.0, 1.0, 2.0),
new Rotation3d(
Units.degreesToRadians(90.0),
Units.degreesToRadians(-45.0),
Units.degreesToRadians(90.0))),
CoordinateSystem.EDN(),
CoordinateSystem.NED());
}
}

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

@@ -4,13 +4,14 @@
#include "frc/geometry/CoordinateSystem.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Transform3d.h"
#include "gtest/gtest.h"
using namespace frc;
void CheckConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
const CoordinateSystem& coordFrom,
const CoordinateSystem& coordTo) {
void CheckPose3dConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
const CoordinateSystem& coordFrom,
const CoordinateSystem& coordTo) {
// "from" to "to"
EXPECT_EQ(
poseTo.Translation(),
@@ -28,46 +29,135 @@ void CheckConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
EXPECT_EQ(poseFrom, CoordinateSystem::Convert(poseTo, coordTo, coordFrom));
}
TEST(CoordinateSystemTest, EDNtoNWU) {
void CheckTransform3dConvert(const Transform3d& transformFrom,
const Transform3d& transformTo,
const CoordinateSystem& coordFrom,
const CoordinateSystem& coordTo) {
// "from" to "to"
EXPECT_EQ(transformTo.Translation(),
CoordinateSystem::Convert(transformFrom.Translation(), coordFrom,
coordTo));
EXPECT_EQ(
transformTo.Rotation(),
CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo));
EXPECT_EQ(transformTo,
CoordinateSystem::Convert(transformFrom, coordFrom, coordTo));
// "to" to "from"
EXPECT_EQ(
transformFrom.Translation(),
CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom));
EXPECT_EQ(
transformFrom.Rotation(),
CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom));
EXPECT_EQ(transformFrom,
CoordinateSystem::Convert(transformTo, coordTo, coordFrom));
}
TEST(CoordinateSystemTest, Pose3dEDNtoNWU) {
// 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());
CheckPose3dConvert(
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());
// 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());
CheckPose3dConvert(
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());
// 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());
CheckPose3dConvert(
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());
// 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());
CheckPose3dConvert(
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) {
TEST(CoordinateSystemTest, Pose3dEDNtoNED) {
// 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());
CheckPose3dConvert(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());
// 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());
CheckPose3dConvert(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());
// 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());
CheckPose3dConvert(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());
// 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());
CheckPose3dConvert(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());
}
TEST(CoordinateSystemTest, Transform3dEDNtoNWU) {
// No rotation from EDN to NWU
CheckTransform3dConvert(
Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
Rotation3d{-90_deg, 0_deg, -90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
// 45° roll from EDN to NWU
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{45_deg, 0_deg, 0_deg}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
Rotation3d{-45_deg, 0_deg, -90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
// 45° pitch from EDN to NWU
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 45_deg, 0_deg}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
Rotation3d{-90_deg, 0_deg, -135_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
// 45° yaw from EDN to NWU
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 0_deg, 45_deg}},
Transform3d{Translation3d{3_m, -1_m, -2_m},
Rotation3d{-90_deg, 45_deg, -90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NWU());
}
TEST(CoordinateSystemTest, Transform3dEDNtoNED) {
// No rotation from EDN to NED
CheckTransform3dConvert(
Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
Rotation3d{90_deg, 0_deg, 90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
// 45° roll from EDN to NED
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{45_deg, 0_deg, 0_deg}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
Rotation3d{135_deg, 0_deg, 90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
// 45° pitch from EDN to NED
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 45_deg, 0_deg}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
Rotation3d{90_deg, 0_deg, 135_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
// 45° yaw from EDN to NED
CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
Rotation3d{0_deg, 0_deg, 45_deg}},
Transform3d{Translation3d{3_m, 1_m, 2_m},
Rotation3d{90_deg, -45_deg, 90_deg}},
CoordinateSystem::EDN(), CoordinateSystem::NED());
}