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..f843fde7b9d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -76,6 +76,78 @@ 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.
+ *
+ * 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.
+ * @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 as a double array.
+ */
+ public static native double[] 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 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.
+ * @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 as a double array.
+ */
+ public static native double[] 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 9e46ddb064e..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
@@ -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,28 @@ 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);
+ var quaternion = this.getRotation().getQuaternion();
+ 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])));
}
/**
@@ -258,51 +227,31 @@ 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()));
-
+ var thisQuaternion = this.getRotation().getQuaternion();
+ var endQuaternion = end.getRotation().getQuaternion();
+ 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(
- 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));
+ resultArray[0],
+ resultArray[1],
+ resultArray[2],
+ resultArray[3],
+ resultArray[4],
+ resultArray[5]);
}
/**
@@ -353,31 +302,4 @@ public Pose3d interpolate(Pose3d endValue, double t) {
return this.exp(scaledTwist);
}
}
-
- /**
- * Applies the hat operator to a rotation vector.
- *
- * It takes a rotation vector and returns the corresponding matrix representation of the Lie
- * algebra element (a 3x3 rotation matrix).
- *
- * @param rotation The rotation vector.
- * @return The rotation vector as a 3x3 rotation matrix.
- */
- 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);
- }
}
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index 4cdcf706f62..bfc07cc16fc 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"
@@ -187,6 +188,60 @@ Java_edu_wpi_first_math_WPIMathJNI_pow
env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
}
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: expPose3d
+ * Signature: (DDDDDDDDDDDDD)[D
+ */
+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,
+ jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
+ jdouble twistRy, jdouble twistRz)
+{
+ 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 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 = pose.Exp(twist);
+
+ const auto& resultQuaternion = result.Rotation().GetQuaternion();
+ return MakeJDoubleArray(
+ env, {{result.X().value(), result.Y().value(), result.Z().value(),
+ resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
+ resultQuaternion.Z()}});
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: logPose3d
+ * Signature: (DDDDDDDDDDDDDD)[D
+ */
+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,
+ jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,
+ jdouble endQy, jdouble endQz)
+{
+ 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 endPose{
+ units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
+ frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
+
+ frc::Twist3d result = startPose.Log(endPose);
+
+ return MakeJDoubleArray(
+ env, {{result.dx.value(), result.dy.value(), result.dz.value(),
+ result.rx.value(), result.ry.value(), result.rz.value()}});
+}
+
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: isStabilizable