Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31: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));
}
}
}

52
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -4,6 +4,8 @@
#include <cmath>
#include <wpi/array.h>
#include "frc/geometry/Pose3d.h"
#include "gtest/gtest.h"
@@ -95,3 +97,53 @@ TEST(Pose3dTest, ToPose2d) {
EXPECT_EQ(expected, pose.ToPose2d());
}
TEST(Pose3dTest, ComplexTwists) {
wpi::array<Pose3d, 5> initial_poses{
Pose3d{0.698303_m, -0.959096_m, 0.271076_m,
Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}},
Pose3d{0.634892_m, -0.765209_m, 0.117543_m,
Rotation3d{Quaternion{0.84987, -0.070829, 0.162097, 0.496415}}},
Pose3d{0.584827_m, -0.590303_m, -0.02557_m,
Rotation3d{Quaternion{0.832743, -0.041991, 0.202188, 0.513708}}},
Pose3d{0.505038_m, -0.451479_m, -0.112835_m,
Rotation3d{Quaternion{0.816515, -0.002673, 0.226182, 0.531166}}},
Pose3d{0.428178_m, -0.329692_m, -0.189707_m,
Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}},
};
wpi::array<Pose3d, 5> final_poses{
Pose3d{-0.230448_m, -0.511957_m, 0.198406_m,
Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}},
Pose3d{-0.088932_m, -0.343253_m, 0.095018_m,
Rotation3d{Quaternion{0.638738, 0.413016, 0.536281, 0.365833}}},
Pose3d{-0.107908_m, -0.317552_m, 0.133946_m,
Rotation3d{Quaternion{0.653444, 0.417069, 0.465505, 0.427046}}},
Pose3d{-0.123383_m, -0.156411_m, -0.047435_m,
Rotation3d{Quaternion{0.652983, 0.40644, 0.431566, 0.47135}}},
Pose3d{-0.084654_m, -0.019305_m, -0.030022_m,
Rotation3d{Quaternion{0.620243, 0.429104, 0.479384, 0.44873}}},
};
for (size_t i = 0; i < initial_poses.size(); i++) {
auto start = initial_poses[i];
auto end = final_poses[i];
auto twist = start.Log(end);
auto start_exp = start.Exp(twist);
auto eps = 1E-5;
EXPECT_NEAR(start_exp.X().value(), end.X().value(), eps);
EXPECT_NEAR(start_exp.Y().value(), end.Y().value(), eps);
EXPECT_NEAR(start_exp.Z().value(), end.Z().value(), eps);
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().W(),
end.Rotation().GetQuaternion().W(), eps);
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().X(),
end.Rotation().GetQuaternion().X(), eps);
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Y(),
end.Rotation().GetQuaternion().Y(), eps);
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Z(),
end.Rotation().GetQuaternion().Z(), eps);
}
}