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 8e3a3fe91c..38d7513128 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 @@ -14,6 +14,7 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import java.util.Objects; @@ -200,34 +201,44 @@ public class Pose3d implements Interpolatable { * @return The new pose of the robot. */ public Pose3d exp(Twist3d twist) { - final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz)); - final var OmegaSq = Omega.times(Omega); + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf + final var u = VecBuilder.fill(twist.dx, twist.dy, twist.dz); + final var rvec = VecBuilder.fill(twist.rx, twist.ry, twist.rz); + final var omega = rotationVectorToMatrix(rvec); + final var omegaSq = omega.pow(2); + double theta = rvec.norm(); + double thetaSq = theta * theta; - double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz; - - // Get left Jacobian of SO3. See first line in right column of - // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf - Matrix J; - if (thetaSq < 1E-9 * 1E-9) { - // J = I + 0.5ω - J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5)); + double A; + double B; + double C; + if (theta < 1E-9) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/3! - θ²/5! + θ⁴/7! + A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120; + B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720; + C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040; } else { - double theta = Math.sqrt(thetaSq); - // J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω² - J = - Matrix.eye(Nat.N3()) - .plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq)) - .plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta))); + // A = sin(θ)/θ + // B = (1 - cos(θ)) / θ² + // C = (1 - A) / θ² + A = Math.sin(theta) / theta; + B = (1 - Math.cos(theta)) / thetaSq; + C = (1 - A) / thetaSq; } - // Get translation component - final var translation = - J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz)); - + Matrix R = Matrix.eye(Nat.N3()).plus(omega.times(A)).plus(omegaSq.times(B)); + Matrix V = Matrix.eye(Nat.N3()).plus(omega.times(B)).plus(omegaSq.times(C)); + Matrix translation_component = V.times(u); final var transform = new Transform3d( - new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)), - new Rotation3d(twist.rx, twist.ry, twist.rz)); + new Translation3d( + translation_component.get(0, 0), + translation_component.get(1, 0), + translation_component.get(2, 0)), + new Rotation3d(R)); return this.plus(transform); } @@ -240,50 +251,43 @@ public class Pose3d implements Interpolatable { * @return The twist that maps this to end. */ public Twist3d log(Pose3d end) { + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf final var transform = end.relativeTo(this); - final var rotVec = transform.getRotation().getQuaternion().toRotationVector(); + final var rvec = transform.getRotation().getQuaternion().toRotationVector(); - final var Omega = rotationVectorToMatrix(rotVec); - final var OmegaSq = Omega.times(Omega); + final var omega = rotationVectorToMatrix(rvec); + final var theta = transform.getRotation().getAngle(); + final var thetaSq = theta * theta; - double thetaSq = - rotVec.get(0, 0) * rotVec.get(0, 0) - + rotVec.get(1, 0) * rotVec.get(1, 0) - + rotVec.get(2, 0) * rotVec.get(2, 0); - - // Get left Jacobian inverse of SO3. See fourth line in right column of - // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf - Matrix Jinv; - if (thetaSq < 1E-9 * 1E-9) { - // J⁻¹ = I − 0.5ω + 1/12 ω² - Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0)); + double C; + if (theta < 1E-9) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!) + C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040; } else { - double theta = Math.sqrt(thetaSq); - double halfTheta = 0.5 * theta; - - // J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω² - Jinv = - Matrix.eye(Nat.N3()) - .minus(Omega.times(0.5)) - .plus( - OmegaSq.times( - (1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq)); + // A = sin(θ)/θ + // B = (1 - cos(θ)) / θ² + // C = (1 - A/(2*B)) / θ² + double A = Math.sin(theta) / theta; + double B = (1 - Math.cos(theta)) / thetaSq; + C = (1 - A / (2 * B)) / thetaSq; } - // Get dtranslation component - final var dtranslation = - Jinv.times( - new MatBuilder<>(Nat.N3(), Nat.N1()) - .fill(transform.getX(), transform.getY(), transform.getZ())); + final var V_inv = Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omega.pow(2).times(C)); + + final var twist_translation = + V_inv.times(VecBuilder.fill(transform.getX(), transform.getY(), transform.getZ())); return new Twist3d( - dtranslation.get(0, 0), - dtranslation.get(1, 0), - dtranslation.get(2, 0), - rotVec.get(0, 0), - rotVec.get(1, 0), - rotVec.get(2, 0)); + twist_translation.get(0, 0), + twist_translation.get(1, 0), + twist_translation.get(2, 0), + rvec.get(0, 0), + rvec.get(1, 0), + rvec.get(2, 0)); } /** diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 69d37c219e..65afa58cb4 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -71,73 +71,86 @@ Pose3d Pose3d::RelativeTo(const Pose3d& other) const { } Pose3d Pose3d::Exp(const Twist3d& twist) const { - Matrixd<3, 3> Omega = RotationVectorToMatrix( - Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()}); - Matrixd<3, 3> OmegaSq = Omega * Omega; + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf + auto u = Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()}; + auto rvec = Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()}; + auto omega = RotationVectorToMatrix(rvec); + auto omegaSq = omega * omega; + auto theta = rvec.norm(); + auto thetaSq = theta * theta; - double thetaSq = - (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value(); - - // Get left Jacobian of SO3. See first line in right column of - // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf - Matrixd<3, 3> J; - if (thetaSq < 1E-9 * 1E-9) { - // J = I + 0.5ω - J = Matrixd<3, 3>::Identity() + 0.5 * Omega; + double A; + double B; + double C; + if (theta < 1E-9) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/3! - θ²/5! + θ⁴/7! + A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120; + B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720; + C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040; } else { - double theta = std::sqrt(thetaSq); - // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω² - J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega + - (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq; + // A = std::sin(θ)/θ + // B = (1 - std::cos(θ)) / θ² + // C = (1 - A) / θ² + A = std::sin(theta) / theta; + B = (1 - std::cos(theta)) / thetaSq; + C = (1 - A) / thetaSq; } - // Get translation component - Vectord<3> translation = - J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()}; + auto R = Matrixd<3, 3>::Identity() + A * omega + B * omegaSq; + auto V = Matrixd<3, 3>::Identity() + B * omega + C * omegaSq; - const Transform3d transform{Translation3d{units::meter_t{translation(0)}, - units::meter_t{translation(1)}, - units::meter_t{translation(2)}}, - Rotation3d{twist.rx, twist.ry, twist.rz}}; + auto translation_component = V * u; + const Transform3d transform{ + Translation3d{units::meter_t{translation_component(0)}, + units::meter_t{translation_component(1)}, + units::meter_t{translation_component(2)}}, + Rotation3d{R}}; return *this + transform; } Twist3d Pose3d::Log(const Pose3d& end) const { + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf const auto transform = end.RelativeTo(*this); - Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector(); + auto u = Vectord<3>{transform.X().value(), transform.Y().value(), + transform.Z().value()}; + auto rvec = transform.Rotation().GetQuaternion().ToRotationVector(); - Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec); - Matrixd<3, 3> OmegaSq = Omega * Omega; + auto omega = RotationVectorToMatrix(rvec); + auto omegaSq = omega * omega; + auto theta = rvec.norm(); + auto thetaSq = theta * theta; - double thetaSq = rotVec.squaredNorm(); - - // Get left Jacobian inverse of SO3. See fourth line in right column of - // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf - Matrixd<3, 3> Jinv; - if (thetaSq < 1E-9 * 1E-9) { - // J⁻¹ = I − 0.5ω + 1/12 ω² - Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq; + double C; + if (theta < 1E-9) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!) + C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040; } else { - double theta = std::sqrt(thetaSq); - double halfTheta = 0.5 * theta; - - // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω² - Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + - (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) / - thetaSq * OmegaSq; + // A = std::sin(θ)/θ + // B = (1 - std::cos(θ)) / θ² + // C = (1 - A/(2*B)) / θ² + double A = std::sin(theta) / theta; + double B = (1 - std::cos(theta)) / thetaSq; + C = (1 - A / (2 * B)) / thetaSq; } - // Get dtranslation component - Vectord<3> dtranslation = - Jinv * Vectord<3>{transform.X().value(), transform.Y().value(), - transform.Z().value()}; + auto V_inv = Matrixd<3, 3>::Identity() - 0.5 * omega + C * omegaSq; - return Twist3d{ - units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)}, - units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)}, - units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}}; + auto translation_component = V_inv * u; + + return Twist3d{units::meter_t{translation_component(0)}, + units::meter_t{translation_component(1)}, + units::meter_t{translation_component(2)}, + units::radian_t{rvec(0)}, + units::radian_t{rvec(1)}, + units::radian_t{rvec(2)}}; } Pose2d Pose3d::ToPose2d() const { 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 f13819f03d..f317066357 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 @@ -10,6 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.util.Units; +import java.util.Arrays; import org.junit.jupiter.api.Test; class Pose3dTest { @@ -49,10 +50,13 @@ class Pose3dTest { 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 + // 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 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 = @@ -153,4 +157,77 @@ class Pose3dTest { assertEquals(expected, pose.toPose2d()); } + + @Test + void testComplexTwists() { + var initial_poses = + Arrays.asList( + new Pose3d( + new Translation3d(0.698303, -0.959096, 0.271076), + new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))), + new Pose3d( + new Translation3d(0.634892, -0.765209, 0.117543), + new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))), + new Pose3d( + new Translation3d(0.584827, -0.590303, -0.02557), + new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))), + new Pose3d( + new Translation3d(0.505038, -0.451479, -0.112835), + new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))), + new Pose3d( + new Translation3d(0.428178, -0.329692, -0.189707), + new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157)))); + + var final_poses = + Arrays.asList( + new Pose3d( + new Translation3d(-0.230448, -0.511957, 0.198406), + new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))), + new Pose3d( + new Translation3d(-0.088932, -0.343253, 0.095018), + new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))), + new Pose3d( + new Translation3d(-0.107908, -0.317552, 0.133946), + new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))), + new Pose3d( + new Translation3d(-0.123383, -0.156411, -0.047435), + new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))), + new Pose3d( + new Translation3d(-0.084654, -0.019305, -0.030022), + new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873)))); + + final var eps = 1E-5; + for (int i = 0; i < initial_poses.size(); i++) { + var start = initial_poses.get(i); + var end = final_poses.get(i); + + var twist = start.log(end); + var start_exp = start.exp(twist); + + assertAll( + () -> assertEquals(start_exp.getX(), end.getX(), eps), + () -> assertEquals(start_exp.getY(), end.getY(), eps), + () -> assertEquals(start_exp.getZ(), end.getZ(), eps), + () -> + assertEquals( + start_exp.getRotation().getQuaternion().getW(), + end.getRotation().getQuaternion().getW(), + eps), + () -> + assertEquals( + start_exp.getRotation().getQuaternion().getX(), + end.getRotation().getQuaternion().getX(), + eps), + () -> + assertEquals( + start_exp.getRotation().getQuaternion().getY(), + end.getRotation().getQuaternion().getY(), + eps), + () -> + assertEquals( + start_exp.getRotation().getQuaternion().getZ(), + end.getRotation().getQuaternion().getZ(), + eps)); + } + } } diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 8c4452cfcc..2ed74158c3 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -4,6 +4,8 @@ #include +#include + #include "frc/geometry/Pose3d.h" #include "gtest/gtest.h" @@ -95,3 +97,53 @@ TEST(Pose3dTest, ToPose2d) { EXPECT_EQ(expected, pose.ToPose2d()); } + +TEST(Pose3dTest, ComplexTwists) { + wpi::array initial_poses{ + Pose3d{0.698303_m, -0.959096_m, 0.271076_m, + Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}}, + Pose3d{0.634892_m, -0.765209_m, 0.117543_m, + Rotation3d{Quaternion{0.84987, -0.070829, 0.162097, 0.496415}}}, + Pose3d{0.584827_m, -0.590303_m, -0.02557_m, + Rotation3d{Quaternion{0.832743, -0.041991, 0.202188, 0.513708}}}, + Pose3d{0.505038_m, -0.451479_m, -0.112835_m, + Rotation3d{Quaternion{0.816515, -0.002673, 0.226182, 0.531166}}}, + Pose3d{0.428178_m, -0.329692_m, -0.189707_m, + Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}}, + }; + + wpi::array final_poses{ + Pose3d{-0.230448_m, -0.511957_m, 0.198406_m, + Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}}, + Pose3d{-0.088932_m, -0.343253_m, 0.095018_m, + Rotation3d{Quaternion{0.638738, 0.413016, 0.536281, 0.365833}}}, + Pose3d{-0.107908_m, -0.317552_m, 0.133946_m, + Rotation3d{Quaternion{0.653444, 0.417069, 0.465505, 0.427046}}}, + Pose3d{-0.123383_m, -0.156411_m, -0.047435_m, + Rotation3d{Quaternion{0.652983, 0.40644, 0.431566, 0.47135}}}, + Pose3d{-0.084654_m, -0.019305_m, -0.030022_m, + Rotation3d{Quaternion{0.620243, 0.429104, 0.479384, 0.44873}}}, + }; + + for (size_t i = 0; i < initial_poses.size(); i++) { + auto start = initial_poses[i]; + auto end = final_poses[i]; + + auto twist = start.Log(end); + auto start_exp = start.Exp(twist); + + auto eps = 1E-5; + + EXPECT_NEAR(start_exp.X().value(), end.X().value(), eps); + EXPECT_NEAR(start_exp.Y().value(), end.Y().value(), eps); + EXPECT_NEAR(start_exp.Z().value(), end.Z().value(), eps); + EXPECT_NEAR(start_exp.Rotation().GetQuaternion().W(), + end.Rotation().GetQuaternion().W(), eps); + EXPECT_NEAR(start_exp.Rotation().GetQuaternion().X(), + end.Rotation().GetQuaternion().X(), eps); + EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Y(), + end.Rotation().GetQuaternion().Y(), eps); + EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Z(), + end.Rotation().GetQuaternion().Z(), eps); + } +}