Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix Pose3d exp()/log() and add rotation vector constructor to Rotation3d (#5072)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2023-02-09 00:31:03 -05:00
committed by GitHub
parent 37f065032f
commit 59be120982
7 changed files with 93 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -205,18 +205,25 @@ public class Pose3d implements Interpolatable<Pose3d> {
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.pow(2);
final var omegaSq = omega.times(omega);
double theta = rvec.norm();
double thetaSq = theta * theta;
double A;
double B;
double C;
if (theta < 1E-9) {
if (Math.abs(theta) < 1E-9) {
// 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;
@@ -257,16 +264,23 @@ public class Pose3d implements Interpolatable<Pose3d> {
final var rvec = transform.getRotation().getQuaternion().toRotationVector();
final var omega = rotationVectorToMatrix(rvec);
final var theta = transform.getRotation().getAngle();
final var theta = rvec.norm();
final var thetaSq = theta * theta;
double C;
if (theta < 1E-9) {
if (Math.abs(theta) < 1E-9) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
// 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(θ)) / θ²
@@ -276,7 +290,8 @@ public class Pose3d implements Interpolatable<Pose3d> {
C = (1 - A / (2 * B)) / thetaSq;
}
final var V_inv = Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omega.pow(2).times(C));
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()));

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -72,6 +72,17 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
cr * cp * sy - sr * sp * cy);
}
/**
* Constructs a Rotation3d with the given rotation vector representation. This representation is
* equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the
* axis in radians.
*
* @param rvec The rotation vector.
*/
public Rotation3d(Vector<N3> rvec) {
this(rvec, rvec.norm());
}
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
* normalized.