mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[wpimath] Pose3d: Switch to JNI for exp and log (#5444)
The pure Java implementations allocate a lot of temporary objects, and the JNI implementation is substantially more performant.
This commit is contained in:
Joseph Eng
@@ -76,6 +76,78 @@ public final class WPIMathJNI {
|
||||
*/
|
||||
public static native void pow(double[] src, int rows, double exponent, double[] dst);
|
||||
|
||||
/**
|
||||
* Obtain a Pose3d from a (constant curvature) velocity.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
|
||||
@@ -8,14 +8,8 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
|
||||
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 class Pose3d implements Interpolatable<Pose3d> {
|
||||
* @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<N3, N3> R = Matrix.eye(Nat.N3()).plus(omega.times(A)).plus(omegaSq.times(B));
|
||||
Matrix<N3, N3> V = Matrix.eye(Nat.N3()).plus(omega.times(B)).plus(omegaSq.times(C));
|
||||
Matrix<N3, N1> 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 class Pose3d implements Interpolatable<Pose3d> {
|
||||
* @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 class Pose3d implements Interpolatable<Pose3d> {
|
||||
return this.exp(scaledTwist);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Applies the hat operator to a rotation vector.
|
||||
*
|
||||
* <p>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<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
|
||||
// Given a rotation vector <a, b, c>,
|
||||
// [ 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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user