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] 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);

35
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -13,6 +13,41 @@
using namespace frc;
TEST(Rotation3dTest, GimbalLockAccuracy) {
auto rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
auto rot2 = Rotation3d{units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
auto rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result1 = rot1 + rot2 + rot3;
const auto expected1 =
Rotation3d{0_rad, -units::radian_t{std::numbers::pi / 2},
units::radian_t{std::numbers::pi / 2}};
EXPECT_EQ(expected1, result1);
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result1.X() + result1.Z()).value());
EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, result1.Y().value());
rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
rot3 = Rotation3d{units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result2 = rot1 + rot2 + rot3;
const auto expected2 =
Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
units::radian_t{std::numbers::pi / 2}};
EXPECT_EQ(expected2, result2);
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result2.Z() - result2.X()).value());
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result2.Y().value());
rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result3 = rot1 + rot2 + rot3;
const auto expected3 =
Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
units::radian_t{std::numbers::pi / 6}};
EXPECT_EQ(expected3, result3);
EXPECT_DOUBLE_EQ(std::numbers::pi / 6, (result3.Z() - result3.X()).value());
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result3.Y().value());
}
TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};