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] Improve Euler angle calculations in gimbal lock (#5437)

Browse Source
This commit is contained in:
Joseph Eng
2023-07-17 17:19:42 -07:00
committed by GitHub
parent daf022d3da
commit 593767c8c7
5 changed files with 365 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
View File

@@ -19,6 +19,39 @@ import org.junit.jupiter.api.Test;
class Rotation3dTest {
private static final double kEpsilon = 1E-9;
@Test
void testGimbalLockAccuracy() {
var rot1 = new Rotation3d(0, 0, Math.PI / 2);
var rot2 = new Rotation3d(Math.PI, 0, 0);
var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
final var result1 = rot1.plus(rot2).plus(rot3);
final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
assertAll(
() -> assertEquals(expected1, result1),
() -> assertEquals(Math.PI / 2, result1.getX() + result1.getZ(), kEpsilon),
() -> assertEquals(-Math.PI / 2, result1.getY(), kEpsilon));
rot1 = new Rotation3d(0, 0, Math.PI / 2);
rot2 = new Rotation3d(-Math.PI, 0, 0);
rot3 = new Rotation3d(Math.PI / 2, 0, 0);
final var result2 = rot1.plus(rot2).plus(rot3);
final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
assertAll(
() -> assertEquals(expected2, result2),
() -> assertEquals(Math.PI / 2, result2.getZ() - result2.getX(), kEpsilon),
() -> assertEquals(Math.PI / 2, result2.getY(), kEpsilon));
rot1 = new Rotation3d(0, 0, Math.PI / 2);
rot2 = new Rotation3d(0, Math.PI / 3, 0);
rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
final var result3 = rot1.plus(rot2).plus(rot3);
final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
assertAll(
() -> assertEquals(expected3, result3),
() -> assertEquals(Math.PI / 6, result3.getZ() - result3.getX(), kEpsilon),
() -> assertEquals(Math.PI / 2, result3.getY(), kEpsilon));
}
@Test
void testInitAxisAngleAndRollPitchYaw() {
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);