From 9c0f1eeffc8af513aba931e32b4ae617dbfc0bf4 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 14 Aug 2026 20:41:09 -0700 Subject: [PATCH] [wpimath] Implement Twist3d.exp() and Transform3d.log() with EJML --- wpimath/CMakeLists.txt | 2 - .../wpilib/math/geometry/GeometryUtil.java | 39 ++++++++++++ .../org/wpilib/math/geometry/Transform3d.java | 60 +++++++++++++------ .../org/wpilib/math/geometry/Twist3d.java | 56 ++++++++++++++--- .../org/wpilib/math/jni/Transform3dJNI.java | 34 ----------- .../java/org/wpilib/math/jni/Twist3dJNI.java | 32 ---------- .../main/native/cpp/jni/Transform3dJNI.cpp | 42 ------------- .../src/main/native/cpp/jni/Twist3dJNI.cpp | 42 ------------- .../wpilib/math/jni/Transform3dJNITest.java | 16 ----- .../org/wpilib/math/jni/Twist3dJNITest.java | 16 ----- 10 files changed, 131 insertions(+), 208 deletions(-) create mode 100644 wpimath/src/main/java/org/wpilib/math/geometry/GeometryUtil.java delete mode 100644 wpimath/src/main/java/org/wpilib/math/jni/Transform3dJNI.java delete mode 100644 wpimath/src/main/java/org/wpilib/math/jni/Twist3dJNI.java delete mode 100644 wpimath/src/main/native/cpp/jni/Transform3dJNI.cpp delete mode 100644 wpimath/src/main/native/cpp/jni/Twist3dJNI.cpp delete mode 100644 wpimath/src/test/java/org/wpilib/math/jni/Transform3dJNITest.java delete mode 100644 wpimath/src/test/java/org/wpilib/math/jni/Twist3dJNITest.java diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 4eeedefd01f..20d9bdec980 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -10,8 +10,6 @@ file( src/main/native/cpp/jni/EigenJNI.cpp src/main/native/cpp/jni/Exceptions.cpp src/main/native/cpp/jni/LinearSystemUtilJNI.cpp - src/main/native/cpp/jni/Transform3dJNI.cpp - src/main/native/cpp/jni/Twist3dJNI.cpp src/main/native/cpp/jni/autodiff/GradientJNI.cpp src/main/native/cpp/jni/autodiff/HessianJNI.cpp src/main/native/cpp/jni/autodiff/JacobianJNI.cpp diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/GeometryUtil.java b/wpimath/src/main/java/org/wpilib/math/geometry/GeometryUtil.java new file mode 100644 index 00000000000..0f80fc2177c --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/geometry/GeometryUtil.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.math.geometry; + +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.Vector; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.util.Nat; + +/** Geometry utilities. */ +final class GeometryUtil { + private GeometryUtil() { + throw new AssertionError("utility class"); + } + + /** + * Converts a rotation vector to a rotation matrix. + * + * @param rotation The rotation vector. + * @return The rotation matrix. + */ + static Matrix rotationVectorToMatrix(Vector rotation) { + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + 0.0, + -rotation.get(2, 0), + rotation.get(1, 0), + rotation.get(2, 0), + 0.0, + -rotation.get(0, 0), + -rotation.get(1, 0), + rotation.get(0, 0), + 0.0); + } +} diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java index 40820bd9261..99c001bb336 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java @@ -9,9 +9,9 @@ import java.util.Objects; import org.wpilib.math.geometry.proto.Transform3dProto; import org.wpilib.math.geometry.struct.Transform3dStruct; -import org.wpilib.math.jni.Transform3dJNI; import org.wpilib.math.linalg.MatBuilder; import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N4; import org.wpilib.math.util.Nat; import org.wpilib.units.measure.Distance; @@ -264,23 +264,49 @@ public Rotation3d getRotation() { * @return The twist that maps the current transform. */ public Twist3d log() { - var thisQuaternion = m_rotation.getQuaternion(); - double[] resultArray = - Transform3dJNI.log( - this.getX(), - this.getY(), - this.getZ(), - thisQuaternion.getW(), - thisQuaternion.getX(), - thisQuaternion.getY(), - thisQuaternion.getZ()); + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf + + var u = VecBuilder.fill(m_translation.getX(), m_translation.getY(), m_translation.getZ()); + var rvec = m_rotation.toVector(); + var omega = GeometryUtil.rotationVectorToMatrix(rvec); + var omegaSq = omega.times(omega); + double theta = rvec.norm(); + double thetaSq = theta * theta; + + double C; + if (Math.abs(theta) < 1E-7) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!) + // sources: + // A: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0 + // B: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + // C: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240; + } else { + // A = sinθ/θ + // B = (1 - cosθ)/θ² + // C = (1 - A/(2B))/θ² + double A = Math.sin(theta) / theta; + double B = (1 - Math.cos(theta)) / thetaSq; + C = (1 - A / (2 * B)) / thetaSq; + } + + var V_inv = Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omegaSq.times(C)); + + var translation_component = V_inv.times(u); + return new Twist3d( - resultArray[0], - resultArray[1], - resultArray[2], - resultArray[3], - resultArray[4], - resultArray[5]); + translation_component.get(0, 0), + translation_component.get(1, 0), + translation_component.get(2, 0), + rvec.get(0, 0), + rvec.get(1, 0), + rvec.get(2, 0)); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Twist3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Twist3d.java index 0e9cebf352e..df0f19434d7 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Twist3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Twist3d.java @@ -7,7 +7,9 @@ import java.util.Objects; import org.wpilib.math.geometry.proto.Twist3dProto; import org.wpilib.math.geometry.struct.Twist3dStruct; -import org.wpilib.math.jni.Twist3dJNI; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.util.Nat; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; @@ -74,13 +76,53 @@ public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) * @return The pose delta of the robot. */ public Transform3d exp() { - double[] resultArray = Twist3dJNI.exp(dx, dy, dz, rx, ry, rz); + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf + + var u = VecBuilder.fill(dx, dy, dz); + var rvec = VecBuilder.fill(rx, ry, rz); + var omega = GeometryUtil.rotationVectorToMatrix(rvec); + var omegaSq = omega.times(omega); + double theta = rvec.norm(); + double thetaSq = theta * theta; + + double A; + double B; + double C; + if (Math.abs(theta) < 1E-7) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/3! - θ²/5! + θ⁴/7! + // sources: + // A: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0 + // B: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + // C: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + 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 { + // A = sinθ/θ + // B = (1 - cosθ)/θ² + // C = (1 - A)/θ² + A = Math.sin(theta) / theta; + B = (1 - Math.cos(theta)) / thetaSq; + C = (1 - A) / thetaSq; + } + + var R = Matrix.eye(Nat.N3()).plus(omega.times(A)).plus(omegaSq.times(B)); + var V = Matrix.eye(Nat.N3()).plus(omega.times(B)).plus(omegaSq.times(C)); + + var translation_component = V.times(u); + return new Transform3d( - resultArray[0], - resultArray[1], - resultArray[2], - new Rotation3d( - new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6]))); + new Translation3d( + translation_component.get(0, 0), + translation_component.get(1, 0), + translation_component.get(2, 0)), + new Rotation3d(R)); } @Override diff --git a/wpimath/src/main/java/org/wpilib/math/jni/Transform3dJNI.java b/wpimath/src/main/java/org/wpilib/math/jni/Transform3dJNI.java deleted file mode 100644 index d3cb657a208..00000000000 --- a/wpimath/src/main/java/org/wpilib/math/jni/Transform3dJNI.java +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.wpilib.math.jni; - -/** Transform3d JNI. */ -public final class Transform3dJNI extends WPIMathJNI { - /** - * Returns a Twist3d that maps the Transform3d (pose delta). - * - *

The double array returned is of the form [dx, dy, dz, rx, ry, rz]. - * - * @param relX The transform's translational X component. - * @param relY The transform's translational Y component. - * @param relZ The transform's translational Z component. - * @param relQw The transform quaternion's W component. - * @param relQx The transform quaternion's X component. - * @param relQy The transform quaternion's Y component. - * @param relQz The transform quaternion's Z component. - * @return The twist that maps start to end as a double array. - */ - public static native double[] log( - double relX, - double relY, - double relZ, - double relQw, - double relQx, - double relQy, - double relQz); - - /** Utility class. */ - private Transform3dJNI() {} -} diff --git a/wpimath/src/main/java/org/wpilib/math/jni/Twist3dJNI.java b/wpimath/src/main/java/org/wpilib/math/jni/Twist3dJNI.java deleted file mode 100644 index db66946080a..00000000000 --- a/wpimath/src/main/java/org/wpilib/math/jni/Twist3dJNI.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.wpilib.math.jni; - -/** Twist3d JNI. */ -public final class Twist3dJNI extends WPIMathJNI { - /** - * Obtain a Transform3d from a (constant curvature) velocity. - * - *

The double array returned is of the form [dx, dy, dz, qw, qx, qy, qz]. - * - * @param twistDx The twist's dx value. - * @param twistDy The twist's dy value. - * @param twistDz The twist's dz value. - * @param twistRx The twist's rx value. - * @param twistRy The twist's ry value. - * @param twistRz The twist's rz value. - * @return The new pose as a double array. - */ - public static native double[] exp( - double twistDx, - double twistDy, - double twistDz, - double twistRx, - double twistRy, - double twistRz); - - /** Utility class. */ - private Twist3dJNI() {} -} diff --git a/wpimath/src/main/native/cpp/jni/Transform3dJNI.cpp b/wpimath/src/main/native/cpp/jni/Transform3dJNI.cpp deleted file mode 100644 index b46fe916f07..00000000000 --- a/wpimath/src/main/native/cpp/jni/Transform3dJNI.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include - -#include "org_wpilib_math_jni_Transform3dJNI.h" -#include "wpi/math/geometry/Quaternion.hpp" -#include "wpi/math/geometry/Rotation3d.hpp" -#include "wpi/math/geometry/Transform3d.hpp" -#include "wpi/math/geometry/Twist3d.hpp" -#include "wpi/units/angle.hpp" -#include "wpi/units/length.hpp" -#include "wpi/util/jni_util.hpp" - -using namespace wpi::util::java; - -extern "C" { - -/* - * Class: org_wpilib_math_jni_Transform3dJNI - * Method: log - * Signature: (DDDDDDD)[D - */ -JNIEXPORT jdoubleArray JNICALL -Java_org_wpilib_math_jni_Transform3dJNI_log - (JNIEnv* env, jclass, jdouble relX, jdouble relY, jdouble relZ, jdouble relQw, - jdouble relQx, jdouble relQy, jdouble relQz) -{ - wpi::math::Transform3d transform3d{ - wpi::units::meter_t{relX}, wpi::units::meter_t{relY}, - wpi::units::meter_t{relZ}, - wpi::math::Rotation3d{wpi::math::Quaternion{relQw, relQx, relQy, relQz}}}; - - wpi::math::Twist3d result = transform3d.Log(); - - return MakeJDoubleArray( - env, {{result.dx.value(), result.dy.value(), result.dz.value(), - result.rx.value(), result.ry.value(), result.rz.value()}}); -} - -} // extern "C" diff --git a/wpimath/src/main/native/cpp/jni/Twist3dJNI.cpp b/wpimath/src/main/native/cpp/jni/Twist3dJNI.cpp deleted file mode 100644 index 23fd35bc7dc..00000000000 --- a/wpimath/src/main/native/cpp/jni/Twist3dJNI.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include - -#include "org_wpilib_math_jni_Twist3dJNI.h" -#include "wpi/math/geometry/Transform3d.hpp" -#include "wpi/math/geometry/Twist3d.hpp" -#include "wpi/units/angle.hpp" -#include "wpi/units/length.hpp" -#include "wpi/util/jni_util.hpp" - -using namespace wpi::util::java; - -extern "C" { - -/* - * Class: org_wpilib_math_jni_Twist3dJNI - * Method: exp - * Signature: (DDDDDD)[D - */ -JNIEXPORT jdoubleArray JNICALL -Java_org_wpilib_math_jni_Twist3dJNI_exp - (JNIEnv* env, jclass, jdouble twistDx, jdouble twistDy, jdouble twistDz, - jdouble twistRx, jdouble twistRy, jdouble twistRz) -{ - wpi::math::Twist3d twist{ - wpi::units::meter_t{twistDx}, wpi::units::meter_t{twistDy}, - wpi::units::meter_t{twistDz}, wpi::units::radian_t{twistRx}, - wpi::units::radian_t{twistRy}, wpi::units::radian_t{twistRz}}; - - wpi::math::Transform3d result = twist.Exp(); - - const auto& resultQuaternion = result.Rotation().GetQuaternion(); - return MakeJDoubleArray( - env, {{result.X().value(), result.Y().value(), result.Z().value(), - resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(), - resultQuaternion.Z()}}); -} - -} // extern "C" diff --git a/wpimath/src/test/java/org/wpilib/math/jni/Transform3dJNITest.java b/wpimath/src/test/java/org/wpilib/math/jni/Transform3dJNITest.java deleted file mode 100644 index 6c1d096cf3c..00000000000 --- a/wpimath/src/test/java/org/wpilib/math/jni/Transform3dJNITest.java +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.wpilib.math.jni; - -import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; - -import org.junit.jupiter.api.Test; - -public class Transform3dJNITest { - @Test - public void testLink() { - assertDoesNotThrow(Transform3dJNI::forceLoad); - } -} diff --git a/wpimath/src/test/java/org/wpilib/math/jni/Twist3dJNITest.java b/wpimath/src/test/java/org/wpilib/math/jni/Twist3dJNITest.java deleted file mode 100644 index 1707318eff5..00000000000 --- a/wpimath/src/test/java/org/wpilib/math/jni/Twist3dJNITest.java +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.wpilib.math.jni; - -import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; - -import org.junit.jupiter.api.Test; - -public class Twist3dJNITest { - @Test - public void testLink() { - assertDoesNotThrow(Twist3dJNI::forceLoad); - } -}