From b88f064ce1b7fe97911eb99902783cd058d1ef12 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Thu, 13 Jul 2023 17:55:32 -0700 Subject: [PATCH 01/21] Add JNI for Pose3d exp and log --- .../java/edu/wpi/first/math/WPIMathJNI.java | 20 ++ .../src/main/native/cpp/jni/WPIMathJNI.cpp | 236 ++++++++++++++++++ .../edu/wpi/first/math/WPIMathJNITest.java | 22 ++ 3 files changed, 278 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index d86d141561e..e6bc7c820f6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -4,6 +4,8 @@ package edu.wpi.first.math; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; @@ -76,6 +78,24 @@ public static native void dare( */ public static native void pow(double[] src, int rows, double exponent, double[] dst); + /** + * Obtain a Pose3d from a (constant curvature) velocity. + * + * @param start The starting Pose3d. + * @param twist The twist to apply. + * @return The new pose. + */ + public static native Pose3d expPose3d(Pose3d start, Twist3d twist); + + /** + * Returns a Twist3d that maps this pose to the end pose. + * + * @param start The starting Pose3d. + * @param end The ending Pose3d. + * @return The twist that maps start to end. + */ + public static native Twist3d logPose3d(Pose3d start, Pose3d end); + /** * Returns true if (A, B) is a stabilizable pair. * diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 4cdcf706f62..6a7b26dc99f 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -14,6 +14,7 @@ #include "Eigen/QR" #include "edu_wpi_first_math_WPIMathJNI.h" #include "frc/DARE.h" +#include "frc/geometry/Pose3d.h" #include "frc/trajectory/TrajectoryUtil.h" #include "unsupported/Eigen/MatrixFunctions" @@ -57,6 +58,207 @@ bool IsStabilizable(const Eigen::Ref& A, } // namespace +static JClass pose3dCls; +static JClass quaternionCls; +static JClass rotation3dCls; +static JClass translation3dCls; +static JClass twist3dCls; + +static const JClassInit classes[] = { + {"edu/wpi/first/math/geometry/Pose3d", &pose3dCls}, + {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls}, + {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls}, + {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}, + {"edu/wpi/first/math/geometry/Twist3d", &twist3dCls}}; + +extern "C" { + +JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { + JNIEnv* env; + if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return JNI_ERR; + } + + for (auto& c : classes) { + *c.cls = JClass(env, c.name); + if (!*c.cls) { + return JNI_ERR; + } + } + + return JNI_VERSION_1_6; +} + +JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { + JNIEnv* env; + if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return; + } + + for (auto& c : classes) { + c.cls->free(env); + } +} + +} // extern "C" + +frc::Pose3d FromJavaPose3d(JNIEnv* env, jobject pose) { + static jfieldID poseTranslationField = nullptr; + static jfieldID poseRotationField = nullptr; + static jfieldID translationXField = nullptr; + static jfieldID translationYField = nullptr; + static jfieldID translationZField = nullptr; + static jfieldID rotationQuaternionField = nullptr; + static jfieldID quaternionWField = nullptr; + static jmethodID quaternionXMethod = nullptr; + static jmethodID quaternionYMethod = nullptr; + static jmethodID quaternionZMethod = nullptr; + + if (!poseTranslationField) { + poseTranslationField = + env->GetFieldID(pose3dCls, "m_translation", + "Ledu/wpi/first/math/geometry/Translation3d;"); + } + if (!poseRotationField) { + poseRotationField = env->GetFieldID( + pose3dCls, "m_rotation", "Ledu/wpi/first/math/geometry/Rotation3d;"); + } + if (!translationXField) { + translationXField = env->GetFieldID(translation3dCls, "m_x", "D"); + } + if (!translationYField) { + translationYField = env->GetFieldID(translation3dCls, "m_y", "D"); + } + if (!translationZField) { + translationZField = env->GetFieldID(translation3dCls, "m_z", "D"); + } + if (!rotationQuaternionField) { + rotationQuaternionField = env->GetFieldID( + rotation3dCls, "m_q", "Ledu/wpi/first/math/geometry/Quaternion;"); + } + if (!quaternionWField) { + quaternionWField = env->GetFieldID(quaternionCls, "m_r", "D"); + } + if (!quaternionXMethod) { + quaternionXMethod = env->GetMethodID(quaternionCls, "getX", "()D"); + } + if (!quaternionYMethod) { + quaternionYMethod = env->GetMethodID(quaternionCls, "getY", "()D"); + } + if (!quaternionZMethod) { + quaternionZMethod = env->GetMethodID(quaternionCls, "getZ", "()D"); + } + + jobject translation = env->GetObjectField(pose, poseTranslationField); + jdouble dx = env->GetDoubleField(translation, translationXField); + jdouble dy = env->GetDoubleField(translation, translationYField); + jdouble dz = env->GetDoubleField(translation, translationZField); + jobject rotation = env->GetObjectField(pose, poseRotationField); + jobject quaternion = env->GetObjectField(rotation, rotationQuaternionField); + jdouble qw = env->GetDoubleField(quaternion, quaternionWField); + jdouble qx = env->CallDoubleMethod(quaternion, quaternionXMethod); + jdouble qy = env->CallDoubleMethod(quaternion, quaternionYMethod); + jdouble qz = env->CallDoubleMethod(quaternion, quaternionZMethod); + + return {units::meter_t{dx}, units::meter_t{dy}, units::meter_t{dz}, + frc::Rotation3d{frc::Quaternion{qw, qx, qy, qz}}}; +} + +frc::Twist3d FromJavaTwist3d(JNIEnv* env, jobject twist) { + static jfieldID twistDxField = nullptr; + static jfieldID twistDyField = nullptr; + static jfieldID twistDzField = nullptr; + static jfieldID twistRxField = nullptr; + static jfieldID twistRyField = nullptr; + static jfieldID twistRzField = nullptr; + + if (!twistDxField) { + twistDxField = env->GetFieldID(twist3dCls, "dx", "D"); + } + if (!twistDyField) { + twistDyField = env->GetFieldID(twist3dCls, "dy", "D"); + } + if (!twistDzField) { + twistDzField = env->GetFieldID(twist3dCls, "dz", "D"); + } + if (!twistRxField) { + twistRxField = env->GetFieldID(twist3dCls, "rx", "D"); + } + if (!twistRyField) { + twistRyField = env->GetFieldID(twist3dCls, "ry", "D"); + } + if (!twistRzField) { + twistRzField = env->GetFieldID(twist3dCls, "rz", "D"); + } + + jdouble twistDx = env->GetDoubleField(twist, twistDxField); + jdouble twistDy = env->GetDoubleField(twist, twistDyField); + jdouble twistDz = env->GetDoubleField(twist, twistDzField); + jdouble twistRx = env->GetDoubleField(twist, twistRxField); + jdouble twistRy = env->GetDoubleField(twist, twistRyField); + jdouble twistRz = env->GetDoubleField(twist, twistRzField); + + return {units::meter_t{twistDx}, units::meter_t{twistDy}, + units::meter_t{twistDz}, units::radian_t{twistRx}, + units::radian_t{twistRy}, units::radian_t{twistRz}}; +} + +jobject MakeJObject(JNIEnv* env, const frc::Pose3d& pose) { + static jmethodID pose3dCtor = nullptr; + static jmethodID quaternionCtor = nullptr; + static jmethodID rotation3dCtor = nullptr; + static jmethodID translation3dCtor = nullptr; + + if (!pose3dCtor) { + pose3dCtor = + env->GetMethodID(pose3dCls, "", + "(Ledu/wpi/first/math/geometry/Translation3d;Ledu/wpi/" + "first/math/geometry/Rotation3d;)V"); + } + if (!quaternionCtor) { + quaternionCtor = env->GetMethodID(quaternionCls, "", "(DDDD)V"); + } + if (!rotation3dCtor) { + rotation3dCtor = env->GetMethodID( + rotation3dCls, "", "(Ledu/wpi/first/math/geometry/Quaternion;)V"); + } + if (!translation3dCtor) { + translation3dCtor = env->GetMethodID(translation3dCls, "", "(DDD)V"); + } + + frc::Quaternion quaternion = pose.Rotation().GetQuaternion(); + + double dx = pose.X().value(); + double dy = pose.Y().value(); + double dz = pose.Z().value(); + double qw = quaternion.W(); + double qx = quaternion.X(); + double qy = quaternion.Y(); + double qz = quaternion.Z(); + + jobject jTranslation = + env->NewObject(translation3dCls, translation3dCtor, dx, dy, dz); + jobject jQuaternion = + env->NewObject(quaternionCls, quaternionCtor, qw, qx, qy, qz); + jobject jRotation = + env->NewObject(rotation3dCls, rotation3dCtor, jQuaternion); + jobject jPose = + env->NewObject(pose3dCls, pose3dCtor, jTranslation, jRotation); + + return jPose; +} + +jobject MakeJObject(JNIEnv* env, const frc::Twist3d& twist) { + static jmethodID twist3dCtor = nullptr; + if (!twist3dCtor) { + twist3dCtor = env->GetMethodID(twist3dCls, "", "(DDDDDD)V"); + } + + return env->NewObject(twist3dCls, twist3dCtor, twist.dx.value(), + twist.dy.value(), twist.dz.value(), twist.rx.value(), + twist.ry.value(), twist.rz.value()); +} + std::vector GetElementsFromTrajectory( const frc::Trajectory& trajectory) { std::vector elements; @@ -187,6 +389,40 @@ Java_edu_wpi_first_math_WPIMathJNI_pow env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data()); } +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: expPose3d + * Signature: (Ljava/lang/Object;Ljava/lang/Object;)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_math_WPIMathJNI_expPose3d + (JNIEnv* env, jclass, jobject startPose, jobject twist) +{ + frc::Pose3d startPoseCpp = FromJavaPose3d(env, startPose); + frc::Twist3d twistCpp = FromJavaTwist3d(env, twist); + + frc::Pose3d result = startPoseCpp.Exp(twistCpp); + + return MakeJObject(env, result); +} + +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: logPose3d + * Signature: (Ljava/lang/Object;Ljava/lang/Object;)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_math_WPIMathJNI_logPose3d + (JNIEnv* env, jclass, jobject start, jobject end) +{ + frc::Pose3d startCpp = FromJavaPose3d(env, start); + frc::Pose3d endCpp = FromJavaPose3d(env, end); + + frc::Twist3d result = startCpp.Log(endCpp); + + return MakeJObject(env, result); +} + /* * Class: edu_wpi_first_math_WPIMathJNI * Method: isStabilizable diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 6a1bf2edaff..647abe2eb79 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -5,7 +5,11 @@ package edu.wpi.first.math; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Twist3d; import org.junit.jupiter.api.Test; public class WPIMathJNITest { @@ -13,4 +17,22 @@ public class WPIMathJNITest { public void testLink() { assertDoesNotThrow(WPIMathJNI::forceLoad); } + + @Test + public void testExpPose3d() { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var twist = new Twist3d(0, 1, 0, Math.PI, 0, 0); + final var expected = start.exp(twist); + final var result = WPIMathJNI.expPose3d(start, twist); + assertEquals(expected, result); + } + + @Test + public void testLogPose3d() { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); + final var expected = start.log(end); + final var result = WPIMathJNI.logPose3d(start, end); + assertEquals(expected, result); + } } From 3c4fd9cf4c67e81c8a10623b4e9d9c4285f02db7 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Jul 2023 11:55:09 -0700 Subject: [PATCH 02/21] Switch to double[] --- .../java/edu/wpi/first/math/WPIMathJNI.java | 24 +- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 258 ++++-------------- .../edu/wpi/first/math/WPIMathJNITest.java | 33 ++- 3 files changed, 98 insertions(+), 217 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index e6bc7c820f6..869642831f4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -4,8 +4,6 @@ package edu.wpi.first.math; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; @@ -81,20 +79,26 @@ public static native void dare( /** * Obtain a Pose3d from a (constant curvature) velocity. * - * @param start The starting Pose3d. - * @param twist The twist to apply. - * @return The new pose. + *

A Pose3d is represented by [t_x, t_y, t_z, q_w, q_x, q_y, q_z] and a Twist3d is represented + * by [dx, dy, dz, rx, ry, rz]. + * + * @param start The starting Pose3d as a double array. + * @param twist The twist to apply as a double array. + * @return The new pose as a double array. */ - public static native Pose3d expPose3d(Pose3d start, Twist3d twist); + public static native double[] expPose3d(double[] start, double[] twist); /** * Returns a Twist3d that maps this pose to the end pose. * - * @param start The starting Pose3d. - * @param end The ending Pose3d. - * @return The twist that maps start to end. + *

A Pose3d is represented by [t_x, t_y, t_z, q_w, q_x, q_y, q_z] and a Twist3d is represented + * by [dx, dy, dz, rx, ry, rz]. + * + * @param start The starting Pose3d as a double array. + * @param end The ending Pose3d as a double array. + * @return The twist that maps start to end as a double array. */ - public static native Twist3d logPose3d(Pose3d start, Pose3d end); + public static native double[] logPose3d(double[] start, double[] end); /** * Returns true if (A, B) is a stabilizable pair. diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 6a7b26dc99f..f483a1b5f4c 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -58,205 +58,53 @@ bool IsStabilizable(const Eigen::Ref& A, } // namespace -static JClass pose3dCls; -static JClass quaternionCls; -static JClass rotation3dCls; -static JClass translation3dCls; -static JClass twist3dCls; - -static const JClassInit classes[] = { - {"edu/wpi/first/math/geometry/Pose3d", &pose3dCls}, - {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls}, - {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls}, - {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}, - {"edu/wpi/first/math/geometry/Twist3d", &twist3dCls}}; - -extern "C" { - -JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { - JNIEnv* env; - if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { - return JNI_ERR; - } - - for (auto& c : classes) { - *c.cls = JClass(env, c.name); - if (!*c.cls) { - return JNI_ERR; - } - } - - return JNI_VERSION_1_6; -} - -JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { - JNIEnv* env; - if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { - return; - } - - for (auto& c : classes) { - c.cls->free(env); - } -} - -} // extern "C" - -frc::Pose3d FromJavaPose3d(JNIEnv* env, jobject pose) { - static jfieldID poseTranslationField = nullptr; - static jfieldID poseRotationField = nullptr; - static jfieldID translationXField = nullptr; - static jfieldID translationYField = nullptr; - static jfieldID translationZField = nullptr; - static jfieldID rotationQuaternionField = nullptr; - static jfieldID quaternionWField = nullptr; - static jmethodID quaternionXMethod = nullptr; - static jmethodID quaternionYMethod = nullptr; - static jmethodID quaternionZMethod = nullptr; - - if (!poseTranslationField) { - poseTranslationField = - env->GetFieldID(pose3dCls, "m_translation", - "Ledu/wpi/first/math/geometry/Translation3d;"); - } - if (!poseRotationField) { - poseRotationField = env->GetFieldID( - pose3dCls, "m_rotation", "Ledu/wpi/first/math/geometry/Rotation3d;"); - } - if (!translationXField) { - translationXField = env->GetFieldID(translation3dCls, "m_x", "D"); - } - if (!translationYField) { - translationYField = env->GetFieldID(translation3dCls, "m_y", "D"); - } - if (!translationZField) { - translationZField = env->GetFieldID(translation3dCls, "m_z", "D"); - } - if (!rotationQuaternionField) { - rotationQuaternionField = env->GetFieldID( - rotation3dCls, "m_q", "Ledu/wpi/first/math/geometry/Quaternion;"); - } - if (!quaternionWField) { - quaternionWField = env->GetFieldID(quaternionCls, "m_r", "D"); - } - if (!quaternionXMethod) { - quaternionXMethod = env->GetMethodID(quaternionCls, "getX", "()D"); - } - if (!quaternionYMethod) { - quaternionYMethod = env->GetMethodID(quaternionCls, "getY", "()D"); - } - if (!quaternionZMethod) { - quaternionZMethod = env->GetMethodID(quaternionCls, "getZ", "()D"); - } - - jobject translation = env->GetObjectField(pose, poseTranslationField); - jdouble dx = env->GetDoubleField(translation, translationXField); - jdouble dy = env->GetDoubleField(translation, translationYField); - jdouble dz = env->GetDoubleField(translation, translationZField); - jobject rotation = env->GetObjectField(pose, poseRotationField); - jobject quaternion = env->GetObjectField(rotation, rotationQuaternionField); - jdouble qw = env->GetDoubleField(quaternion, quaternionWField); - jdouble qx = env->CallDoubleMethod(quaternion, quaternionXMethod); - jdouble qy = env->CallDoubleMethod(quaternion, quaternionYMethod); - jdouble qz = env->CallDoubleMethod(quaternion, quaternionZMethod); - - return {units::meter_t{dx}, units::meter_t{dy}, units::meter_t{dz}, - frc::Rotation3d{frc::Quaternion{qw, qx, qy, qz}}}; +frc::Pose3d Pose3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { + jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); + frc::Translation3d translation{units::meter_t{nativeArray[0]}, + units::meter_t{nativeArray[1]}, + units::meter_t{nativeArray[2]}}; + frc::Quaternion quaternion{nativeArray[3], nativeArray[4], nativeArray[5], + nativeArray[6]}; + env->ReleaseDoubleArrayElements(arr, nativeArray, 0); + return {translation, frc::Rotation3d{quaternion}}; } -frc::Twist3d FromJavaTwist3d(JNIEnv* env, jobject twist) { - static jfieldID twistDxField = nullptr; - static jfieldID twistDyField = nullptr; - static jfieldID twistDzField = nullptr; - static jfieldID twistRxField = nullptr; - static jfieldID twistRyField = nullptr; - static jfieldID twistRzField = nullptr; - - if (!twistDxField) { - twistDxField = env->GetFieldID(twist3dCls, "dx", "D"); - } - if (!twistDyField) { - twistDyField = env->GetFieldID(twist3dCls, "dy", "D"); - } - if (!twistDzField) { - twistDzField = env->GetFieldID(twist3dCls, "dz", "D"); - } - if (!twistRxField) { - twistRxField = env->GetFieldID(twist3dCls, "rx", "D"); - } - if (!twistRyField) { - twistRyField = env->GetFieldID(twist3dCls, "ry", "D"); - } - if (!twistRzField) { - twistRzField = env->GetFieldID(twist3dCls, "rz", "D"); - } - - jdouble twistDx = env->GetDoubleField(twist, twistDxField); - jdouble twistDy = env->GetDoubleField(twist, twistDyField); - jdouble twistDz = env->GetDoubleField(twist, twistDzField); - jdouble twistRx = env->GetDoubleField(twist, twistRxField); - jdouble twistRy = env->GetDoubleField(twist, twistRyField); - jdouble twistRz = env->GetDoubleField(twist, twistRzField); - - return {units::meter_t{twistDx}, units::meter_t{twistDy}, - units::meter_t{twistDz}, units::radian_t{twistRx}, - units::radian_t{twistRy}, units::radian_t{twistRz}}; +frc::Twist3d Twist3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { + jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); + frc::Twist3d twist = { + units::meter_t{nativeArray[0]}, units::meter_t{nativeArray[1]}, + units::meter_t{nativeArray[2]}, units::radian_t{nativeArray[3]}, + units::radian_t{nativeArray[4]}, units::radian_t{nativeArray[5]}}; + env->ReleaseDoubleArrayElements(arr, nativeArray, 0); + return twist; } -jobject MakeJObject(JNIEnv* env, const frc::Pose3d& pose) { - static jmethodID pose3dCtor = nullptr; - static jmethodID quaternionCtor = nullptr; - static jmethodID rotation3dCtor = nullptr; - static jmethodID translation3dCtor = nullptr; - - if (!pose3dCtor) { - pose3dCtor = - env->GetMethodID(pose3dCls, "", - "(Ledu/wpi/first/math/geometry/Translation3d;Ledu/wpi/" - "first/math/geometry/Rotation3d;)V"); - } - if (!quaternionCtor) { - quaternionCtor = env->GetMethodID(quaternionCls, "", "(DDDD)V"); - } - if (!rotation3dCtor) { - rotation3dCtor = env->GetMethodID( - rotation3dCls, "", "(Ledu/wpi/first/math/geometry/Quaternion;)V"); - } - if (!translation3dCtor) { - translation3dCtor = env->GetMethodID(translation3dCls, "", "(DDD)V"); - } - - frc::Quaternion quaternion = pose.Rotation().GetQuaternion(); - - double dx = pose.X().value(); - double dy = pose.Y().value(); - double dz = pose.Z().value(); - double qw = quaternion.W(); - double qx = quaternion.X(); - double qy = quaternion.Y(); - double qz = quaternion.Z(); - - jobject jTranslation = - env->NewObject(translation3dCls, translation3dCtor, dx, dy, dz); - jobject jQuaternion = - env->NewObject(quaternionCls, quaternionCtor, qw, qx, qy, qz); - jobject jRotation = - env->NewObject(rotation3dCls, rotation3dCtor, jQuaternion); - jobject jPose = - env->NewObject(pose3dCls, pose3dCtor, jTranslation, jRotation); - - return jPose; +jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Pose3d& pose) { + const frc::Quaternion& quaternion = pose.Rotation().GetQuaternion(); + jdoubleArray results = env->NewDoubleArray(7); + jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); + buffer[0] = pose.X().value(); + buffer[1] = pose.Y().value(); + buffer[2] = pose.Z().value(); + buffer[3] = quaternion.W(); + buffer[4] = quaternion.X(); + buffer[5] = quaternion.Y(); + buffer[6] = quaternion.Z(); + env->ReleaseDoubleArrayElements(results, buffer, 0); + return results; } -jobject MakeJObject(JNIEnv* env, const frc::Twist3d& twist) { - static jmethodID twist3dCtor = nullptr; - if (!twist3dCtor) { - twist3dCtor = env->GetMethodID(twist3dCls, "", "(DDDDDD)V"); - } - - return env->NewObject(twist3dCls, twist3dCtor, twist.dx.value(), - twist.dy.value(), twist.dz.value(), twist.rx.value(), - twist.ry.value(), twist.rz.value()); +jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Twist3d& twist) { + jdoubleArray results = env->NewDoubleArray(6); + jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); + buffer[0] = twist.dx.value(); + buffer[1] = twist.dy.value(); + buffer[2] = twist.dz.value(); + buffer[3] = twist.rx.value(); + buffer[4] = twist.ry.value(); + buffer[5] = twist.rz.value(); + env->ReleaseDoubleArrayElements(results, buffer, 0); + return results; } std::vector GetElementsFromTrajectory( @@ -392,35 +240,35 @@ Java_edu_wpi_first_math_WPIMathJNI_pow /* * Class: edu_wpi_first_math_WPIMathJNI * Method: expPose3d - * Signature: (Ljava/lang/Object;Ljava/lang/Object;)Ljava/lang/Object; + * Signature: ([D[D)[D */ -JNIEXPORT jobject JNICALL +JNIEXPORT jdoubleArray JNICALL Java_edu_wpi_first_math_WPIMathJNI_expPose3d - (JNIEnv* env, jclass, jobject startPose, jobject twist) + (JNIEnv* env, jclass, jdoubleArray startPose, jdoubleArray twist) { - frc::Pose3d startPoseCpp = FromJavaPose3d(env, startPose); - frc::Twist3d twistCpp = FromJavaTwist3d(env, twist); + frc::Pose3d startPoseCpp = Pose3dFromJDoubleArray(env, startPose); + frc::Twist3d twistCpp = Twist3dFromJDoubleArray(env, twist); frc::Pose3d result = startPoseCpp.Exp(twistCpp); - return MakeJObject(env, result); + return MakeJDoubleArray(env, result); } /* * Class: edu_wpi_first_math_WPIMathJNI * Method: logPose3d - * Signature: (Ljava/lang/Object;Ljava/lang/Object;)Ljava/lang/Object; + * Signature: ([D[D)[D */ -JNIEXPORT jobject JNICALL +JNIEXPORT jdoubleArray JNICALL Java_edu_wpi_first_math_WPIMathJNI_logPose3d - (JNIEnv* env, jclass, jobject start, jobject end) + (JNIEnv* env, jclass, jdoubleArray start, jdoubleArray end) { - frc::Pose3d startCpp = FromJavaPose3d(env, start); - frc::Pose3d endCpp = FromJavaPose3d(env, end); + frc::Pose3d startCpp = Pose3dFromJDoubleArray(env, start); + frc::Pose3d endCpp = Pose3dFromJDoubleArray(env, end); frc::Twist3d result = startCpp.Log(endCpp); - return MakeJObject(env, result); + return MakeJDoubleArray(env, result); } /* diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 647abe2eb79..b8a3d3c4a12 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -8,11 +8,38 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Twist3d; import org.junit.jupiter.api.Test; public class WPIMathJNITest { + private static double[] toDoubleArray(Pose3d pose) { + Quaternion quaternion = pose.getRotation().getQuaternion(); + return new double[] { + pose.getX(), + pose.getY(), + pose.getZ(), + quaternion.getW(), + quaternion.getX(), + quaternion.getY(), + quaternion.getZ() + }; + } + + private static double[] toDoubleArray(Twist3d twist) { + return new double[] {twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz}; + } + + private static Pose3d pose3dFromDoubleArray(double[] arr) { + return new Pose3d( + arr[0], arr[1], arr[2], new Rotation3d(new Quaternion(arr[3], arr[4], arr[5], arr[6]))); + } + + private static Twist3d twist3dFromDoubleArray(double[] arr) { + return new Twist3d(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5]); + } + @Test public void testLink() { assertDoesNotThrow(WPIMathJNI::forceLoad); @@ -23,7 +50,8 @@ public void testExpPose3d() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var twist = new Twist3d(0, 1, 0, Math.PI, 0, 0); final var expected = start.exp(twist); - final var result = WPIMathJNI.expPose3d(start, twist); + final var result = + pose3dFromDoubleArray(WPIMathJNI.expPose3d(toDoubleArray(start), toDoubleArray(twist))); assertEquals(expected, result); } @@ -32,7 +60,8 @@ public void testLogPose3d() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); final var expected = start.log(end); - final var result = WPIMathJNI.logPose3d(start, end); + final var result = + twist3dFromDoubleArray(WPIMathJNI.logPose3d(toDoubleArray(start), toDoubleArray(end))); assertEquals(expected, result); } } From b725e5a9512be259413962b4ce411465cd6d9f17 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Jul 2023 13:40:12 -0700 Subject: [PATCH 03/21] Specify type in name of toDoubleArray --- .../src/test/java/edu/wpi/first/math/WPIMathJNITest.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index b8a3d3c4a12..00c92052b53 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -14,7 +14,7 @@ import org.junit.jupiter.api.Test; public class WPIMathJNITest { - private static double[] toDoubleArray(Pose3d pose) { + private static double[] pose3dToDoubleArray(Pose3d pose) { Quaternion quaternion = pose.getRotation().getQuaternion(); return new double[] { pose.getX(), @@ -27,7 +27,7 @@ private static double[] toDoubleArray(Pose3d pose) { }; } - private static double[] toDoubleArray(Twist3d twist) { + private static double[] twist3dToDoubleArray(Twist3d twist) { return new double[] {twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz}; } @@ -51,7 +51,7 @@ public void testExpPose3d() { final var twist = new Twist3d(0, 1, 0, Math.PI, 0, 0); final var expected = start.exp(twist); final var result = - pose3dFromDoubleArray(WPIMathJNI.expPose3d(toDoubleArray(start), toDoubleArray(twist))); + pose3dFromDoubleArray(WPIMathJNI.expPose3d(pose3dToDoubleArray(start), twist3dToDoubleArray(twist))); assertEquals(expected, result); } @@ -61,7 +61,7 @@ public void testLogPose3d() { final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); final var expected = start.log(end); final var result = - twist3dFromDoubleArray(WPIMathJNI.logPose3d(toDoubleArray(start), toDoubleArray(end))); + twist3dFromDoubleArray(WPIMathJNI.logPose3d(pose3dToDoubleArray(start), pose3dToDoubleArray(end))); assertEquals(expected, result); } } From 0d282146f2216ba2a79ef88d52c404a81d10c56d Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Jul 2023 14:03:00 -0700 Subject: [PATCH 04/21] Make Pose3d use JNI --- .../edu/wpi/first/math/geometry/Pose3d.java | 169 +++++------------- .../edu/wpi/first/math/WPIMathJNITest.java | 51 ------ 2 files changed, 49 insertions(+), 171 deletions(-) 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 9e46ddb064e..605d83877ad 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 @@ -8,14 +8,8 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; +import edu.wpi.first.math.WPIMathJNI; 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; /** Represents a 3D pose containing translational and rotational elements. */ @@ -201,53 +195,8 @@ public Pose3d relativeTo(Pose3d other) { * @return The new pose of the robot. */ public Pose3d exp(Twist3d twist) { - // 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.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; - } - - 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_component.get(0, 0), - translation_component.get(1, 0), - translation_component.get(2, 0)), - new Rotation3d(R)); - - return this.plus(transform); + return pose3dFromDoubleArray( + WPIMathJNI.expPose3d(pose3dToDoubleArray(this), twist3dToDoubleArray(twist))); } /** @@ -258,51 +207,8 @@ public Pose3d exp(Twist3d twist) { * @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 rvec = transform.getRotation().getQuaternion().toRotationVector(); - - final var omega = rotationVectorToMatrix(rvec); - final var theta = rvec.norm(); - final var 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/(2*B)) / θ² - double A = Math.sin(theta) / theta; - double B = (1 - Math.cos(theta)) / thetaSq; - C = (1 - A / (2 * B)) / thetaSq; - } - - final var V_inv = - Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omega.times(omega).times(C)); - - final var twist_translation = - V_inv.times(VecBuilder.fill(transform.getX(), transform.getY(), transform.getZ())); - - return new Twist3d( - 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)); + return twist3dFromDoubleArray( + WPIMathJNI.logPose3d(pose3dToDoubleArray(this), pose3dToDoubleArray(end))); } /** @@ -355,29 +261,52 @@ public Pose3d interpolate(Pose3d endValue, double t) { } /** - * Applies the hat operator to a rotation vector. + * Converts a Pose3d to the double array format for the JNI. + * + * @param pose The pose to convert. + * @return The double array representing the pose. + */ + private static double[] pose3dToDoubleArray(Pose3d pose) { + Quaternion quaternion = pose.getRotation().getQuaternion(); + return new double[] { + pose.getX(), + pose.getY(), + pose.getZ(), + quaternion.getW(), + quaternion.getX(), + quaternion.getY(), + quaternion.getZ() + }; + } + + /** + * Converts a Twist3d to the double array format for the JNI. + * + * @param twist The twist to convert. + * @return The double array representing the twist. + */ + private static double[] twist3dToDoubleArray(Twist3d twist) { + return new double[] {twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz}; + } + + /** + * Constructs a Pose3d from the JNI double array format. * - *

It takes a rotation vector and returns the corresponding matrix representation of the Lie - * algebra element (a 3x3 rotation matrix). + * @param arr The JNI double array to convert. + * @return The corresponding pose. + */ + private static Pose3d pose3dFromDoubleArray(double[] arr) { + return new Pose3d( + arr[0], arr[1], arr[2], new Rotation3d(new Quaternion(arr[3], arr[4], arr[5], arr[6]))); + } + + /** + * Constructs a Twist3d from the JNI double array format. * - * @param rotation The rotation vector. - * @return The rotation vector as a 3x3 rotation matrix. + * @param arr The JNI double array to convert. + * @return The corresponding twist. */ - private Matrix rotationVectorToMatrix(Vector rotation) { - // Given a rotation vector , - // [ 0 -c b] - // Omega = [ c 0 -a] - // [-b a 0] - return new MatBuilder<>(Nat.N3(), Nat.N3()) - .fill( - 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); + private static Twist3d twist3dFromDoubleArray(double[] arr) { + return new Twist3d(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5]); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 00c92052b53..6a1bf2edaff 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -5,63 +5,12 @@ package edu.wpi.first.math; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; -import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Twist3d; import org.junit.jupiter.api.Test; public class WPIMathJNITest { - private static double[] pose3dToDoubleArray(Pose3d pose) { - Quaternion quaternion = pose.getRotation().getQuaternion(); - return new double[] { - pose.getX(), - pose.getY(), - pose.getZ(), - quaternion.getW(), - quaternion.getX(), - quaternion.getY(), - quaternion.getZ() - }; - } - - private static double[] twist3dToDoubleArray(Twist3d twist) { - return new double[] {twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz}; - } - - private static Pose3d pose3dFromDoubleArray(double[] arr) { - return new Pose3d( - arr[0], arr[1], arr[2], new Rotation3d(new Quaternion(arr[3], arr[4], arr[5], arr[6]))); - } - - private static Twist3d twist3dFromDoubleArray(double[] arr) { - return new Twist3d(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5]); - } - @Test public void testLink() { assertDoesNotThrow(WPIMathJNI::forceLoad); } - - @Test - public void testExpPose3d() { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var twist = new Twist3d(0, 1, 0, Math.PI, 0, 0); - final var expected = start.exp(twist); - final var result = - pose3dFromDoubleArray(WPIMathJNI.expPose3d(pose3dToDoubleArray(start), twist3dToDoubleArray(twist))); - assertEquals(expected, result); - } - - @Test - public void testLogPose3d() { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); - final var expected = start.log(end); - final var result = - twist3dFromDoubleArray(WPIMathJNI.logPose3d(pose3dToDoubleArray(start), pose3dToDoubleArray(end))); - assertEquals(expected, result); - } } From ad63760d4bf7d5728272a29e9424aea6801d0f12 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Jul 2023 14:28:08 -0700 Subject: [PATCH 05/21] Add jank timing system --- .../edu/wpi/first/math/WPIMathJNITest.java | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 6a1bf2edaff..1d19190ec3d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -6,6 +6,9 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Twist3d; import org.junit.jupiter.api.Test; public class WPIMathJNITest { @@ -13,4 +16,48 @@ public class WPIMathJNITest { public void testLink() { assertDoesNotThrow(WPIMathJNI::forceLoad); } + + public static double timePose3dExpMillis(Pose3d start, Twist3d twist, int count) { + final long startTime = System.nanoTime(); + for (int i = 0; i < count; i++) { + start.exp(twist); + } + final long endTime = System.nanoTime(); + double totalTimeMillis = (endTime - startTime) / 1000.0; + return totalTimeMillis / count; + } + + public static double timePose3dLogMillis(Pose3d start, Pose3d end, int count) { + final long startTime = System.nanoTime(); + for (int i = 0; i < count; i++) { + start.log(end); + } + final long endTime = System.nanoTime(); + double totalTimeMillis = (endTime - startTime) / 1000.0; + return totalTimeMillis / count; + } + + @Test + public void timePose3dExp() { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); + var msg = new StringBuilder(); + msg.append("Pose3d.exp():\n"); + msg.append(" start = " + start + "\n"); + msg.append(" twist = " + twist + "\n"); + msg.append(" average milliseconds: " + timePose3dExpMillis(start, twist, 1_000) + "\n"); + throw new RuntimeException("report:\n" + msg); + } + + @Test + public void timePose3dLog() { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); + var msg = new StringBuilder(); + msg.append("Pose3d.log():\n"); + msg.append(" start = " + start + "\n"); + msg.append(" end = " + end + "\n"); + msg.append(" average milliseconds: " + timePose3dLogMillis(start, end, 1_000) + "\n"); + throw new RuntimeException("report:\n" + msg); + } } From 546800c37322aff883c754a95b94355bcbed8525 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Jul 2023 20:34:21 -0700 Subject: [PATCH 06/21] Improve testing Use correct time unit Print number of iterations Show multiple runs --- .../edu/wpi/first/math/WPIMathJNITest.java | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 1d19190ec3d..1e66a5d9e23 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -17,35 +17,39 @@ public void testLink() { assertDoesNotThrow(WPIMathJNI::forceLoad); } - public static double timePose3dExpMillis(Pose3d start, Twist3d twist, int count) { + public static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) { final long startTime = System.nanoTime(); for (int i = 0; i < count; i++) { start.exp(twist); } final long endTime = System.nanoTime(); - double totalTimeMillis = (endTime - startTime) / 1000.0; - return totalTimeMillis / count; + double totalTimeMicros = (endTime - startTime) / 1000.0; + return totalTimeMicros / count; } - public static double timePose3dLogMillis(Pose3d start, Pose3d end, int count) { + public static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { final long startTime = System.nanoTime(); for (int i = 0; i < count; i++) { start.log(end); } final long endTime = System.nanoTime(); - double totalTimeMillis = (endTime - startTime) / 1000.0; - return totalTimeMillis / count; + double totalTimeMicros = (endTime - startTime) / 1000.0; + return totalTimeMicros / count; } @Test public void timePose3dExp() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); + final int count = 1_000; var msg = new StringBuilder(); msg.append("Pose3d.exp():\n"); msg.append(" start = " + start + "\n"); msg.append(" twist = " + twist + "\n"); - msg.append(" average milliseconds: " + timePose3dExpMillis(start, twist, 1_000) + "\n"); + msg.append(" iterations per run: " + count + "\n"); + for (int i = 1; i <= 5; i++) { + msg.append(" run " + i + ": average " + timePose3dExpMicros(start, twist, count) + "μs\n"); + } throw new RuntimeException("report:\n" + msg); } @@ -53,11 +57,15 @@ public void timePose3dExp() { public void timePose3dLog() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); + final int count = 1_000; var msg = new StringBuilder(); msg.append("Pose3d.log():\n"); msg.append(" start = " + start + "\n"); msg.append(" end = " + end + "\n"); - msg.append(" average milliseconds: " + timePose3dLogMillis(start, end, 1_000) + "\n"); + msg.append(" iterations per run: " + count + "\n"); + for (int i = 1; i <= 5; i++) { + msg.append(" run " + i + ": average " + timePose3dLogMicros(start, end, count) + "μs\n"); + } throw new RuntimeException("report:\n" + msg); } } From 3e14242be18820217285dc4c15b0ce969c7495a8 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Jul 2023 20:47:02 -0700 Subject: [PATCH 07/21] Switch to many double args and returning Java object --- .../java/edu/wpi/first/math/WPIMathJNI.java | 78 +++++-- .../edu/wpi/first/math/geometry/Pose3d.java | 36 ++- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 211 +++++++++++++----- 3 files changed, 252 insertions(+), 73 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 869642831f4..3f3baad4a6d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -4,6 +4,8 @@ package edu.wpi.first.math; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; @@ -79,26 +81,70 @@ public static native void dare( /** * Obtain a Pose3d from a (constant curvature) velocity. * - *

A Pose3d is represented by [t_x, t_y, t_z, q_w, q_x, q_y, q_z] and a Twist3d is represented - * by [dx, dy, dz, rx, ry, rz]. - * - * @param start The starting Pose3d as a double array. - * @param twist The twist to apply as a double array. - * @return The new pose as a double array. + * @param poseX The pose's translational X component. + * @param poseY The pose's translational Y component. + * @param poseZ The pose's translational Z component. + * @param poseQw The pose quaternion's W component. + * @param poseQx The pose quaternion's X component. + * @param poseQy The pose quaternion's Y component. + * @param poseQz The pose quaternion's Z component. + * @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. */ - public static native double[] expPose3d(double[] start, double[] twist); + public static native Pose3d expPose3d( + double poseX, + double poseY, + double poseZ, + double poseQw, + double poseQx, + double poseQy, + double poseQz, + double twistDx, + double twistDy, + double twistDz, + double twistRx, + double twistRy, + double twistRz); /** - * Returns a Twist3d that maps this pose to the end pose. - * - *

A Pose3d is represented by [t_x, t_y, t_z, q_w, q_x, q_y, q_z] and a Twist3d is represented - * by [dx, dy, dz, rx, ry, rz]. - * - * @param start The starting Pose3d as a double array. - * @param end The ending Pose3d as a double array. - * @return The twist that maps start to end as a double array. + * Returns a Twist3d that maps the starting pose to the end pose. + * + * @param startX The starting pose's translational X component. + * @param startY The starting pose's translational Y component. + * @param startZ The starting pose's translational Z component. + * @param startQw The starting pose quaternion's W component. + * @param startQx The starting pose quaternion's X component. + * @param startQy The starting pose quaternion's Y component. + * @param startQz The starting pose quaternion's Z component. + * @param endX The ending pose's translational X component. + * @param endY The ending pose's translational Y component. + * @param endZ The ending pose's translational Z component. + * @param endQw The ending pose quaternion's W component. + * @param endQx The ending pose quaternion's X component. + * @param endQy The ending pose quaternion's Y component. + * @param endQz The ending pose quaternion's Z component. + * @return The twist that maps start to end. */ - public static native double[] logPose3d(double[] start, double[] end); + public static native Twist3d logPose3d( + double startX, + double startY, + double startZ, + double startQw, + double startQx, + double startQy, + double startQz, + double endX, + double endY, + double endZ, + double endQw, + double endQx, + double endQy, + double endQz); /** * Returns true if (A, B) is a stabilizable pair. 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 605d83877ad..6f626b83fa6 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 @@ -195,8 +195,21 @@ public Pose3d relativeTo(Pose3d other) { * @return The new pose of the robot. */ public Pose3d exp(Twist3d twist) { - return pose3dFromDoubleArray( - WPIMathJNI.expPose3d(pose3dToDoubleArray(this), twist3dToDoubleArray(twist))); + Quaternion quaternion = this.getRotation().getQuaternion(); + return WPIMathJNI.expPose3d( + this.getX(), + this.getY(), + this.getZ(), + quaternion.getW(), + quaternion.getX(), + quaternion.getY(), + quaternion.getZ(), + twist.dx, + twist.dy, + twist.dz, + twist.rx, + twist.ry, + twist.rz); } /** @@ -207,8 +220,23 @@ public Pose3d exp(Twist3d twist) { * @return The twist that maps this to end. */ public Twist3d log(Pose3d end) { - return twist3dFromDoubleArray( - WPIMathJNI.logPose3d(pose3dToDoubleArray(this), pose3dToDoubleArray(end))); + Quaternion thisQuaternion = this.getRotation().getQuaternion(); + Quaternion endQuaternion = end.getRotation().getQuaternion(); + return WPIMathJNI.logPose3d( + this.getX(), + this.getY(), + this.getZ(), + thisQuaternion.getW(), + thisQuaternion.getX(), + thisQuaternion.getY(), + thisQuaternion.getZ(), + end.getX(), + end.getY(), + end.getZ(), + endQuaternion.getW(), + endQuaternion.getX(), + endQuaternion.getY(), + endQuaternion.getZ()); } /** diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index f483a1b5f4c..e090ee3c65d 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -58,55 +58,146 @@ bool IsStabilizable(const Eigen::Ref& A, } // namespace -frc::Pose3d Pose3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { - jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); - frc::Translation3d translation{units::meter_t{nativeArray[0]}, - units::meter_t{nativeArray[1]}, - units::meter_t{nativeArray[2]}}; - frc::Quaternion quaternion{nativeArray[3], nativeArray[4], nativeArray[5], - nativeArray[6]}; - env->ReleaseDoubleArrayElements(arr, nativeArray, 0); - return {translation, frc::Rotation3d{quaternion}}; +static JClass pose3dCls; +static JClass quaternionCls; +static JClass rotation3dCls; +static JClass translation3dCls; +static JClass twist3dCls; + +static const JClassInit classes[] = { + {"edu/wpi/first/math/geometry/Pose3d", &pose3dCls}, + {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls}, + {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls}, + {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}, + {"edu/wpi/first/math/geometry/Twist3d", &twist3dCls}}; + +extern "C" { + +JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { + JNIEnv* env; + if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return JNI_ERR; + } + + for (auto& c : classes) { + *c.cls = JClass(env, c.name); + if (!*c.cls) { + return JNI_ERR; + } + } + + return JNI_VERSION_1_6; } -frc::Twist3d Twist3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { - jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); - frc::Twist3d twist = { - units::meter_t{nativeArray[0]}, units::meter_t{nativeArray[1]}, - units::meter_t{nativeArray[2]}, units::radian_t{nativeArray[3]}, - units::radian_t{nativeArray[4]}, units::radian_t{nativeArray[5]}}; - env->ReleaseDoubleArrayElements(arr, nativeArray, 0); - return twist; +JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { + JNIEnv* env; + if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return; + } + + for (auto& c : classes) { + c.cls->free(env); + } } -jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Pose3d& pose) { +} // extern "C" + +// frc::Pose3d Pose3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { +// jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); +// frc::Translation3d translation{units::meter_t{nativeArray[0]}, +// units::meter_t{nativeArray[1]}, +// units::meter_t{nativeArray[2]}}; +// frc::Quaternion quaternion{nativeArray[3], nativeArray[4], nativeArray[5], +// nativeArray[6]}; +// env->ReleaseDoubleArrayElements(arr, nativeArray, 0); +// return {translation, frc::Rotation3d{quaternion}}; +// } +// +// frc::Twist3d Twist3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { +// jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); +// frc::Twist3d twist = { +// units::meter_t{nativeArray[0]}, units::meter_t{nativeArray[1]}, +// units::meter_t{nativeArray[2]}, units::radian_t{nativeArray[3]}, +// units::radian_t{nativeArray[4]}, units::radian_t{nativeArray[5]}}; +// env->ReleaseDoubleArrayElements(arr, nativeArray, 0); +// return twist; +// } + +jobject MakeJObject(JNIEnv* env, const frc::Pose3d& pose) { + static jmethodID quaternionCtor = nullptr; + static jmethodID rotation3dCtor = nullptr; + static jmethodID pose3dCtor = nullptr; + static jmethodID translation3dCtor = nullptr; + static bool loaded = false; + if (!loaded) { + quaternionCtor = env->GetMethodID(quaternionCls, "", "(DDDD)V"); + rotation3dCtor = env->GetMethodID( + rotation3dCls, "", "(Ledu/wpi/first/math/geometry/Quaternion;)V"); + pose3dCtor = + env->GetMethodID(pose3dCls, "", + "(Ledu/wpi/first/math/geometry/Translation3d;Ledu/wpi/" + "first/math/geometry/Rotation3d;)V"); + translation3dCtor = env->GetMethodID(translation3dCls, "", "(DDD)V"); + loaded = true; + } + const frc::Quaternion& quaternion = pose.Rotation().GetQuaternion(); - jdoubleArray results = env->NewDoubleArray(7); - jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); - buffer[0] = pose.X().value(); - buffer[1] = pose.Y().value(); - buffer[2] = pose.Z().value(); - buffer[3] = quaternion.W(); - buffer[4] = quaternion.X(); - buffer[5] = quaternion.Y(); - buffer[6] = quaternion.Z(); - env->ReleaseDoubleArrayElements(results, buffer, 0); - return results; + + jobject jTranslation = + env->NewObject(translation3dCls, translation3dCtor, pose.X().value(), + pose.Y().value(), pose.Z().value()); + jobject jQuaternion = + env->NewObject(quaternionCls, quaternionCtor, quaternion.W(), + quaternion.X(), quaternion.Y(), quaternion.Z()); + jobject jRotation = + env->NewObject(rotation3dCls, rotation3dCtor, jQuaternion); + jobject jPose = + env->NewObject(pose3dCls, pose3dCtor, jTranslation, jRotation); + + return jPose; } -jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Twist3d& twist) { - jdoubleArray results = env->NewDoubleArray(6); - jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); - buffer[0] = twist.dx.value(); - buffer[1] = twist.dy.value(); - buffer[2] = twist.dz.value(); - buffer[3] = twist.rx.value(); - buffer[4] = twist.ry.value(); - buffer[5] = twist.rz.value(); - env->ReleaseDoubleArrayElements(results, buffer, 0); - return results; +jobject MakeJObject(JNIEnv* env, const frc::Twist3d& twist) { + static jmethodID twist3dCtor = nullptr; + if (!twist3dCtor) { + twist3dCtor = env->GetMethodID(twist3dCls, "", "(DDDDDD)V"); + } + + jobject jTwist = env->NewObject( + twist3dCls, twist3dCtor, twist.dx.value(), twist.dy.value(), + twist.dz.value(), twist.rx.value(), twist.ry.value(), twist.rz.value()); + + return jTwist; } +// jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Pose3d& pose) { +// const frc::Quaternion& quaternion = pose.Rotation().GetQuaternion(); +// jdoubleArray results = env->NewDoubleArray(7); +// jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); +// buffer[0] = pose.X().value(); +// buffer[1] = pose.Y().value(); +// buffer[2] = pose.Z().value(); +// buffer[3] = quaternion.W(); +// buffer[4] = quaternion.X(); +// buffer[5] = quaternion.Y(); +// buffer[6] = quaternion.Z(); +// env->ReleaseDoubleArrayElements(results, buffer, 0); +// return results; +// } +// +// jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Twist3d& twist) { +// jdoubleArray results = env->NewDoubleArray(6); +// jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); +// buffer[0] = twist.dx.value(); +// buffer[1] = twist.dy.value(); +// buffer[2] = twist.dz.value(); +// buffer[3] = twist.rx.value(); +// buffer[4] = twist.ry.value(); +// buffer[5] = twist.rz.value(); +// env->ReleaseDoubleArrayElements(results, buffer, 0); +// return results; +// } + std::vector GetElementsFromTrajectory( const frc::Trajectory& trajectory) { std::vector elements; @@ -240,35 +331,49 @@ Java_edu_wpi_first_math_WPIMathJNI_pow /* * Class: edu_wpi_first_math_WPIMathJNI * Method: expPose3d - * Signature: ([D[D)[D + * Signature: (DDDDDDDDDDDDD)Ljava/lang/Object; */ -JNIEXPORT jdoubleArray JNICALL +JNIEXPORT jobject JNICALL Java_edu_wpi_first_math_WPIMathJNI_expPose3d - (JNIEnv* env, jclass, jdoubleArray startPose, jdoubleArray twist) + (JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ, + jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz, + jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx, + jdouble twistRy, jdouble twistRz) { - frc::Pose3d startPoseCpp = Pose3dFromJDoubleArray(env, startPose); - frc::Twist3d twistCpp = Twist3dFromJDoubleArray(env, twist); + frc::Pose3d poseCpp = { + units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ}, + frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}}; + frc::Twist3d twistCpp = {units::meter_t{twistDx}, units::meter_t{twistDy}, + units::meter_t{twistDz}, units::radian_t{twistRx}, + units::radian_t{twistRy}, units::radian_t{twistRz}}; - frc::Pose3d result = startPoseCpp.Exp(twistCpp); + frc::Pose3d result = poseCpp.Exp(twistCpp); - return MakeJDoubleArray(env, result); + return MakeJObject(env, result); } /* * Class: edu_wpi_first_math_WPIMathJNI * Method: logPose3d - * Signature: ([D[D)[D + * Signature: (DDDDDDDDDDDDDD)Ljava/lang/Object; */ -JNIEXPORT jdoubleArray JNICALL +JNIEXPORT jobject JNICALL Java_edu_wpi_first_math_WPIMathJNI_logPose3d - (JNIEnv* env, jclass, jdoubleArray start, jdoubleArray end) + (JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ, + jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz, + jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx, + jdouble endQy, jdouble endQz) { - frc::Pose3d startCpp = Pose3dFromJDoubleArray(env, start); - frc::Pose3d endCpp = Pose3dFromJDoubleArray(env, end); + frc::Pose3d startCpp = { + units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ}, + frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}}; + frc::Pose3d endCpp = { + units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ}, + frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}}; frc::Twist3d result = startCpp.Log(endCpp); - return MakeJDoubleArray(env, result); + return MakeJObject(env, result); } /* From e6ae45d8e4516932c178025d49f5d7463fd4e5c2 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 15 Jul 2023 09:38:24 -0700 Subject: [PATCH 08/21] Clean up --- .../edu/wpi/first/math/geometry/Pose3d.java | 56 +---------------- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 61 ++----------------- 2 files changed, 9 insertions(+), 108 deletions(-) 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 6f626b83fa6..ef63217bd7e 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 @@ -195,7 +195,7 @@ public Pose3d relativeTo(Pose3d other) { * @return The new pose of the robot. */ public Pose3d exp(Twist3d twist) { - Quaternion quaternion = this.getRotation().getQuaternion(); + var quaternion = this.getRotation().getQuaternion(); return WPIMathJNI.expPose3d( this.getX(), this.getY(), @@ -220,8 +220,8 @@ public Pose3d exp(Twist3d twist) { * @return The twist that maps this to end. */ public Twist3d log(Pose3d end) { - Quaternion thisQuaternion = this.getRotation().getQuaternion(); - Quaternion endQuaternion = end.getRotation().getQuaternion(); + var thisQuaternion = this.getRotation().getQuaternion(); + var endQuaternion = end.getRotation().getQuaternion(); return WPIMathJNI.logPose3d( this.getX(), this.getY(), @@ -287,54 +287,4 @@ public Pose3d interpolate(Pose3d endValue, double t) { return this.exp(scaledTwist); } } - - /** - * Converts a Pose3d to the double array format for the JNI. - * - * @param pose The pose to convert. - * @return The double array representing the pose. - */ - private static double[] pose3dToDoubleArray(Pose3d pose) { - Quaternion quaternion = pose.getRotation().getQuaternion(); - return new double[] { - pose.getX(), - pose.getY(), - pose.getZ(), - quaternion.getW(), - quaternion.getX(), - quaternion.getY(), - quaternion.getZ() - }; - } - - /** - * Converts a Twist3d to the double array format for the JNI. - * - * @param twist The twist to convert. - * @return The double array representing the twist. - */ - private static double[] twist3dToDoubleArray(Twist3d twist) { - return new double[] {twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz}; - } - - /** - * Constructs a Pose3d from the JNI double array format. - * - * @param arr The JNI double array to convert. - * @return The corresponding pose. - */ - private static Pose3d pose3dFromDoubleArray(double[] arr) { - return new Pose3d( - arr[0], arr[1], arr[2], new Rotation3d(new Quaternion(arr[3], arr[4], arr[5], arr[6]))); - } - - /** - * Constructs a Twist3d from the JNI double array format. - * - * @param arr The JNI double array to convert. - * @return The corresponding twist. - */ - private static Twist3d twist3dFromDoubleArray(double[] arr) { - return new Twist3d(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5]); - } } diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index e090ee3c65d..c3b3eac73e4 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -102,27 +102,6 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { } // extern "C" -// frc::Pose3d Pose3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { -// jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); -// frc::Translation3d translation{units::meter_t{nativeArray[0]}, -// units::meter_t{nativeArray[1]}, -// units::meter_t{nativeArray[2]}}; -// frc::Quaternion quaternion{nativeArray[3], nativeArray[4], nativeArray[5], -// nativeArray[6]}; -// env->ReleaseDoubleArrayElements(arr, nativeArray, 0); -// return {translation, frc::Rotation3d{quaternion}}; -// } -// -// frc::Twist3d Twist3dFromJDoubleArray(JNIEnv* env, jdoubleArray arr) { -// jdouble* nativeArray = env->GetDoubleArrayElements(arr, nullptr); -// frc::Twist3d twist = { -// units::meter_t{nativeArray[0]}, units::meter_t{nativeArray[1]}, -// units::meter_t{nativeArray[2]}, units::radian_t{nativeArray[3]}, -// units::radian_t{nativeArray[4]}, units::radian_t{nativeArray[5]}}; -// env->ReleaseDoubleArrayElements(arr, nativeArray, 0); -// return twist; -// } - jobject MakeJObject(JNIEnv* env, const frc::Pose3d& pose) { static jmethodID quaternionCtor = nullptr; static jmethodID rotation3dCtor = nullptr; @@ -170,34 +149,6 @@ jobject MakeJObject(JNIEnv* env, const frc::Twist3d& twist) { return jTwist; } -// jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Pose3d& pose) { -// const frc::Quaternion& quaternion = pose.Rotation().GetQuaternion(); -// jdoubleArray results = env->NewDoubleArray(7); -// jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); -// buffer[0] = pose.X().value(); -// buffer[1] = pose.Y().value(); -// buffer[2] = pose.Z().value(); -// buffer[3] = quaternion.W(); -// buffer[4] = quaternion.X(); -// buffer[5] = quaternion.Y(); -// buffer[6] = quaternion.Z(); -// env->ReleaseDoubleArrayElements(results, buffer, 0); -// return results; -// } -// -// jdoubleArray MakeJDoubleArray(JNIEnv* env, const frc::Twist3d& twist) { -// jdoubleArray results = env->NewDoubleArray(6); -// jdouble* buffer = env->GetDoubleArrayElements(results, nullptr); -// buffer[0] = twist.dx.value(); -// buffer[1] = twist.dy.value(); -// buffer[2] = twist.dz.value(); -// buffer[3] = twist.rx.value(); -// buffer[4] = twist.ry.value(); -// buffer[5] = twist.rz.value(); -// env->ReleaseDoubleArrayElements(results, buffer, 0); -// return results; -// } - std::vector GetElementsFromTrajectory( const frc::Trajectory& trajectory) { std::vector elements; @@ -340,14 +291,14 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx, jdouble twistRy, jdouble twistRz) { - frc::Pose3d poseCpp = { + frc::Pose3d pose{ units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ}, frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}}; - frc::Twist3d twistCpp = {units::meter_t{twistDx}, units::meter_t{twistDy}, + frc::Twist3d twist{units::meter_t{twistDx}, units::meter_t{twistDy}, units::meter_t{twistDz}, units::radian_t{twistRx}, units::radian_t{twistRy}, units::radian_t{twistRz}}; - frc::Pose3d result = poseCpp.Exp(twistCpp); + frc::Pose3d result = pose.Exp(twist); return MakeJObject(env, result); } @@ -364,14 +315,14 @@ Java_edu_wpi_first_math_WPIMathJNI_logPose3d jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx, jdouble endQy, jdouble endQz) { - frc::Pose3d startCpp = { + frc::Pose3d startPose{ units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ}, frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}}; - frc::Pose3d endCpp = { + frc::Pose3d endPose{ units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ}, frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}}; - frc::Twist3d result = startCpp.Log(endCpp); + frc::Twist3d result = startPose.Log(endPose); return MakeJObject(env, result); } From 40bad3dc3aa6449c50a304525a926a2e424d05f0 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 15 Jul 2023 10:01:11 -0700 Subject: [PATCH 09/21] Add warmup time --- .../java/edu/wpi/first/math/WPIMathJNITest.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 1e66a5d9e23..94fad749b0e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -41,11 +41,18 @@ public static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { public void timePose3dExp() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); + final int warmupCount = 100_000; final int count = 1_000; var msg = new StringBuilder(); msg.append("Pose3d.exp():\n"); msg.append(" start = " + start + "\n"); msg.append(" twist = " + twist + "\n"); + msg.append( + " warm up (" + + warmupCount + + " iterations): average " + + timePose3dExpMicros(start, twist, warmupCount) + + "µs\n"); msg.append(" iterations per run: " + count + "\n"); for (int i = 1; i <= 5; i++) { msg.append(" run " + i + ": average " + timePose3dExpMicros(start, twist, count) + "μs\n"); @@ -57,11 +64,18 @@ public void timePose3dExp() { public void timePose3dLog() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); + final int warmupCount = 100_000; final int count = 1_000; var msg = new StringBuilder(); msg.append("Pose3d.log():\n"); msg.append(" start = " + start + "\n"); msg.append(" end = " + end + "\n"); + msg.append( + " warm up (" + + warmupCount + + " iterations): average " + + timePose3dLogMicros(start, end, warmupCount) + + "µs\n"); msg.append(" iterations per run: " + count + "\n"); for (int i = 1; i <= 5; i++) { msg.append(" run " + i + ": average " + timePose3dLogMicros(start, end, count) + "μs\n"); From 5f3cd03fcf978595046e9f0b01bbe7df9267147e Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 15 Jul 2023 12:28:37 -0700 Subject: [PATCH 10/21] wpiformat --- wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index c3b3eac73e4..d86c207a946 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -295,8 +295,8 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ}, frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}}; frc::Twist3d twist{units::meter_t{twistDx}, units::meter_t{twistDy}, - units::meter_t{twistDz}, units::radian_t{twistRx}, - units::radian_t{twistRy}, units::radian_t{twistRz}}; + units::meter_t{twistDz}, units::radian_t{twistRx}, + units::radian_t{twistRy}, units::radian_t{twistRz}}; frc::Pose3d result = pose.Exp(twist); From 1449c5795d27701e2460a5c883322ec356c9ca0c Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 15 Jul 2023 12:29:21 -0700 Subject: [PATCH 11/21] Fix gradlew wpimath:run --- wpimath/build.gradle | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wpimath/build.gradle b/wpimath/build.gradle index c604dd16449..8b188348290 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -43,6 +43,10 @@ dependencies { api "com.fasterxml.jackson.core:jackson-databind:2.12.4" } +configurations { + devImplementation.extendsFrom(implementation) +} + def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja") def natFileInput = file("src/generate/Nat.java.jinja") def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/math/numbers") From 9a1278fd862d4da3dee716f345859413001d2f69 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 15 Jul 2023 12:45:22 -0700 Subject: [PATCH 12/21] Move timing to wpimath DevMain.java --- .../dev/java/edu/wpi/first/math/DevMain.java | 68 ++++++++++++++++++- .../edu/wpi/first/math/WPIMathJNITest.java | 66 ------------------ 2 files changed, 66 insertions(+), 68 deletions(-) diff --git a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java index c7b779e0073..ebe9cea207d 100644 --- a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java +++ b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java @@ -4,11 +4,75 @@ package edu.wpi.first.math; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Twist3d; + public final class DevMain { + public static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) { + final long startTime = System.nanoTime(); + for (int i = 0; i < count; i++) { + start.exp(twist); + } + final long endTime = System.nanoTime(); + double totalTimeMicros = (endTime - startTime) / 1000.0; + return totalTimeMicros / count; + } + + public static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { + final long startTime = System.nanoTime(); + for (int i = 0; i < count; i++) { + start.log(end); + } + final long endTime = System.nanoTime(); + double totalTimeMicros = (endTime - startTime) / 1000.0; + return totalTimeMicros / count; + } + + public static void timeSuitePose3dExp() { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); + final int warmupCount = 100_000; + final int normalCount = 1_000; + System.out.println("Pose3d.exp():"); + System.out.println(" start = " + start); + System.out.println(" twist = " + twist); + System.out.println( + " warm up (" + + warmupCount + + " iterations): average " + + timePose3dExpMicros(start, twist, warmupCount) + + "µs"); + System.out.println(" iterations per run: " + normalCount); + for (int i = 1; i <= 5; i++) { + System.out.println(" run " + i + ": average " + timePose3dExpMicros(start, twist, normalCount) + "μs"); + } + } + + public static void timeSuitePose3dLog() { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); + final int warmupCount = 100_000; + final int normalCount = 1_000; + System.out.println("Pose3d.log():"); + System.out.println(" start = " + start); + System.out.println(" end = " + end); + System.out.println( + " warm up (" + + warmupCount + + " iterations): average " + + timePose3dLogMicros(start, end, warmupCount) + + "µs"); + System.out.println(" iterations per run: " + normalCount); + for (int i = 1; i <= 5; i++) { + System.out.println(" run " + i + ": average " + timePose3dLogMicros(start, end, normalCount) + "μs"); + } + } + /** Main entry point. */ public static void main(String[] args) { - System.out.println("Hello World!"); - System.out.println(MathUtil.angleModulus(-5.0)); + timeSuitePose3dExp(); + timeSuitePose3dLog(); } private DevMain() {} diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 94fad749b0e..4da211ecdc5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -16,70 +16,4 @@ public class WPIMathJNITest { public void testLink() { assertDoesNotThrow(WPIMathJNI::forceLoad); } - - public static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) { - final long startTime = System.nanoTime(); - for (int i = 0; i < count; i++) { - start.exp(twist); - } - final long endTime = System.nanoTime(); - double totalTimeMicros = (endTime - startTime) / 1000.0; - return totalTimeMicros / count; - } - - public static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { - final long startTime = System.nanoTime(); - for (int i = 0; i < count; i++) { - start.log(end); - } - final long endTime = System.nanoTime(); - double totalTimeMicros = (endTime - startTime) / 1000.0; - return totalTimeMicros / count; - } - - @Test - public void timePose3dExp() { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); - final int warmupCount = 100_000; - final int count = 1_000; - var msg = new StringBuilder(); - msg.append("Pose3d.exp():\n"); - msg.append(" start = " + start + "\n"); - msg.append(" twist = " + twist + "\n"); - msg.append( - " warm up (" - + warmupCount - + " iterations): average " - + timePose3dExpMicros(start, twist, warmupCount) - + "µs\n"); - msg.append(" iterations per run: " + count + "\n"); - for (int i = 1; i <= 5; i++) { - msg.append(" run " + i + ": average " + timePose3dExpMicros(start, twist, count) + "μs\n"); - } - throw new RuntimeException("report:\n" + msg); - } - - @Test - public void timePose3dLog() { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); - final int warmupCount = 100_000; - final int count = 1_000; - var msg = new StringBuilder(); - msg.append("Pose3d.log():\n"); - msg.append(" start = " + start + "\n"); - msg.append(" end = " + end + "\n"); - msg.append( - " warm up (" - + warmupCount - + " iterations): average " - + timePose3dLogMicros(start, end, warmupCount) - + "µs\n"); - msg.append(" iterations per run: " + count + "\n"); - for (int i = 1; i <= 5; i++) { - msg.append(" run " + i + ": average " + timePose3dLogMicros(start, end, count) + "μs\n"); - } - throw new RuntimeException("report:\n" + msg); - } } From a9491fa3677fce7ccd2992414e57514661a541e4 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 15 Jul 2023 18:53:34 -0700 Subject: [PATCH 13/21] Formatting --- .../src/dev/java/edu/wpi/first/math/DevMain.java | 14 ++++++++------ .../java/edu/wpi/first/math/WPIMathJNITest.java | 3 --- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java index ebe9cea207d..a7531c0de39 100644 --- a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java +++ b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.geometry.Twist3d; public final class DevMain { - public static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) { + private static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) { final long startTime = System.nanoTime(); for (int i = 0; i < count; i++) { start.exp(twist); @@ -19,7 +19,7 @@ public static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) return totalTimeMicros / count; } - public static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { + private static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { final long startTime = System.nanoTime(); for (int i = 0; i < count; i++) { start.log(end); @@ -29,7 +29,7 @@ public static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { return totalTimeMicros / count; } - public static void timeSuitePose3dExp() { + private static void timeSuitePose3dExp() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); final int warmupCount = 100_000; @@ -45,11 +45,12 @@ public static void timeSuitePose3dExp() { + "µs"); System.out.println(" iterations per run: " + normalCount); for (int i = 1; i <= 5; i++) { - System.out.println(" run " + i + ": average " + timePose3dExpMicros(start, twist, normalCount) + "μs"); + System.out.println( + " run " + i + ": average " + timePose3dExpMicros(start, twist, normalCount) + "μs"); } } - public static void timeSuitePose3dLog() { + private static void timeSuitePose3dLog() { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); final int warmupCount = 100_000; @@ -65,7 +66,8 @@ public static void timeSuitePose3dLog() { + "µs"); System.out.println(" iterations per run: " + normalCount); for (int i = 1; i <= 5; i++) { - System.out.println(" run " + i + ": average " + timePose3dLogMicros(start, end, normalCount) + "μs"); + System.out.println( + " run " + i + ": average " + timePose3dLogMicros(start, end, normalCount) + "μs"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java index 4da211ecdc5..6a1bf2edaff 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java @@ -6,9 +6,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Twist3d; import org.junit.jupiter.api.Test; public class WPIMathJNITest { From b2b5071c3f456e58450223bc4a5ed5f43f423bcb Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 17 Jul 2023 10:12:55 -0700 Subject: [PATCH 14/21] Write toString() of calculation to DataLog --- wpimath/build.gradle | 1 + .../dev/java/edu/wpi/first/math/DevMain.java | 56 +++++++++++++------ 2 files changed, 39 insertions(+), 18 deletions(-) diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 8b188348290..0f744fb349d 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -41,6 +41,7 @@ dependencies { api "com.fasterxml.jackson.core:jackson-annotations:2.12.4" api "com.fasterxml.jackson.core:jackson-core:2.12.4" api "com.fasterxml.jackson.core:jackson-databind:2.12.4" + devImplementation project(":wpiutil") } configurations { diff --git a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java index a7531c0de39..67fcfbfeb78 100644 --- a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java +++ b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java @@ -7,29 +7,39 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.StringLogEntry; public final class DevMain { - private static double timePose3dExpMicros(Pose3d start, Twist3d twist, int count) { - final long startTime = System.nanoTime(); + private static double timePose3dExpMicros( + StringLogEntry discardEntry, Pose3d start, Twist3d twist, int count) { + long totalNanos = 0; for (int i = 0; i < count; i++) { - start.exp(twist); + final long startTime = System.nanoTime(); + final var result = start.exp(twist); + final long endTime = System.nanoTime(); + totalNanos += endTime - startTime; + discardEntry.append(result.toString()); } - final long endTime = System.nanoTime(); - double totalTimeMicros = (endTime - startTime) / 1000.0; + double totalTimeMicros = totalNanos / 1000.0; return totalTimeMicros / count; } - private static double timePose3dLogMicros(Pose3d start, Pose3d end, int count) { - final long startTime = System.nanoTime(); + private static double timePose3dLogMicros( + StringLogEntry discardEntry, Pose3d start, Pose3d end, int count) { + long totalNanos = 0; for (int i = 0; i < count; i++) { - start.log(end); + final long startTime = System.nanoTime(); + final var result = start.log(end); + final long endTime = System.nanoTime(); + totalNanos += endTime - startTime; + discardEntry.append(result.toString()); } - final long endTime = System.nanoTime(); - double totalTimeMicros = (endTime - startTime) / 1000.0; + double totalTimeMicros = totalNanos / 1000.0; return totalTimeMicros / count; } - private static void timeSuitePose3dExp() { + private static void timeSuitePose3dExp(StringLogEntry discardEntry) { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); final int warmupCount = 100_000; @@ -41,16 +51,20 @@ private static void timeSuitePose3dExp() { " warm up (" + warmupCount + " iterations): average " - + timePose3dExpMicros(start, twist, warmupCount) + + timePose3dExpMicros(discardEntry, start, twist, warmupCount) + "µs"); System.out.println(" iterations per run: " + normalCount); for (int i = 1; i <= 5; i++) { System.out.println( - " run " + i + ": average " + timePose3dExpMicros(start, twist, normalCount) + "μs"); + " run " + + i + + ": average " + + timePose3dExpMicros(discardEntry, start, twist, normalCount) + + "μs"); } } - private static void timeSuitePose3dLog() { + private static void timeSuitePose3dLog(StringLogEntry discardEntry) { final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); final int warmupCount = 100_000; @@ -62,19 +76,25 @@ private static void timeSuitePose3dLog() { " warm up (" + warmupCount + " iterations): average " - + timePose3dLogMicros(start, end, warmupCount) + + timePose3dLogMicros(discardEntry, start, end, warmupCount) + "µs"); System.out.println(" iterations per run: " + normalCount); for (int i = 1; i <= 5; i++) { System.out.println( - " run " + i + ": average " + timePose3dLogMicros(start, end, normalCount) + "μs"); + " run " + + i + + ": average " + + timePose3dLogMicros(discardEntry, start, end, normalCount) + + "μs"); } } /** Main entry point. */ public static void main(String[] args) { - timeSuitePose3dExp(); - timeSuitePose3dLog(); + final var datalog = new DataLog(); + final var discardEntry = new StringLogEntry(datalog, "discard"); + timeSuitePose3dExp(discardEntry); + timeSuitePose3dLog(discardEntry); } private DevMain() {} From b7918f171c39b7ef1cffac4a4b05f4ac19a5ecd3 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 17 Jul 2023 10:25:59 -0700 Subject: [PATCH 15/21] Report standard deviation --- .../dev/java/edu/wpi/first/math/DevMain.java | 52 +++++++++++-------- 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java index 67fcfbfeb78..fbe02693ad7 100644 --- a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java +++ b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java @@ -11,32 +11,44 @@ import edu.wpi.first.util.datalog.StringLogEntry; public final class DevMain { - private static double timePose3dExpMicros( + private static double[] timePose3dExpMicros( StringLogEntry discardEntry, Pose3d start, Twist3d twist, int count) { - long totalNanos = 0; + double[] times = new double[count]; for (int i = 0; i < count; i++) { final long startTime = System.nanoTime(); final var result = start.exp(twist); final long endTime = System.nanoTime(); - totalNanos += endTime - startTime; + times[i] = (endTime - startTime) / 1000.0; discardEntry.append(result.toString()); } - double totalTimeMicros = totalNanos / 1000.0; - return totalTimeMicros / count; + return times; } - private static double timePose3dLogMicros( + private static double[] timePose3dLogMicros( StringLogEntry discardEntry, Pose3d start, Pose3d end, int count) { - long totalNanos = 0; + double[] times = new double[count]; for (int i = 0; i < count; i++) { final long startTime = System.nanoTime(); final var result = start.log(end); final long endTime = System.nanoTime(); - totalNanos += endTime - startTime; + times[i] = (endTime - startTime) / 1000.0; discardEntry.append(result.toString()); } - double totalTimeMicros = totalNanos / 1000.0; - return totalTimeMicros / count; + return times; + } + + private static String summarizeMicros(double[] times) { + double total = 0; + for (double time : times) { + total += time; + } + double mean = total / times.length; + double sumsOfSquares = 0; + for (double time : times) { + sumsOfSquares += (time - mean) * (time - mean); + } + double stdDev = Math.sqrt(sumsOfSquares / times.length); + return "mean " + mean + "µs, std dev " + stdDev + "µs"; } private static void timeSuitePose3dExp(StringLogEntry discardEntry) { @@ -50,17 +62,15 @@ private static void timeSuitePose3dExp(StringLogEntry discardEntry) { System.out.println( " warm up (" + warmupCount - + " iterations): average " - + timePose3dExpMicros(discardEntry, start, twist, warmupCount) - + "µs"); + + " iterations): " + + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, warmupCount))); System.out.println(" iterations per run: " + normalCount); for (int i = 1; i <= 5; i++) { System.out.println( " run " + i - + ": average " - + timePose3dExpMicros(discardEntry, start, twist, normalCount) - + "μs"); + + ": " + + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, normalCount))); } } @@ -75,17 +85,15 @@ private static void timeSuitePose3dLog(StringLogEntry discardEntry) { System.out.println( " warm up (" + warmupCount - + " iterations): average " - + timePose3dLogMicros(discardEntry, start, end, warmupCount) - + "µs"); + + " iterations): " + + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, warmupCount))); System.out.println(" iterations per run: " + normalCount); for (int i = 1; i <= 5; i++) { System.out.println( " run " + i - + ": average " - + timePose3dLogMicros(discardEntry, start, end, normalCount) - + "μs"); + + ": " + + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, normalCount))); } } From 30dd17f9ef0881029b16294c1c772b4747f4e3d6 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 17 Jul 2023 11:30:28 -0700 Subject: [PATCH 16/21] Change dev executable to release mode --- shared/javacpp/setupBuild.gradle | 2 +- shared/jni/setupBuild.gradle | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index 5977ce7d90a..c053dda8702 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -65,7 +65,7 @@ model { // By default, a development executable will be generated. This is to help the case of // testing specific functionality of the library. "${nativeName}Dev"(NativeExecutableSpec) { - targetBuildTypes 'debug' + targetBuildTypes 'release' sources { cpp { source { diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index 7e5d04d5a71..09036ec4e7a 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -195,7 +195,7 @@ model { // By default, a development executable will be generated. This is to help the case of // testing specific functionality of the library. "${nativeName}Dev"(NativeExecutableSpec) { - targetBuildTypes 'debug' + targetBuildTypes 'release' sources { cpp { From ffcc914ef84b37b868ff5e0e8f9376e9968eb787 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 17 Jul 2023 13:31:02 -0700 Subject: [PATCH 17/21] Change return type to double[] --- .../java/edu/wpi/first/math/WPIMathJNI.java | 14 +- .../edu/wpi/first/math/geometry/Pose3d.java | 73 ++++++---- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 125 ++++-------------- 3 files changed, 80 insertions(+), 132 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 3f3baad4a6d..f843fde7b9d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -4,8 +4,6 @@ package edu.wpi.first.math; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; @@ -81,6 +79,8 @@ public static native void dare( /** * Obtain a Pose3d from a (constant curvature) velocity. * + *

The double array returned is of the form [dx, dy, dz, qx, qy, qz]. + * * @param poseX The pose's translational X component. * @param poseY The pose's translational Y component. * @param poseZ The pose's translational Z component. @@ -94,9 +94,9 @@ public static native void dare( * @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. + * @return The new pose as a double array. */ - public static native Pose3d expPose3d( + public static native double[] expPose3d( double poseX, double poseY, double poseZ, @@ -114,6 +114,8 @@ public static native Pose3d expPose3d( /** * Returns a Twist3d that maps the starting pose to the end pose. * + *

The double array returned is of the form [dx, dy, dz, rx, ry, rz]. + * * @param startX The starting pose's translational X component. * @param startY The starting pose's translational Y component. * @param startZ The starting pose's translational Z component. @@ -128,9 +130,9 @@ public static native Pose3d expPose3d( * @param endQx The ending pose quaternion's X component. * @param endQy The ending pose quaternion's Y component. * @param endQz The ending pose quaternion's Z component. - * @return The twist that maps start to end. + * @return The twist that maps start to end as a double array. */ - public static native Twist3d logPose3d( + public static native double[] logPose3d( double startX, double startY, double startZ, 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 ef63217bd7e..26a3d56d5a7 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 @@ -196,20 +196,27 @@ public Pose3d relativeTo(Pose3d other) { */ public Pose3d exp(Twist3d twist) { var quaternion = this.getRotation().getQuaternion(); - return WPIMathJNI.expPose3d( - this.getX(), - this.getY(), - this.getZ(), - quaternion.getW(), - quaternion.getX(), - quaternion.getY(), - quaternion.getZ(), - twist.dx, - twist.dy, - twist.dz, - twist.rx, - twist.ry, - twist.rz); + double[] resultArray = + WPIMathJNI.expPose3d( + this.getX(), + this.getY(), + this.getZ(), + quaternion.getW(), + quaternion.getX(), + quaternion.getY(), + quaternion.getZ(), + twist.dx, + twist.dy, + twist.dz, + twist.rx, + twist.ry, + twist.rz); + return new Pose3d( + resultArray[0], + resultArray[1], + resultArray[2], + new Rotation3d( + new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6]))); } /** @@ -222,21 +229,29 @@ public Pose3d exp(Twist3d twist) { public Twist3d log(Pose3d end) { var thisQuaternion = this.getRotation().getQuaternion(); var endQuaternion = end.getRotation().getQuaternion(); - return WPIMathJNI.logPose3d( - this.getX(), - this.getY(), - this.getZ(), - thisQuaternion.getW(), - thisQuaternion.getX(), - thisQuaternion.getY(), - thisQuaternion.getZ(), - end.getX(), - end.getY(), - end.getZ(), - endQuaternion.getW(), - endQuaternion.getX(), - endQuaternion.getY(), - endQuaternion.getZ()); + double[] resultArray = + WPIMathJNI.logPose3d( + this.getX(), + this.getY(), + this.getZ(), + thisQuaternion.getW(), + thisQuaternion.getX(), + thisQuaternion.getY(), + thisQuaternion.getZ(), + end.getX(), + end.getY(), + end.getZ(), + endQuaternion.getW(), + endQuaternion.getX(), + endQuaternion.getY(), + endQuaternion.getZ()); + return new Twist3d( + resultArray[0], + resultArray[1], + resultArray[2], + resultArray[3], + resultArray[4], + resultArray[5]); } /** diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index d86c207a946..a50968e8276 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -58,97 +58,6 @@ bool IsStabilizable(const Eigen::Ref& A, } // namespace -static JClass pose3dCls; -static JClass quaternionCls; -static JClass rotation3dCls; -static JClass translation3dCls; -static JClass twist3dCls; - -static const JClassInit classes[] = { - {"edu/wpi/first/math/geometry/Pose3d", &pose3dCls}, - {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls}, - {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls}, - {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}, - {"edu/wpi/first/math/geometry/Twist3d", &twist3dCls}}; - -extern "C" { - -JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { - JNIEnv* env; - if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { - return JNI_ERR; - } - - for (auto& c : classes) { - *c.cls = JClass(env, c.name); - if (!*c.cls) { - return JNI_ERR; - } - } - - return JNI_VERSION_1_6; -} - -JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { - JNIEnv* env; - if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { - return; - } - - for (auto& c : classes) { - c.cls->free(env); - } -} - -} // extern "C" - -jobject MakeJObject(JNIEnv* env, const frc::Pose3d& pose) { - static jmethodID quaternionCtor = nullptr; - static jmethodID rotation3dCtor = nullptr; - static jmethodID pose3dCtor = nullptr; - static jmethodID translation3dCtor = nullptr; - static bool loaded = false; - if (!loaded) { - quaternionCtor = env->GetMethodID(quaternionCls, "", "(DDDD)V"); - rotation3dCtor = env->GetMethodID( - rotation3dCls, "", "(Ledu/wpi/first/math/geometry/Quaternion;)V"); - pose3dCtor = - env->GetMethodID(pose3dCls, "", - "(Ledu/wpi/first/math/geometry/Translation3d;Ledu/wpi/" - "first/math/geometry/Rotation3d;)V"); - translation3dCtor = env->GetMethodID(translation3dCls, "", "(DDD)V"); - loaded = true; - } - - const frc::Quaternion& quaternion = pose.Rotation().GetQuaternion(); - - jobject jTranslation = - env->NewObject(translation3dCls, translation3dCtor, pose.X().value(), - pose.Y().value(), pose.Z().value()); - jobject jQuaternion = - env->NewObject(quaternionCls, quaternionCtor, quaternion.W(), - quaternion.X(), quaternion.Y(), quaternion.Z()); - jobject jRotation = - env->NewObject(rotation3dCls, rotation3dCtor, jQuaternion); - jobject jPose = - env->NewObject(pose3dCls, pose3dCtor, jTranslation, jRotation); - - return jPose; -} - -jobject MakeJObject(JNIEnv* env, const frc::Twist3d& twist) { - static jmethodID twist3dCtor = nullptr; - if (!twist3dCtor) { - twist3dCtor = env->GetMethodID(twist3dCls, "", "(DDDDDD)V"); - } - - jobject jTwist = env->NewObject( - twist3dCls, twist3dCtor, twist.dx.value(), twist.dy.value(), - twist.dz.value(), twist.rx.value(), twist.ry.value(), twist.rz.value()); - - return jTwist; -} - std::vector GetElementsFromTrajectory( const frc::Trajectory& trajectory) { std::vector elements; @@ -282,9 +191,9 @@ Java_edu_wpi_first_math_WPIMathJNI_pow /* * Class: edu_wpi_first_math_WPIMathJNI * Method: expPose3d - * Signature: (DDDDDDDDDDDDD)Ljava/lang/Object; + * Signature: (DDDDDDDDDDDDD)[D */ -JNIEXPORT jobject JNICALL +JNIEXPORT jdoubleArray JNICALL Java_edu_wpi_first_math_WPIMathJNI_expPose3d (JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ, jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz, @@ -300,15 +209,27 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d frc::Pose3d result = pose.Exp(twist); - return MakeJObject(env, result); + jdoubleArray resultArray = env->NewDoubleArray(7); + jdouble* nativeResultArray = + env->GetDoubleArrayElements(resultArray, nullptr); + const frc::Quaternion& resultQuaternion = result.Rotation().GetQuaternion(); + nativeResultArray[0] = result.X().value(); + nativeResultArray[1] = result.Y().value(); + nativeResultArray[2] = result.Z().value(); + nativeResultArray[3] = resultQuaternion.W(); + nativeResultArray[4] = resultQuaternion.X(); + nativeResultArray[5] = resultQuaternion.Y(); + nativeResultArray[6] = resultQuaternion.Z(); + env->ReleaseDoubleArrayElements(resultArray, nativeResultArray, 0); + return resultArray; } /* * Class: edu_wpi_first_math_WPIMathJNI * Method: logPose3d - * Signature: (DDDDDDDDDDDDDD)Ljava/lang/Object; + * Signature: (DDDDDDDDDDDDDD)[D */ -JNIEXPORT jobject JNICALL +JNIEXPORT jdoubleArray JNICALL Java_edu_wpi_first_math_WPIMathJNI_logPose3d (JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ, jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz, @@ -324,7 +245,17 @@ Java_edu_wpi_first_math_WPIMathJNI_logPose3d frc::Twist3d result = startPose.Log(endPose); - return MakeJObject(env, result); + jdoubleArray resultArray = env->NewDoubleArray(6); + jdouble* nativeResultArray = + env->GetDoubleArrayElements(resultArray, nullptr); + nativeResultArray[0] = result.dx.value(); + nativeResultArray[1] = result.dy.value(); + nativeResultArray[2] = result.dz.value(); + nativeResultArray[3] = result.rx.value(); + nativeResultArray[4] = result.ry.value(); + nativeResultArray[5] = result.rz.value(); + env->ReleaseDoubleArrayElements(resultArray, nativeResultArray, 0); + return resultArray; } /* From b9fbdf5fb85991988cf5a45a3c435deb554a2c5a Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 17 Jul 2023 13:43:45 -0700 Subject: [PATCH 18/21] Copy timing code to MyRobot ctor --- myRobot/src/main/java/frc/robot/MyRobot.java | 99 ++++++++++++++++++++ 1 file changed, 99 insertions(+) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index a5f88bd3f36..b72fb89452d 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,9 +4,108 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.TimedRobot; public class MyRobot extends TimedRobot { + private static double[] timePose3dExpMicros( + StringLogEntry discardEntry, Pose3d start, Twist3d twist, int count) { + double[] times = new double[count]; + for (int i = 0; i < count; i++) { + final long startTime = System.nanoTime(); + final var result = start.exp(twist); + final long endTime = System.nanoTime(); + times[i] = (endTime - startTime) / 1000.0; + discardEntry.append(result.toString()); + } + return times; + } + + private static double[] timePose3dLogMicros( + StringLogEntry discardEntry, Pose3d start, Pose3d end, int count) { + double[] times = new double[count]; + for (int i = 0; i < count; i++) { + final long startTime = System.nanoTime(); + final var result = start.log(end); + final long endTime = System.nanoTime(); + times[i] = (endTime - startTime) / 1000.0; + discardEntry.append(result.toString()); + } + return times; + } + + private static String summarizeMicros(double[] times) { + double total = 0; + for (double time : times) { + total += time; + } + double mean = total / times.length; + double sumsOfSquares = 0; + for (double time : times) { + sumsOfSquares += (time - mean) * (time - mean); + } + double stdDev = Math.sqrt(sumsOfSquares / times.length); + return "mean " + mean + "µs, std dev " + stdDev + "µs"; + } + + private static void timeSuitePose3dExp(StringLogEntry discardEntry) { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); + final int warmupCount = 100_000; + final int normalCount = 1_000; + System.out.println("Pose3d.exp():"); + System.out.println(" start = " + start); + System.out.println(" twist = " + twist); + System.out.println( + " warm up (" + + warmupCount + + " iterations): " + + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, warmupCount))); + System.out.println(" iterations per run: " + normalCount); + for (int i = 1; i <= 5; i++) { + System.out.println( + " run " + + i + + ": " + + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, normalCount))); + } + } + + private static void timeSuitePose3dLog(StringLogEntry discardEntry) { + final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); + final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); + final int warmupCount = 100_000; + final int normalCount = 1_000; + System.out.println("Pose3d.log():"); + System.out.println(" start = " + start); + System.out.println(" end = " + end); + System.out.println( + " warm up (" + + warmupCount + + " iterations): " + + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, warmupCount))); + System.out.println(" iterations per run: " + normalCount); + for (int i = 1; i <= 5; i++) { + System.out.println( + " run " + + i + + ": " + + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, normalCount))); + } + } + + /** Constructs a MyRobot instance. Also times Pose3d.exp() and Pose3d.log(). */ + public MyRobot() { + final var datalog = new DataLog(); + final var discardEntry = new StringLogEntry(datalog, "discard"); + timeSuitePose3dExp(discardEntry); + timeSuitePose3dLog(discardEntry); + } + /** * This function is run when the robot is first started up and should be used for any * initialization code. From c7927ceba71e30eaf02b60466e77c915d2c5b6eb Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 17 Jul 2023 13:57:03 -0700 Subject: [PATCH 19/21] Use auto --- wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index a50968e8276..d2ef4c45c22 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -212,7 +212,7 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d jdoubleArray resultArray = env->NewDoubleArray(7); jdouble* nativeResultArray = env->GetDoubleArrayElements(resultArray, nullptr); - const frc::Quaternion& resultQuaternion = result.Rotation().GetQuaternion(); + const auto& resultQuaternion = result.Rotation().GetQuaternion(); nativeResultArray[0] = result.X().value(); nativeResultArray[1] = result.Y().value(); nativeResultArray[2] = result.Z().value(); From 6553a265648b194d16e354819d6dbd53efb7ce34 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Tue, 18 Jul 2023 13:04:56 -0700 Subject: [PATCH 20/21] Remove testing code --- myRobot/src/main/java/frc/robot/MyRobot.java | 99 ------------------- shared/javacpp/setupBuild.gradle | 2 +- shared/jni/setupBuild.gradle | 2 +- wpimath/build.gradle | 5 - .../dev/java/edu/wpi/first/math/DevMain.java | 98 +----------------- 5 files changed, 4 insertions(+), 202 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index b72fb89452d..a5f88bd3f36 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,108 +4,9 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Twist3d; -import edu.wpi.first.util.datalog.DataLog; -import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.TimedRobot; public class MyRobot extends TimedRobot { - private static double[] timePose3dExpMicros( - StringLogEntry discardEntry, Pose3d start, Twist3d twist, int count) { - double[] times = new double[count]; - for (int i = 0; i < count; i++) { - final long startTime = System.nanoTime(); - final var result = start.exp(twist); - final long endTime = System.nanoTime(); - times[i] = (endTime - startTime) / 1000.0; - discardEntry.append(result.toString()); - } - return times; - } - - private static double[] timePose3dLogMicros( - StringLogEntry discardEntry, Pose3d start, Pose3d end, int count) { - double[] times = new double[count]; - for (int i = 0; i < count; i++) { - final long startTime = System.nanoTime(); - final var result = start.log(end); - final long endTime = System.nanoTime(); - times[i] = (endTime - startTime) / 1000.0; - discardEntry.append(result.toString()); - } - return times; - } - - private static String summarizeMicros(double[] times) { - double total = 0; - for (double time : times) { - total += time; - } - double mean = total / times.length; - double sumsOfSquares = 0; - for (double time : times) { - sumsOfSquares += (time - mean) * (time - mean); - } - double stdDev = Math.sqrt(sumsOfSquares / times.length); - return "mean " + mean + "µs, std dev " + stdDev + "µs"; - } - - private static void timeSuitePose3dExp(StringLogEntry discardEntry) { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); - final int warmupCount = 100_000; - final int normalCount = 1_000; - System.out.println("Pose3d.exp():"); - System.out.println(" start = " + start); - System.out.println(" twist = " + twist); - System.out.println( - " warm up (" - + warmupCount - + " iterations): " - + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, warmupCount))); - System.out.println(" iterations per run: " + normalCount); - for (int i = 1; i <= 5; i++) { - System.out.println( - " run " - + i - + ": " - + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, normalCount))); - } - } - - private static void timeSuitePose3dLog(StringLogEntry discardEntry) { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); - final int warmupCount = 100_000; - final int normalCount = 1_000; - System.out.println("Pose3d.log():"); - System.out.println(" start = " + start); - System.out.println(" end = " + end); - System.out.println( - " warm up (" - + warmupCount - + " iterations): " - + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, warmupCount))); - System.out.println(" iterations per run: " + normalCount); - for (int i = 1; i <= 5; i++) { - System.out.println( - " run " - + i - + ": " - + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, normalCount))); - } - } - - /** Constructs a MyRobot instance. Also times Pose3d.exp() and Pose3d.log(). */ - public MyRobot() { - final var datalog = new DataLog(); - final var discardEntry = new StringLogEntry(datalog, "discard"); - timeSuitePose3dExp(discardEntry); - timeSuitePose3dLog(discardEntry); - } - /** * This function is run when the robot is first started up and should be used for any * initialization code. diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index c053dda8702..5977ce7d90a 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -65,7 +65,7 @@ model { // By default, a development executable will be generated. This is to help the case of // testing specific functionality of the library. "${nativeName}Dev"(NativeExecutableSpec) { - targetBuildTypes 'release' + targetBuildTypes 'debug' sources { cpp { source { diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index 09036ec4e7a..7e5d04d5a71 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -195,7 +195,7 @@ model { // By default, a development executable will be generated. This is to help the case of // testing specific functionality of the library. "${nativeName}Dev"(NativeExecutableSpec) { - targetBuildTypes 'release' + targetBuildTypes 'debug' sources { cpp { diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 0f744fb349d..c604dd16449 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -41,11 +41,6 @@ dependencies { api "com.fasterxml.jackson.core:jackson-annotations:2.12.4" api "com.fasterxml.jackson.core:jackson-core:2.12.4" api "com.fasterxml.jackson.core:jackson-databind:2.12.4" - devImplementation project(":wpiutil") -} - -configurations { - devImplementation.extendsFrom(implementation) } def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja") diff --git a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java index fbe02693ad7..c7b779e0073 100644 --- a/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java +++ b/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java @@ -4,105 +4,11 @@ package edu.wpi.first.math; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Twist3d; -import edu.wpi.first.util.datalog.DataLog; -import edu.wpi.first.util.datalog.StringLogEntry; - public final class DevMain { - private static double[] timePose3dExpMicros( - StringLogEntry discardEntry, Pose3d start, Twist3d twist, int count) { - double[] times = new double[count]; - for (int i = 0; i < count; i++) { - final long startTime = System.nanoTime(); - final var result = start.exp(twist); - final long endTime = System.nanoTime(); - times[i] = (endTime - startTime) / 1000.0; - discardEntry.append(result.toString()); - } - return times; - } - - private static double[] timePose3dLogMicros( - StringLogEntry discardEntry, Pose3d start, Pose3d end, int count) { - double[] times = new double[count]; - for (int i = 0; i < count; i++) { - final long startTime = System.nanoTime(); - final var result = start.log(end); - final long endTime = System.nanoTime(); - times[i] = (endTime - startTime) / 1000.0; - discardEntry.append(result.toString()); - } - return times; - } - - private static String summarizeMicros(double[] times) { - double total = 0; - for (double time : times) { - total += time; - } - double mean = total / times.length; - double sumsOfSquares = 0; - for (double time : times) { - sumsOfSquares += (time - mean) * (time - mean); - } - double stdDev = Math.sqrt(sumsOfSquares / times.length); - return "mean " + mean + "µs, std dev " + stdDev + "µs"; - } - - private static void timeSuitePose3dExp(StringLogEntry discardEntry) { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var twist = new Twist3d(0, 0, 0, Math.PI, 0, 0); - final int warmupCount = 100_000; - final int normalCount = 1_000; - System.out.println("Pose3d.exp():"); - System.out.println(" start = " + start); - System.out.println(" twist = " + twist); - System.out.println( - " warm up (" - + warmupCount - + " iterations): " - + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, warmupCount))); - System.out.println(" iterations per run: " + normalCount); - for (int i = 1; i <= 5; i++) { - System.out.println( - " run " - + i - + ": " - + summarizeMicros(timePose3dExpMicros(discardEntry, start, twist, normalCount))); - } - } - - private static void timeSuitePose3dLog(StringLogEntry discardEntry) { - final var start = new Pose3d(1, 0, 0, new Rotation3d(0, -Math.PI / 2, 0)); - final var end = new Pose3d(0, 1, 0, new Rotation3d(Math.PI / 2, 0, 0)); - final int warmupCount = 100_000; - final int normalCount = 1_000; - System.out.println("Pose3d.log():"); - System.out.println(" start = " + start); - System.out.println(" end = " + end); - System.out.println( - " warm up (" - + warmupCount - + " iterations): " - + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, warmupCount))); - System.out.println(" iterations per run: " + normalCount); - for (int i = 1; i <= 5; i++) { - System.out.println( - " run " - + i - + ": " - + summarizeMicros(timePose3dLogMicros(discardEntry, start, end, normalCount))); - } - } - /** Main entry point. */ public static void main(String[] args) { - final var datalog = new DataLog(); - final var discardEntry = new StringLogEntry(datalog, "discard"); - timeSuitePose3dExp(discardEntry); - timeSuitePose3dLog(discardEntry); + System.out.println("Hello World!"); + System.out.println(MathUtil.angleModulus(-5.0)); } private DevMain() {} From e318b9e912e358afc18000fc127eb6dc46ce7876 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Tue, 18 Jul 2023 15:21:59 -0700 Subject: [PATCH 21/21] Use MakeJDoubleArray --- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 30 +++++-------------- 1 file changed, 7 insertions(+), 23 deletions(-) diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index d2ef4c45c22..bfc07cc16fc 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -209,19 +209,11 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d frc::Pose3d result = pose.Exp(twist); - jdoubleArray resultArray = env->NewDoubleArray(7); - jdouble* nativeResultArray = - env->GetDoubleArrayElements(resultArray, nullptr); const auto& resultQuaternion = result.Rotation().GetQuaternion(); - nativeResultArray[0] = result.X().value(); - nativeResultArray[1] = result.Y().value(); - nativeResultArray[2] = result.Z().value(); - nativeResultArray[3] = resultQuaternion.W(); - nativeResultArray[4] = resultQuaternion.X(); - nativeResultArray[5] = resultQuaternion.Y(); - nativeResultArray[6] = resultQuaternion.Z(); - env->ReleaseDoubleArrayElements(resultArray, nativeResultArray, 0); - return resultArray; + return MakeJDoubleArray( + env, {{result.X().value(), result.Y().value(), result.Z().value(), + resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(), + resultQuaternion.Z()}}); } /* @@ -245,17 +237,9 @@ Java_edu_wpi_first_math_WPIMathJNI_logPose3d frc::Twist3d result = startPose.Log(endPose); - jdoubleArray resultArray = env->NewDoubleArray(6); - jdouble* nativeResultArray = - env->GetDoubleArrayElements(resultArray, nullptr); - nativeResultArray[0] = result.dx.value(); - nativeResultArray[1] = result.dy.value(); - nativeResultArray[2] = result.dz.value(); - nativeResultArray[3] = result.rx.value(); - nativeResultArray[4] = result.ry.value(); - nativeResultArray[5] = result.rz.value(); - env->ReleaseDoubleArrayElements(resultArray, nativeResultArray, 0); - return resultArray; + return MakeJDoubleArray( + env, {{result.dx.value(), result.dy.value(), result.dz.value(), + result.rx.value(), result.ry.value(), result.rz.value()}}); } /*