Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add affine transformation constructors and getters to geometry API (#7430)

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 15:49:17 -08:00
committed by GitHub
parent f772bb141d
commit e222efaa01
32 changed files with 615 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
View File

@@ -153,6 +153,7 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
}
@Test
@SuppressWarnings("removal")
void testPoseToVector() {
Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
var vector = StateSpaceUtil.poseToVector(pose);

8
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -86,6 +86,14 @@ class Pose2dTest {
assertNotEquals(one, two);
}
@Test
void testToMatrix() {
var before = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(20.0));
var after = new Pose2d(before.toMatrix());
assertEquals(before, after);
}
@Test
void testMinus() {
var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));

16
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -169,6 +169,22 @@ class Pose3dTest {
() -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon));
}
@Test
void testToMatrix() {
var before =
new Pose3d(
1.0,
2.0,
3.0,
new Rotation3d(
Units.degreesToRadians(20.0),
Units.degreesToRadians(30.0),
Units.degreesToRadians(40.0)));
var after = new Pose3d(before.toMatrix());
assertEquals(before, after);
}
@Test
void testToPose2d() {
var pose =

8
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
View File

@@ -100,6 +100,14 @@ class Rotation2dTest {
assertNotEquals(rot1, rot2);
}
@Test
void testToMatrix() {
var before = Rotation2d.fromDegrees(20.0);
var after = new Rotation2d(before.toMatrix());
assertEquals(before, after);
}
@Test
void testInterpolate() {
// 50 + (70 - 50) * 0.5 = 60

12
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
View File

@@ -344,6 +344,18 @@ class Rotation3dTest {
assertNotEquals(rot1, rot2);
}
@Test
void testToMatrix() {
var before =
new Rotation3d(
Units.degreesToRadians(10.0),
Units.degreesToRadians(20.0),
Units.degreesToRadians(30.0));
var after = new Rotation3d(before.toMatrix());
assertEquals(before, after);
}
@Test
void testInterpolate() {
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);

8
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
View File

@@ -22,6 +22,14 @@ class Transform2dTest {
assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon);
}
@Test
void testToMatrix() {
var before = new Transform2d(1.0, 2.0, Rotation2d.fromDegrees(20.0));
var after = new Transform2d(before.toMatrix());
assertEquals(before, after);
}
@Test
void testInverse() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));

16
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
View File

@@ -11,6 +11,22 @@ import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
class Transform3dTest {
@Test
void testToMatrix() {
var before =
new Transform3d(
1.0,
2.0,
3.0,
new Rotation3d(
Units.degreesToRadians(20.0),
Units.degreesToRadians(30.0),
Units.degreesToRadians(40.0)));
var after = new Transform3d(before.toMatrix());
assertEquals(before, after);
}
@Test
void testInverse() {
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);

7
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -141,6 +141,13 @@ TEST(Pose2dTest, Nearest) {
.value());
}
TEST(Pose2dTest, ToMatrix) {
Pose2d before{1_m, 2_m, 20_deg};
Pose2d after{before.ToMatrix()};
EXPECT_EQ(before, after);
}
TEST(Pose2dTest, Constexpr) {
constexpr Pose2d defaultConstructed;
constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},

7
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -112,6 +112,13 @@ TEST(Pose3dTest, Minus) {
EXPECT_NEAR(0.0, transform.Rotation().Z().value(), 1e-9);
}
TEST(Pose3dTest, ToMatrix) {
Pose3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}};
Pose3d after{before.ToMatrix()};
EXPECT_EQ(before, after);
}
TEST(Pose3dTest, ToPose2d) {
Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}};
Pose2d expected{1_m, 2_m, 40_deg};

7
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -78,6 +78,13 @@ TEST(Rotation2dTest, Inequality) {
EXPECT_NE(rot1, rot2);
}
TEST(Rotation2dTest, ToMatrix) {
Rotation2d before{20_deg};
Rotation2d after{before.ToMatrix()};
EXPECT_EQ(before, after);
}
TEST(Rotation2dTest, Constexpr) {
constexpr Rotation2d defaultCtor;
constexpr Rotation2d radianCtor{5_rad};

7
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -307,6 +307,13 @@ TEST(Rotation3dTest, Inequality) {
EXPECT_NE(rot1, rot2);
}
TEST(Rotation3dTest, ToMatrix) {
Rotation3d before{10_deg, 20_deg, 30_deg};
Rotation3d after{before.ToMatrix()};
EXPECT_EQ(before, after);
}
TEST(Rotation3dTest, Interpolate) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};

7
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -13,6 +13,13 @@
using namespace frc;
TEST(Transform2dTest, ToMatrix) {
Transform2d before{1_m, 2_m, 20_deg};
Transform2d after{before.ToMatrix()};
EXPECT_EQ(before, after);
}
TEST(Transform2dTest, Inverse) {
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{{5_m, 0_m}, 5_deg};

7
wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
View File

@@ -13,6 +13,13 @@
using namespace frc;
TEST(Transform3dTest, ToMatrix) {
Transform3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}};
Transform3d after{before.ToMatrix()};
EXPECT_EQ(before, after);
}
TEST(Transform3dTest, Inverse) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};