diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 227ba33ade..ed5f4f0a01 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -145,7 +145,7 @@ public class Pose2d implements Interpolatable { public Pose2d transformBy(Transform2d other) { return new Pose2d( m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - m_rotation.plus(other.getRotation())); + other.getRotation().plus(m_rotation)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 4e32909d6a..59c75e8fcc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -165,7 +165,7 @@ public class Pose3d implements Interpolatable { public Pose3d transformBy(Transform3d other) { return new Pose3d( m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - m_rotation.plus(other.getRotation())); + other.getRotation().plus(m_rotation)); } /** diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 7c5aa8be41..8909cc8d10 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -70,7 +70,7 @@ Pose3d Pose3d::operator/(double scalar) const { Pose3d Pose3d::TransformBy(const Transform3d& other) const { return {m_translation + (other.Translation().RotateBy(m_rotation)), - m_rotation + other.Rotation()}; + other.Rotation() + m_rotation}; } Pose3d Pose3d::RelativeTo(const Pose3d& other) const { diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc index feeadfa92b..c549f26963 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc @@ -33,7 +33,7 @@ constexpr Pose2d Pose2d::operator/(double scalar) const { constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const { return {m_translation + (other.Translation().RotateBy(m_rotation)), - m_rotation + other.Rotation()}; + other.Rotation() + m_rotation}; } } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index 96ea6a9ed4..f13819f03d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -15,6 +15,61 @@ import org.junit.jupiter.api.Test; class Pose3dTest { private static final double kEpsilon = 1E-9; + @Test + void testTransformByRotations() { + var initialPose = + new Pose3d( + new Translation3d(0.0, 0.0, 0.0), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(0.0), + Units.degreesToRadians(0.0))); + + var transform1 = + new Transform3d( + new Translation3d(0.0, 0.0, 0.0), + new Rotation3d( + Units.degreesToRadians(90.0), + Units.degreesToRadians(45.0), + Units.degreesToRadians(0.0))); + + var transform2 = + new Transform3d( + new Translation3d(0.0, 0.0, 0.0), + new Rotation3d( + Units.degreesToRadians(-90.0), + Units.degreesToRadians(0.0), + Units.degreesToRadians(0.0))); + + var transform3 = + new Transform3d( + new Translation3d(0.0, 0.0, 0.0), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-45.0), + Units.degreesToRadians(0.0))); + + // This sequence of rotations should diverge from the origin and eventually return to it. When + // each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and + // with + // the axes of the pose that is being transformed, just like how the translation is done 'from + // the + // viewpoint' of the pose that is being transformed. + var finalPose = + initialPose.transformBy(transform1).transformBy(transform2).transformBy(transform3); + + assertAll( + () -> + assertEquals( + finalPose.getRotation().getX(), initialPose.getRotation().getX(), kEpsilon), + () -> + assertEquals( + finalPose.getRotation().getY(), initialPose.getRotation().getY(), kEpsilon), + () -> + assertEquals( + finalPose.getRotation().getZ(), initialPose.getRotation().getZ(), kEpsilon)); + } + @Test void testTransformBy() { var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 46e4a28ae1..8c4452cfcc 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -9,6 +9,29 @@ using namespace frc; +TEST(Pose3dTest, TestTransformByRotations) { + const double kEpsilon = 1E-9; + + const Pose3d initialPose{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}; + const Transform3d transform1{Translation3d{0_m, 0_m, 0_m}, + Rotation3d{90_deg, 45_deg, 0_deg}}; + const Transform3d transform2{Translation3d{0_m, 0_m, 0_m}, + Rotation3d{-90_deg, 0_deg, 0_deg}}; + const Transform3d transform3{Translation3d{0_m, 0_m, 0_m}, + Rotation3d{0_deg, -45_deg, 0_deg}}; + + Pose3d finalPose = initialPose.TransformBy(transform1) + .TransformBy(transform2) + .TransformBy(transform3); + + EXPECT_NEAR(finalPose.Rotation().X().value(), + initialPose.Rotation().X().value(), kEpsilon); + EXPECT_NEAR(finalPose.Rotation().Y().value(), + initialPose.Rotation().Y().value(), kEpsilon); + EXPECT_NEAR(finalPose.Rotation().Z().value(), + initialPose.Rotation().Z().value(), kEpsilon); +} + TEST(Pose3dTest, TransformBy) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0};