mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user