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 transformBy rotation type (#4545)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Griffin Della Grotte
2022-11-07 09:57:33 -08:00
committed by GitHub
parent a4054d702f
commit b1b4c1e9e7
6 changed files with 82 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,6 +15,61 @@ import org.junit.jupiter.api.Test;
class Pose3dTest {
private static final double kEpsilon = 1E-9;
@Test
void testTransformByRotations() {
var initialPose =
new Pose3d(
new Translation3d(0.0, 0.0, 0.0),
new Rotation3d(
Units.degreesToRadians(0.0),
Units.degreesToRadians(0.0),
Units.degreesToRadians(0.0)));
var transform1 =
new Transform3d(
new Translation3d(0.0, 0.0, 0.0),
new Rotation3d(
Units.degreesToRadians(90.0),
Units.degreesToRadians(45.0),
Units.degreesToRadians(0.0)));
var transform2 =
new Transform3d(
new Translation3d(0.0, 0.0, 0.0),
new Rotation3d(
Units.degreesToRadians(-90.0),
Units.degreesToRadians(0.0),
Units.degreesToRadians(0.0)));
var transform3 =
new Transform3d(
new Translation3d(0.0, 0.0, 0.0),
new Rotation3d(
Units.degreesToRadians(0.0),
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
// with
// 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 =
initialPose.transformBy(transform1).transformBy(transform2).transformBy(transform3);
assertAll(
() ->
assertEquals(
finalPose.getRotation().getX(), initialPose.getRotation().getX(), kEpsilon),
() ->
assertEquals(
finalPose.getRotation().getY(), initialPose.getRotation().getY(), kEpsilon),
() ->
assertEquals(
finalPose.getRotation().getZ(), initialPose.getRotation().getZ(), kEpsilon));
}
@Test
void testTransformBy() {
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);