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

[wpimath] Fix Pose3d exponential and clean up Pose3d logarithm (#4970)

Browse Source 
Implementation based on this paper: https://ethaneade.org/lie.pdf
This commit is contained in:
Jordan McMichael
2023-01-18 23:38:03 -05:00
committed by GitHub
parent 5f1a025f27
commit 42c997a3c4
4 changed files with 255 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -10,6 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units;
import java.util.Arrays;
import org.junit.jupiter.api.Test;
class Pose3dTest {
@@ -49,10 +50,13 @@ class Pose3dTest {
Units.degreesToRadians(-45.0),
Units.degreesToRadians(0.0)));
// This sequence of rotations should diverge from the origin and eventually return to it. When
// each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and
// This sequence of rotations should diverge from the origin and eventually
// return to it. When
// each rotation occurs, it should be performed intrinsicly, i.e. 'from the
// viewpoint' of and
// with
// the axes of the pose that is being transformed, just like how the translation is done 'from
// the axes of the pose that is being transformed, just like how the translation
// is done 'from
// the
// viewpoint' of the pose that is being transformed.
var finalPose =
@@ -153,4 +157,77 @@ class Pose3dTest {
assertEquals(expected, pose.toPose2d());
}
@Test
void testComplexTwists() {
var initial_poses =
Arrays.asList(
new Pose3d(
new Translation3d(0.698303, -0.959096, 0.271076),
new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
new Pose3d(
new Translation3d(0.634892, -0.765209, 0.117543),
new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))),
new Pose3d(
new Translation3d(0.584827, -0.590303, -0.02557),
new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))),
new Pose3d(
new Translation3d(0.505038, -0.451479, -0.112835),
new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))),
new Pose3d(
new Translation3d(0.428178, -0.329692, -0.189707),
new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
var final_poses =
Arrays.asList(
new Pose3d(
new Translation3d(-0.230448, -0.511957, 0.198406),
new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
new Pose3d(
new Translation3d(-0.088932, -0.343253, 0.095018),
new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))),
new Pose3d(
new Translation3d(-0.107908, -0.317552, 0.133946),
new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))),
new Pose3d(
new Translation3d(-0.123383, -0.156411, -0.047435),
new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))),
new Pose3d(
new Translation3d(-0.084654, -0.019305, -0.030022),
new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873))));
final var eps = 1E-5;
for (int i = 0; i < initial_poses.size(); i++) {
var start = initial_poses.get(i);
var end = final_poses.get(i);
var twist = start.log(end);
var start_exp = start.exp(twist);
assertAll(
() -> assertEquals(start_exp.getX(), end.getX(), eps),
() -> assertEquals(start_exp.getY(), end.getY(), eps),
() -> assertEquals(start_exp.getZ(), end.getZ(), eps),
() ->
assertEquals(
start_exp.getRotation().getQuaternion().getW(),
end.getRotation().getQuaternion().getW(),
eps),
() ->
assertEquals(
start_exp.getRotation().getQuaternion().getX(),
end.getRotation().getQuaternion().getX(),
eps),
() ->
assertEquals(
start_exp.getRotation().getQuaternion().getY(),
end.getRotation().getQuaternion().getY(),
eps),
() ->
assertEquals(
start_exp.getRotation().getQuaternion().getZ(),
end.getRotation().getQuaternion().getZ(),
eps));
}
}
}