forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
roll_pitch_yaw.cc
236 lines (217 loc) · 11 KB
/
roll_pitch_yaw.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
#include "drake/math/roll_pitch_yaw.h"
#include <string>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include "drake/math/rotation_matrix.h"
namespace drake {
namespace math {
template <typename T>
RotationMatrix<T> RollPitchYaw<T>::ToRotationMatrix() const {
return RotationMatrix<T>(*this);
}
// Uses a quaternion and its associated rotation matrix `R` to accurately
// and efficiently calculate the roll-pitch-yaw angles (SpaceXYZ Euler angles)
// that underlie `this` @RollPitchYaw, even when the pitch angle p is very
// near a singularity (e.g., when p is within 1E-6 of π/2 or -π/2).
// @param[in] quaternion unit quaternion with elements `[e0, e1, e2, e3]`.
// @param[in] R The %RotationMatrix corresponding to `quaternion`.
// @return [r, p, y] with `-π <= r <= π`, `-π/2 <= p <= π/2, `-π <= y <= π`.
// @note The caller of this function is responsible for ensuring `quaternion`
// satisfies `e0^2 + e1^2 + e2^2 + e3^2 = 1` and that the matrix `R` is the
// rotation matrix that corresponds to `quaternion'.
//-----------------------------------------------------------------------------
// <h3>Theory</h3>
//
// This algorithm was created October 2016 by Paul Mitiguy for TRI (Toyota).
// We believe this is a new algorithm (not previously published).
// Some theory/formulation of this algorithm is provided below. More detail
// is in Chapter 6 Rotation Matrices II [Mitiguy 2017] (reference below).
// <pre>
// Notation: Angles q1, q2, q3 designate SpaceXYZ "roll, pitch, yaw" angles.
// A quaternion can be defined in terms of an angle-axis rotation by
// an angle `theta` about a unit vector `lambda`. For example,
// consider right-handed orthogonal unit vectors Ax, Ay, Az and
// Bx, By, Bz fixed in a frame A and a rigid body B, respectively.
// Initially, Bx = Ax, By = Ay, Bz = Az, then B is subjected to a
// right-handed rotation relative to frame A by an angle `theta`
// about `lambda = L1*Ax + L2*Ay + L3*Az = L1*Bx + L2*By + L3*Bz`.
// The elements of `quaternion` are defined e0, e1, e2, e3 as
// `e0 = cos(theta/2)`, `e1 = L1*sin(theta/2)`,
// `e2 = L2*sin(theta/2)`, `e3 = L3*sin(theta/2)`.
//
// Step 1. The 3x3 rotation matrix R is only used (in conjunction with the
// atan2 function) to accurately calculate the pitch angle q2.
// Note: Since only 5 elements of R are used, the algorithm could be
// made slightly more efficient by computing/passing only those 5
// elements (e.g., not calculating the other 4 elements) and/or
// manipulating the relationship between `R` and quaternion to
// further reduce calculations.
//
// Step 2. Realize the quaternion passed to the function can be regarded as
// resulting from multiplication of certain 4x4 and 4x1 matrices, or
// multiplying three rotation quaternions (Hamilton product), to give:
// e0 = sin(q1/2)*sin(q2/2)*sin(q3/2) + cos(q1/2)*cos(q2/2)*cos(q3/2)
// e1 = sin(q3/2)*cos(q1/2)*cos(q2/2) - sin(q1/2)*sin(q2/2)*cos(q3/2)
// e2 = sin(q1/2)*sin(q3/2)*cos(q2/2) + sin(q2/2)*cos(q1/2)*cos(q3/2)
// e3 = sin(q1/2)*cos(q2/2)*cos(q3/2) - sin(q2/2)*sin(q3/2)*cos(q1/2)
//
// Reference for step 2: Chapter 6 Rotation Matrices II [Mitiguy 2017]
//
// Step 3. Since q2 has already been calculated (in Step 1), substitute
// cos(q2/2) = A and sin(q2/2) = f*A.
// Note: The final results are independent of A and f = tan(q2/2).
// Note: -pi/2 <= q2 <= pi/2 so -0.707 <= [A = cos(q2/2)] <= 0.707
// and -1 <= [f = tan(q2/2)] <= 1.
//
// Step 4. Referring to Step 2 form: (1+f)*e1 + (1+f)*e3 and rearrange to:
// sin(q1/2+q3/2) = (e1+e3)/(A*(1-f))
//
// Referring to Step 2 form: (1+f)*e0 - (1+f)*e2 and rearrange to:
// cos(q1/2+q3/2) = (e0-e2)/(A*(1-f))
//
// Combine the two previous results to produce:
// 1/2*( q1 + q3 ) = atan2( e1+e3, e0-e2 )
//
// Step 5. Referring to Step 2 form: (1-f)*e1 - (1-f)*e3 and rearrange to:
// sin(q1/5-q3/5) = -(e1-e3)/(A*(1+f))
//
// Referring to Step 2 form: (1-f)*e0 + (1-f)*e2 and rearrange to:
// cos(q1/2-q3/2) = (e0+e2)/(A*(1+f))
//
// Combine the two previous results to produce:
// 1/2*( q1 - q3 ) = atan2( e3-e1, e0+e2 )
//
// Step 6. Combine Steps 4 and 5 and solve the linear equations for q1, q3.
// Use zA, zB to handle case in which both atan2 arguments are 0.
// zA = (e1+e3==0 && e0-e2==0) ? 0 : atan2( e1+e3, e0-e2 );
// zB = (e3-e1==0 && e0+e2==0) ? 0 : atan2( e3-e1, e0+e2 );
// Solve: 1/2*( q1 + q3 ) = zA To produce: q1 = zA + zB
// 1/2*( q1 - q3 ) = zB q3 = zA - zB
//
// Step 7. As necessary, modify angles by 2*PI to return angles in range:
// -pi <= q1 <= pi
// -pi/2 <= q2 <= pi/2
// -pi <= q3 <= pi
//
// [Mitiguy, 2017]: "Advanced Dynamics and Motion Simulation,
// For professional engineers and scientists,"
// Prodigy Press, Sunnyvale CA, 2017 (Paul Mitiguy).
// Available at www.MotionGenesis.com
// </pre>
// @note This algorithm is specific to SpaceXYZ (roll-pitch-yaw) order.
// It is easily modified for other SpaceIJK and BodyIJI rotation sequences.
// @author Paul Mitiguy
template <typename T>
Vector3<T> CalcRollPitchYawFromQuaternionAndRotationMatrix(
const Eigen::Quaternion<T>& quaternion, const Matrix3<T>& R) {
using std::atan2;
using std::sqrt;
using std::abs;
// Calculate q2 using lots of information in the rotation matrix.
// Rsum = abs( cos(q2) ) is inherently non-negative.
// R20 = -sin(q2) may be negative, zero, or positive.
const T R22 = R(2, 2);
const T R21 = R(2, 1);
const T R10 = R(1, 0);
const T R00 = R(0, 0);
const T Rsum = sqrt((R22 * R22 + R21 * R21 + R10 * R10 + R00 * R00) / 2);
const T R20 = R(2, 0);
const T q2 = atan2(-R20, Rsum);
// Calculate q1 and q3 from Steps 2-6 (documented above).
const T e0 = quaternion.w(), e1 = quaternion.x();
const T e2 = quaternion.y(), e3 = quaternion.z();
const T yA = e1 + e3, xA = e0 - e2;
const T yB = e3 - e1, xB = e0 + e2;
const T epsilon = Eigen::NumTraits<T>::epsilon();
const auto isSingularA = abs(yA) <= epsilon && abs(xA) <= epsilon;
const auto isSingularB = abs(yB) <= epsilon && abs(xB) <= epsilon;
const T zA = if_then_else(isSingularA, T{0.0}, atan2(yA, xA));
const T zB = if_then_else(isSingularB, T{0.0}, atan2(yB, xB));
T q1 = zA - zB; // First angle in rotation sequence.
T q3 = zA + zB; // Third angle in rotation sequence.
// If necessary, modify angles q1 and/or q3 to be between -pi and pi.
if (q1 > M_PI) q1 = q1 - 2 * M_PI;
if (q1 < -M_PI) q1 = q1 + 2 * M_PI;
if (q3 > M_PI) q3 = q3 - 2 * M_PI;
if (q3 < -M_PI) q3 = q3 + 2 * M_PI;
// Return in Drake/ROS conventional SpaceXYZ q1, q2, q3 (roll-pitch-yaw) order
// (which is equivalent to BodyZYX q3, q2, q1 order).
return Vector3<T>(q1, q2, q3);
}
template <typename T>
void RollPitchYaw<T>::SetFromRotationMatrix(const RotationMatrix<T>& R) {
SetFromQuaternionAndRotationMatrix(R.ToQuaternion(), R);
}
template <typename T>
void RollPitchYaw<T>::SetFromQuaternion(
const Eigen::Quaternion<T>& quaternion) {
SetFromQuaternionAndRotationMatrix(quaternion, RotationMatrix<T>(quaternion));
}
template <typename T>
void RollPitchYaw<T>::SetFromQuaternionAndRotationMatrix(
const Eigen::Quaternion<T>& quaternion, const RotationMatrix<T>& R) {
const Vector3<T> rpy =
CalcRollPitchYawFromQuaternionAndRotationMatrix(quaternion, R.matrix());
SetOrThrowIfNotValidInDebugBuild(rpy);
#ifdef DRAKE_ASSERT_IS_ARMED
// Verify that arguments to this method make sense. Ensure the
// rotation_matrix and quaternion correspond to the same orientation.
constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
const RotationMatrix<T> R_quaternion(quaternion);
constexpr double tolerance = 20 * kEpsilon;
if (!R_quaternion.IsNearlyEqualTo(R, tolerance)) {
std::string message = fmt::format("RollPitchYaw::{}():"
" An element of the RotationMatrix R passed to this method differs by"
" more than {:G} from the corresponding element of the RotationMatrix"
" formed by the Quaternion passed to this method. To avoid this"
" inconsistency, ensure the orientation of R and Quaternion align."
" ({}:{}).", __func__, tolerance, __FILE__, __LINE__);
throw std::logic_error(message);
}
// This algorithm converts a quaternion and %RotationMatrix to %RollPitchYaw.
// It is tested by converting the returned %RollPitchYaw to a %RotationMatrix
// and verifying the rotation matrices are within kEpsilon of each other.
// Assuming sine, cosine are accurate to 4*(standard double-precision epsilon
// = 2.22E-16) and there are two sets of two multiplies and one addition for
// each rotation matrix element, I decided to test with 20 * kEpsilon:
// (1+4*eps)*(1+4*eps)*(1+4*eps) = 1 + 3*(4*eps) + 3*(4*eps)^2 + (4*eps)^3.
// Each + or * or sqrt rounds-off, which can introduce 1/2 eps for each.
// Use: (12*eps) + (4 mults + 1 add) * 1/2 eps = 17.5 eps.
const RollPitchYaw<T> roll_pitch_yaw(rpy);
const RotationMatrix<T> R_rpy = RotationMatrix<T>(roll_pitch_yaw);
DRAKE_ASSERT(R_rpy.IsNearlyEqualTo(R, 20 * kEpsilon));
#endif
}
template <typename T>
boolean<T> RollPitchYaw<T>::IsNearlySameOrientation(
const RollPitchYaw<T>& other, double tolerance) const {
// Note: When pitch is close to PI/2 or -PI/2, derivative calculations for
// Euler angles can encounter numerical problems (dividing by nearly 0).
// Although values of angles may "jump around" (difficult derivatives), the
// angles' values should be able to be accurately reproduced.
const RotationMatrix<T> R1(*this);
const RotationMatrix<T> R2(other);
return R1.IsNearlyEqualTo(R2, tolerance);
}
template <typename T>
void RollPitchYaw<T>::ThrowPitchAngleViolatesGimbalLockTolerance(
const char* function_name, const char* file_name, const int line_number,
const T& pitch_angle) {
const double pitch_radians = ExtractDoubleOrThrow(pitch_angle);
const double cos_pitch_angle = std::cos(pitch_radians);
DRAKE_ASSERT(DoesCosPitchAngleViolateGimbalLockTolerance(cos_pitch_angle));
const double tolerance_degrees =
GimbalLockPitchAngleTolerance() * 180 / M_PI;
std::string message = fmt::format("RollPitchYaw::{}():"
" Pitch angle p = {:G} degrees is within {:G} degrees of gimbal-lock."
" There is a divide-by-zero error (singularity) at gimbal-lock. Pitch"
" angles near gimbal-lock cause numerical inaccuracies. To avoid this"
" orientation singularity, use a quaternion -- not RollPitchYaw."
" ({}:{}).", function_name, pitch_radians * 180 / M_PI,
tolerance_degrees, file_name, line_number);
throw std::logic_error(message);
}
} // namespace math
} // namespace drake
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::math::RollPitchYaw)