Skip to content

[wpimath] Implement Twist3d.exp() and Transform3d.log() with EJML #9284

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
calcmogul wants to merge 1 commit into
base: main
Choose a base branch
from
Open

[wpimath] Implement Twist3d.exp() and Transform3d.log() with EJML #9284

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions wpimath/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,6 @@ file(
src/main/native/cpp/jni/EigenJNI.cpp
src/main/native/cpp/jni/Exceptions.cpp
src/main/native/cpp/jni/LinearSystemUtilJNI.cpp
src/main/native/cpp/jni/Transform3dJNI.cpp
src/main/native/cpp/jni/Twist3dJNI.cpp
src/main/native/cpp/jni/autodiff/GradientJNI.cpp
src/main/native/cpp/jni/autodiff/HessianJNI.cpp
src/main/native/cpp/jni/autodiff/JacobianJNI.cpp
Expand Down
39 changes: 39 additions & 0 deletions wpimath/src/main/java/org/wpilib/math/geometry/GeometryUtil.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.wpilib.math.geometry;

import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.Vector;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.util.Nat;

/** Geometry utilities. */
final class GeometryUtil {
private GeometryUtil() {
throw new AssertionError("utility class");
}

/**
* Converts a rotation vector to a rotation matrix.
*
* @param rotation The rotation vector.
* @return The rotation matrix.
*/
static Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
return MatBuilder.fill(
Nat.N3(),
Nat.N3(),
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);
}
}
60 changes: 43 additions & 17 deletions wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
import java.util.Objects;
import org.wpilib.math.geometry.proto.Transform3dProto;
import org.wpilib.math.geometry.struct.Transform3dStruct;
import org.wpilib.math.jni.Transform3dJNI;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N4;
import org.wpilib.math.util.Nat;
import org.wpilib.units.measure.Distance;
Expand Down Expand Up @@ -264,23 +264,49 @@ public Rotation3d getRotation() {
* @return The twist that maps the current transform.
*/
public Twist3d log() {
var thisQuaternion = m_rotation.getQuaternion();
double[] resultArray =
Transform3dJNI.log(
this.getX(),
this.getY(),
this.getZ(),
thisQuaternion.getW(),
thisQuaternion.getX(),
thisQuaternion.getY(),
thisQuaternion.getZ());
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf

var u = VecBuilder.fill(m_translation.getX(), m_translation.getY(), m_translation.getZ());
var rvec = m_rotation.toVector();
var omega = GeometryUtil.rotationVectorToMatrix(rvec);
var omegaSq = omega.times(omega);
double theta = rvec.norm();
double 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/(2B))/θ²
double A = Math.sin(theta) / theta;
double B = (1 - Math.cos(theta)) / thetaSq;
C = (1 - A / (2 * B)) / thetaSq;
}

var V_inv = Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omegaSq.times(C));

var translation_component = V_inv.times(u);

return new Twist3d(
resultArray[0],
resultArray[1],
resultArray[2],
resultArray[3],
resultArray[4],
resultArray[5]);
translation_component.get(0, 0),
translation_component.get(1, 0),
translation_component.get(2, 0),
rvec.get(0, 0),
rvec.get(1, 0),
rvec.get(2, 0));
}

/**
Expand Down
56 changes: 49 additions & 7 deletions wpimath/src/main/java/org/wpilib/math/geometry/Twist3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,9 @@
import java.util.Objects;
import org.wpilib.math.geometry.proto.Twist3dProto;
import org.wpilib.math.geometry.struct.Twist3dStruct;
import org.wpilib.math.jni.Twist3dJNI;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.Nat;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;

Expand Down Expand Up @@ -74,13 +76,53 @@ public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz)
* @return The pose delta of the robot.
*/
public Transform3d exp() {
double[] resultArray = Twist3dJNI.exp(dx, dy, dz, rx, ry, rz);
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf

var u = VecBuilder.fill(dx, dy, dz);
var rvec = VecBuilder.fill(rx, ry, rz);
var omega = GeometryUtil.rotationVectorToMatrix(rvec);
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;
}

var R = Matrix.eye(Nat.N3()).plus(omega.times(A)).plus(omegaSq.times(B));
var V = Matrix.eye(Nat.N3()).plus(omega.times(B)).plus(omegaSq.times(C));

var translation_component = V.times(u);

return new Transform3d(
resultArray[0],
resultArray[1],
resultArray[2],
new Rotation3d(
new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
new Translation3d(
translation_component.get(0, 0),
translation_component.get(1, 0),
translation_component.get(2, 0)),
new Rotation3d(R));
}

@Override
Expand Down
34 changes: 0 additions & 34 deletions wpimath/src/main/java/org/wpilib/math/jni/Transform3dJNI.java
View file
Open in desktop

This file was deleted.

32 changes: 0 additions & 32 deletions wpimath/src/main/java/org/wpilib/math/jni/Twist3dJNI.java
View file
Open in desktop

This file was deleted.

42 changes: 0 additions & 42 deletions wpimath/src/main/native/cpp/jni/Transform3dJNI.cpp
View file
Open in desktop

This file was deleted.

42 changes: 0 additions & 42 deletions wpimath/src/main/native/cpp/jni/Twist3dJNI.cpp
View file
Open in desktop

This file was deleted.

16 changes: 0 additions & 16 deletions wpimath/src/test/java/org/wpilib/math/jni/Transform3dJNITest.java
View file
Open in desktop

This file was deleted.

16 changes: 0 additions & 16 deletions wpimath/src/test/java/org/wpilib/math/jni/Twist3dJNITest.java
View file
Open in desktop

This file was deleted.

Loading