mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-04 03:11:43 +00:00
[wpimath] Improve Euler angle calculations in gimbal lock (#5437)
This commit is contained in:
Joseph Eng
@@ -86,3 +86,264 @@ For q ≠ 0 and r = 0,
|
||||
k = q / q
|
||||
k = 1
|
||||
```
|
||||
|
||||
## Quaternion to Euler angle conversion
|
||||
|
||||
### Conventions
|
||||
|
||||
We'll use the extrinsic X-Y-Z rotation order for Euler angles. The direction of rotation is CCW looking into the positive axis. If you point your right thumb along the positive axis direction, your fingers curl in the direction of rotation.
|
||||
|
||||
The angles are `a_x` around the X-axis, `a_y` around the Y-axis, and `a_z` around the Z-axis, with the following constraints:
|
||||
|
||||
```
|
||||
-π ≤ a_x ≤ π
|
||||
-π/2 ≤ a_y ≤ π/2
|
||||
-π ≤ a_z ≤ π
|
||||
```
|
||||
|
||||
The coordinate system is right-handed. If you point your right thumb along the +Z axis, your fingers curl from the +X axis to the +Y axis.
|
||||
|
||||
The quaternion imaginary numbers are defined as follows:
|
||||
|
||||
```
|
||||
îĵ = k̂
|
||||
ĵk̂ = î
|
||||
k̂î = ĵ
|
||||
îĵ = -k̂
|
||||
k̂ĵ = -î
|
||||
îk̂ = -ĵ
|
||||
î² = ĵ² = k̂² = -1
|
||||
```
|
||||
|
||||
### Quaternion representation of axis rotations
|
||||
|
||||
We will take it as given that a rotation by `θ` radians around a normalized vector `v` is represented with the quaternion `cos(θ/2) + sin(θ/2) (v_x î + v_y ĵ + v_z k̂)`.
|
||||
|
||||
### Derivation
|
||||
|
||||
For convenience, we'll define the following variables:
|
||||
|
||||
```
|
||||
c_x = cos(a_x/2)
|
||||
s_x = sin(a_x/2)
|
||||
c_y = cos(a_y/2)
|
||||
s_y = sin(a_y/2)
|
||||
c_z = cos(a_z/2)
|
||||
s_z = sin(a_z/2)
|
||||
```
|
||||
|
||||
We can calculate the quaternion corresponding to a set of Euler angles by applying each rotation in sequence. Recall that quaternions are composed with left multiplication, like matrices.
|
||||
|
||||
```
|
||||
q = (cos(a_z/2) + sin(a_z/2) k̂)(cos(a_y/2) + sin(a_y/2) ĵ)(cos(a_x/2) + sin(a_x/2) î)
|
||||
q = (c_z + s_z k̂)(c_y + s_y ĵ)(c_x + s_x î)
|
||||
q = (c_y c_z - s_y s_z î + s_y c_z ĵ + c_y s_z k̂)(c_x + s_x î)
|
||||
= (c_x c_y c_z + s_x s_y s_z)
|
||||
+ (s_x c_y c_z - c_x s_y s_z) î
|
||||
+ (s_x c_y s_z + c_x s_y c_z) ĵ
|
||||
+ (c_x c_y s_z - s_x s_y c_z) k̂
|
||||
```
|
||||
|
||||
Letting `q = q_w + q_x î + q_y ĵ + q_z k̂`, we can extract the components of the quaternion:
|
||||
|
||||
```
|
||||
q_w = c_x c_y c_z + s_x s_y s_z
|
||||
q_x = s_x c_y c_z - c_x s_y s_z
|
||||
q_y = c_x s_y c_z + s_x c_y s_z
|
||||
q_z = c_x c_y s_z - s_x s_y c_z
|
||||
```
|
||||
|
||||
### Solving for `a_y`
|
||||
|
||||
Solving for `sin(a_y)`:
|
||||
|
||||
```
|
||||
sin(a_y) = 2 c_y s_y
|
||||
sin(a_y) = 2 (c_x² c_y s_y + s_x² c_y s_y)
|
||||
sin(a_y) = 2 (c_x² c_y s_y c_z² + c_x² c_y s_y s_z²
|
||||
+ s_x² c_y s_y c_z² + s_x² c_y s_y s_z²)
|
||||
sin(a_y) = 2 (c_x² c_y s_y c_z² + s_x² c_y s_y s_z²
|
||||
+ s_x² c_y s_y c_z² + c_x² c_y s_y s_z²)
|
||||
sin(a_y) = 2 (c_x² c_y s_y c_z² + c_x s_x c_y² c_z s_z
|
||||
+ c_x s_x s_y² c_z s_z + s_x² c_y s_y s_z²
|
||||
- c_x s_x c_y² c_z s_z + s_x² c_y s_y c_z²
|
||||
+ c_x² c_y s_y s_z² - c_x s_x s_y² c_z s_z)
|
||||
sin(a_y) = 2 ((c_x c_y c_z + s_x s_y s_z)(c_x s_y c_z + s_x c_y s_z)
|
||||
- (s_x c_y c_z - c_x s_y s_z)(c_x c_y s_z - s_x s_y c_z))
|
||||
sin(a_y) = 2 (q_w q_y - q_x q_z)
|
||||
```
|
||||
|
||||
Then solving for `a_y`:
|
||||
|
||||
```
|
||||
a_y = sin⁻¹(sin(a_y))
|
||||
a_y = sin⁻¹(2 (q_w q_y - q_x q_z))
|
||||
```
|
||||
|
||||
### Solving for `a_x` and `a_z`
|
||||
|
||||
Solving for `cos(a_x) cos(a_y)`:
|
||||
|
||||
```
|
||||
cos(a_x) cos(a_y) = (cos²(a_x/2) - sin²(a_x/2))(cos²(a_y/2) - sin²(a_y/2))
|
||||
cos(a_x) cos(a_y) = (c_x² - s_x²)(c_y² - s_y²)
|
||||
cos(a_x) cos(a_y) = c_x² c_y² - c_x² s_y² - s_x² c_y² + s_x² s_y²
|
||||
cos(a_x) cos(a_y) = c_x² (1 - s_y²) - c_x² s_y² - s_x² c_y² + s_x² (1 - c_y²)
|
||||
cos(a_x) cos(a_y) = c_x² - c_x² s_y² - c_x² s_y² - s_x² c_y² + s_x² - s_x² c_y²
|
||||
cos(a_x) cos(a_y) = c_x² + s_x² - 2 (c_x² s_y² + s_x² c_y²)
|
||||
cos(a_x) cos(a_y) = 1 - 2 (c_x² s_y² + s_x² c_y²)
|
||||
cos(a_x) cos(a_y) = 1 - 2 (c_x² s_y² c_z² + c_x² s_y² s_z²
|
||||
+ s_x² c_y² c_z² + s_x² c_y² s_z²)
|
||||
cos(a_x) cos(a_y) = 1 - 2 (s_x² c_y² c_z² + c_x² s_y² s_z²
|
||||
+ c_x² s_y² c_z² + s_x² c_y² s_z²)
|
||||
cos(a_x) cos(a_y) = 1 - 2 (s_x² c_y² c_z² - 2 c_x s_x c_y s_y c_z s_z + s_x² s_y² s_z²
|
||||
+ c_x² s_y² c_z² + 2 c_x s_x c_y s_y c_z s_z + s_x² c_y² s_z²)
|
||||
cos(a_x) cos(a_y) = 1 - 2 ((s_x c_y c_z - s_x s_y s_z)² + (c_x s_y c_z + s_x c_y s_z)²)
|
||||
cos(a_x) cos(a_y) = 1 - 2 (q_x² + q_y²)
|
||||
```
|
||||
|
||||
Solving for `sin(a_x) cos(a_y)`:
|
||||
|
||||
```
|
||||
sin(a_x) cos(a_y) = (2 cos(a_x/2) sin(a_x/2))(cos²(a_y/2) - sin²(a_y/2))
|
||||
sin(a_x) cos(a_y) = (2 c_x s_x)(c_y² - s_y²)
|
||||
sin(a_x) cos(a_y) = 2 (c_x s_x c_y² - c_x s_x s_y²)
|
||||
sin(a_x) cos(a_y) = 2 (c_x s_x c_y² c_z² + c_x s_x c_y² s_z²
|
||||
- c_x s_x s_y² c_z² - c_x s_x s_y² s_z²)
|
||||
sin(a_x) cos(a_y) = 2 (c_x s_x c_y² c_z² - c_x² c_y s_y c_z s_z
|
||||
+ s_x² c_y s_y c_z s_z - c_x s_x s_y² s_z²
|
||||
+ c_x² c_y s_y c_z s_z - c_x s_x s_y² c_z²
|
||||
+ c_x s_x c_y² s_z² - s_x² c_y s_y c_z s_z)
|
||||
sin(a_x) cos(a_y) = 2 ((c_x c_y c_z + s_x s_y s_z)(s_x c_y c_z - c_x s_y s_z)
|
||||
+ (c_x s_y c_z + s_x c_y s_z)(c_x c_y s_z - s_x s_y c_z))
|
||||
sin(a_x) cos(a_y) = 2 (q_w q_x + q_y q_z)
|
||||
```
|
||||
|
||||
Similarly, solving for `cos(a_z) cos(a_y)`:
|
||||
|
||||
```
|
||||
cos(a_z) cos(a_y) = (cos²(a_z/2) - sin²(a_z/2))(cos²(a_y/2) - sin²(a_y/2))
|
||||
cos(a_z) cos(a_y) = (c_z² - s_z²)(c_y² - s_y²)
|
||||
cos(a_z) cos(a_y) = c_y² c_z² - s_y² c_z² - c_y² s_z² + s_y² s_z²
|
||||
cos(a_z) cos(a_y) = c_y² (1 - s_z²) - s_y² c_z² - c_y² s_z² + s_y² (1 - c_z²)
|
||||
cos(a_z) cos(a_y) = c_y² - c_y² s_z² - s_y² c_z² - c_y² s_z² + s_y² - s_y² c_z²
|
||||
cos(a_z) cos(a_y) = c_y² + s_y² - 2 (c_y² s_z² + s_y² c_z²)
|
||||
cos(a_z) cos(a_y) = 1 - 2 (c_y² s_z² + s_y² c_z²)
|
||||
cos(a_z) cos(a_y) = 1 - 2 (c_x² c_y² s_z² + s_x² c_y² s_z²
|
||||
+ c_x² s_y² c_z² + s_x² s_y² c_z²)
|
||||
cos(a_z) cos(a_y) = 1 - 2 (c_x² s_y² c_z² + s_x² c_y² s_z²
|
||||
+ c_x² c_y² s_z² + s_x² s_y² c_z²)
|
||||
cos(a_z) cos(a_y) = 1 - 2 (c_x² s_y² c_z² + 2 c_x s_x c_y s_y c_z s_z + s_x² c_y² s_z²
|
||||
+ c_x² c_y² s_z² - 2 c_x s_x c_y s_y c_z s_z + s_x² s_y² c_z²)
|
||||
cos(a_z) cos(a_y) = 1 - 2 ((c_x s_y c_z + s_x c_y s_z)² + (c_x c_y s_z - s_x s_y c_z)²)
|
||||
cos(a_z) cos(a_y) = 1 - 2 (q_y² + q_z²)
|
||||
```
|
||||
|
||||
Similarly, solving for `sin(a_z) cos(a_y)`:
|
||||
|
||||
```
|
||||
sin(a_z) cos(a_y) = (2 cos(a_z/2) sin(a_z/2))(cos²(a_y/2) - sin²(a_y/2))
|
||||
sin(a_z) cos(a_y) = (2 c_z s_z)(c_y² - s_y²)
|
||||
sin(a_z) cos(a_y) = 2 (c_y² c_z s_z - s_y² c_z s_z)
|
||||
sin(a_z) cos(a_y) = 2 (c_x² c_y² c_z s_z + s_x² c_y² c_z s_z
|
||||
- c_x² s_y² c_z s_z - s_x² s_y² c_z s_z)
|
||||
sin(a_z) cos(a_y) = 2 (c_x² c_y² c_z s_z - c_x s_x c_y s_y c_z²
|
||||
+ c_x s_x c_y s_y s_z² - s_x² s_y² c_z s_z
|
||||
+ c_x s_x c_y s_y c_z² + s_x² c_y² c_z s_z
|
||||
- c_x² s_y² c_z s_z - c_x s_x c_y s_y s_z²)
|
||||
sin(a_z) cos(a_y) = 2 ((c_x c_y c_z + s_x s_y s_z)(c_x c_y s_z - s_x s_y c_z)
|
||||
+ (s_x c_y c_z - c_x s_y s_z)(c_x s_y c_z + s_x c_y s_z))
|
||||
sin(a_z) cos(a_y) = 2 (q_w q_z + q_x q_y)
|
||||
```
|
||||
|
||||
Solving for `a_x` and `a_z`:
|
||||
|
||||
```
|
||||
a_x = atan2(sin(a_x), cos(a_x))
|
||||
a_z = atan2(sin(a_z), cos(a_z))
|
||||
```
|
||||
|
||||
If `cos(a_y) > 0`:
|
||||
|
||||
```
|
||||
a_x = atan2(sin(a_x) cos(a_y), cos(a_x) cos(a_y))
|
||||
a_z = atan2(sin(a_z) cos(a_y), cos(a_z) cos(a_y))
|
||||
a_x = atan2(2 (q_w q_x + q_y q_z), 1 - 2 (q_x² + q_y²))
|
||||
a_z = atan2(2 (q_w q_z + q_x q_y), 1 - 2 (q_y² + q_z²))
|
||||
```
|
||||
|
||||
Because `-π/2 ≤ a_y ≤ π/2`, `cos(a_y) ≥ 0`. Therefore, the only remaining case is `cos(a_y) = 0`, whose only solutions in that range are `a_y = ±π/2`.
|
||||
|
||||
```
|
||||
a_y = ±π/2
|
||||
a_y/2 = ±π/4
|
||||
cos(a_y/2) = √2/2
|
||||
c_y = √2/2
|
||||
sin(a_y/2) = ±√2/2
|
||||
s_y = ±√2/2
|
||||
```
|
||||
|
||||
Plugging into the quaternion components:
|
||||
|
||||
```
|
||||
q_w = c_x c_y c_z + s_x s_y s_z
|
||||
q_x = s_x c_y c_z - c_x s_y s_z
|
||||
q_y = c_x s_y c_z + s_x c_y s_z
|
||||
q_z = c_x c_y s_z - s_x s_y c_z
|
||||
q_w = √2/2 c_x c_z ± √2/2 s_x s_z
|
||||
q_x = √2/2 s_x c_z ∓ √2/2 c_x s_z
|
||||
q_y = ±√2/2 c_x c_z + √2/2 s_x s_z
|
||||
q_z = √2/2 c_x s_z ∓ √2/2 s_x c_z
|
||||
q_w = √2/2 (c_x c_z ± s_x s_z)
|
||||
q_x = √2/2 (s_x c_z ∓ c_x s_z)
|
||||
q_y = √2/2 (± c_x c_z + s_x s_z)
|
||||
q_z = √2/2 (c_x s_z ∓ s_x c_z)
|
||||
q_w = √2/2 cos(a_z/2 ∓ a_x/2)
|
||||
q_x = √2/2 sin(a_x/2 ∓ a_z/2)
|
||||
q_y = √2/2 -cos(a_x/2 ∓ a_z/2)
|
||||
q_z = √2/2 sin(a_z/2 ∓ a_x/2)
|
||||
```
|
||||
|
||||
In either case only the sum or the difference between `a_x` and `a_z` can be determined. We'll pick the solution where `a_x = 0`.
|
||||
|
||||
```
|
||||
q_w = √2/2 cos(a_z/2 ∓ 0)
|
||||
q_w = √2/2 cos(a_z/2)
|
||||
cos(a_z/2) = √2 q_w
|
||||
q_z = √2/2 sin(a_z/2 ∓ 0)
|
||||
q_z = √2/2 sin(a_z/2)
|
||||
sin(a_z/2) = √2 q_z
|
||||
cos(a_z) = cos²(a_z/2) - sin²(a_z/2)
|
||||
cos(a_z) = (√2 q_w)² - (√2 q_z)²
|
||||
cos(a_z) = 2 q_w² - 2 q_z²
|
||||
cos(a_z) = 2 (q_w² - q_z²)
|
||||
sin(a_z) = 2 cos(a_z/2) sin(a_z/2)
|
||||
sin(a_z) = 2 (√2 q_w) (√2 q_z)
|
||||
sin(a_z) = 4 q_w q_z
|
||||
a_z = atan2(4 q_w q_z, 2 (q_w² - q_z²))
|
||||
a_z = atan2(2 q_w q_z, q_w² - q_z²)
|
||||
```
|
||||
|
||||
### Determining if `cos(a_y) ≈ 0`
|
||||
|
||||
When calculating `a_x`:
|
||||
|
||||
```
|
||||
cos(a_y) ≈ 0
|
||||
cos²(a_y) ≈ 0
|
||||
cos²(a_x) cos²(a_y) + sin²(a_x) cos²(a_y) ≈ 0
|
||||
(cos(a_x) cos(a_y))² + (sin(a_x) cos(a_y))² ≈ 0
|
||||
```
|
||||
|
||||
Note that this reuses the `cos(a_x) cos(a_y)` and `sin(a_x) cos(a_y)` terms needed to calculate `a_x`.
|
||||
|
||||
When calculating `a_z`:
|
||||
|
||||
```
|
||||
cos(a_y) ≈ 0
|
||||
cos²(a_y) ≈ 0
|
||||
cos²(a_y) cos²(a_z) + cos²(a_y) sin²(a_z) ≈ 0
|
||||
(cos(a_y) cos(a_z))² + (cos(a_y) sin(a_z))² ≈ 0
|
||||
```
|
||||
|
||||
Note that this reuses the `cos(a_y) cos(a_z)` and `cos(a_y) sin(a_z)` terms needed to calculate `a_z`.
|
||||
|
||||
@@ -308,8 +308,15 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
|
||||
final var y = m_q.getY();
|
||||
final var z = m_q.getZ();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
|
||||
return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
|
||||
// wpimath/algorithms.md
|
||||
final var cxcy = 1.0 - 2.0 * (x * x + y * y);
|
||||
final var sxcy = 2.0 * (w * x + y * z);
|
||||
final var cy_sq = cxcy * cxcy + sxcy * sxcy;
|
||||
if (cy_sq > 1e-20) {
|
||||
return Math.atan2(sxcy, cxcy);
|
||||
} else {
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -343,8 +350,15 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
|
||||
final var y = m_q.getY();
|
||||
final var z = m_q.getZ();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
|
||||
return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
|
||||
// wpimath/algorithms.md
|
||||
final var cycz = 1.0 - 2.0 * (y * y + z * z);
|
||||
final var cysz = 2.0 * (w * z + x * y);
|
||||
final var cy_sq = cycz * cycz + cysz * cysz;
|
||||
if (cy_sq > 1e-20) {
|
||||
return Math.atan2(cysz, cycz);
|
||||
} else {
|
||||
return Math.atan2(2.0 * w * z, w * w - z * z);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -187,9 +187,15 @@ units::radian_t Rotation3d::X() const {
|
||||
double y = m_q.Y();
|
||||
double z = m_q.Z();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
|
||||
return units::radian_t{
|
||||
std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
|
||||
// wpimath/algorithms.md
|
||||
double cxcy = 1.0 - 2.0 * (x * x + y * y);
|
||||
double sxcy = 2.0 * (w * x + y * z);
|
||||
double cy_sq = cxcy * cxcy + sxcy * sxcy;
|
||||
if (cy_sq > 1e-20) {
|
||||
return units::radian_t{std::atan2(sxcy, cxcy)};
|
||||
} else {
|
||||
return 0_rad;
|
||||
}
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::Y() const {
|
||||
@@ -213,9 +219,15 @@ units::radian_t Rotation3d::Z() const {
|
||||
double y = m_q.Y();
|
||||
double z = m_q.Z();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
|
||||
return units::radian_t{
|
||||
std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
|
||||
// wpimath/algorithms.md
|
||||
double cycz = 1.0 - 2.0 * (y * y + z * z);
|
||||
double cysz = 2.0 * (w * z + x * y);
|
||||
double cy_sq = cycz * cycz + cysz * cysz;
|
||||
if (cy_sq > 1e-20) {
|
||||
return units::radian_t{std::atan2(cysz, cycz)};
|
||||
} else {
|
||||
return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)};
|
||||
}
|
||||
}
|
||||
|
||||
Vectord<3> Rotation3d::Axis() const {
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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}};
|
||||
|
||||
Reference in New Issue
Block a user