Drake

This class represents the orientation between two arbitrary frames A and D associated with a Spacefixed (extrinsic) XYZ rotation by "rollpitchyaw" angles [r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by "yawpitchroll" angles [y, p, r]
.
The rotation matrix R_AD
associated with this rollpitchyaw [r, p, y]
rotation sequence is equal to the matrix multiplication shown below.
Di = Ci = Bi = Ai (i = x, y, z)
where Dx, Dy, Dz and Ax, Ay, Az are orthogonal unit vectors fixed in frames D and A respectively. Similarly for Bx, By, Bz and Cx, Cy, Cz in frame B, C. Then D is subjected to successive righthanded rotations relative to A. r
about Dx = Cx
. Note: D and C are no longer aligned. p
about Cy = By
. Note: C and B are no longer aligned. y
about Bz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D is R_AD
.T  The scalar type, which must be one of the default scalars. 
#include <drake/math/roll_pitch_yaw.h>
Public Member Functions  
RollPitchYaw (const Vector3< T > &rpy)  
Constructs a RollPitchYaw from a 3x1 array of angles. More...  
RollPitchYaw (const T &roll, const T &pitch, const T &yaw)  
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units). More...  
RollPitchYaw (const RotationMatrix< T > &R)  
Uses a RotationMatrix to construct a RollPitchYaw with rollpitchyaw angles [r, p, y] in the range π <= r <= π , π/2 <= p <= π/2 , π <= y <= π . More...  
RollPitchYaw (const Eigen::Quaternion< T > &quaternion)  
Uses a Quaternion to construct a RollPitchYaw with rollpitchyaw angles [r, p, y] in the range π <= r <= π , π/2 <= p <= π/2 , π <= y <= π . More...  
RollPitchYaw< T > &  set (const Vector3< T > &rpy) 
Sets this RollPitchYaw from a 3x1 array of angles. More...  
RollPitchYaw< T > &  set (const T &roll, const T &pitch, const T &yaw) 
Sets this RollPitchYaw from roll, pitch, yaw angles (units of radians). More...  
void  SetFromQuaternion (const Eigen::Quaternion< T > &quaternion) 
Uses a Quaternion to set this RollPitchYaw with rollpitchyaw angles [r, p, y] in the range π <= r <= π , π/2 <= p <= π/2 , π <= y <= π . More...  
void  SetFromRotationMatrix (const RotationMatrix< T > &R) 
Uses a highaccuracy efficient algorithm to set the rollpitchyaw angles [r, p, y] that underlie this @RollPitchYaw, even when the pitch angle p is very near a singularity (when p is within 1E6 of π/2 or π/2). More...  
const Vector3< T > &  vector () const 
Returns the Vector3 underlying this RollPitchYaw. More...  
const T &  roll_angle () const 
Returns the rollangle underlying this RollPitchYaw. More...  
const T &  pitch_angle () const 
Returns the pitchangle underlying this RollPitchYaw. More...  
const T &  yaw_angle () const 
Returns the yawangle underlying this RollPitchYaw. More...  
void  set_roll_angle (const T &r) 
Set the rollangle underlying this RollPitchYaw. More...  
void  set_pitch_angle (const T &p) 
Set the pitchangle underlying this RollPitchYaw. More...  
void  set_yaw_angle (const T &y) 
Set the yawangle underlying this RollPitchYaw. More...  
Eigen::Quaternion< T >  ToQuaternion () const 
Returns a quaternion representation of this RollPitchYaw. More...  
RotationMatrix< T >  ToRotationMatrix () const 
Returns the RotationMatrix representation of this RollPitchYaw. More...  
Matrix3< T >  ToMatrix3ViaRotationMatrix () const 
Returns the 3x3 matrix representation of the RotationMatrix that corresponds to this RollPitchYaw. More...  
boolean< T >  IsNearlyEqualTo (const RollPitchYaw< T > &other, double tolerance) const 
Compares each element of this to the corresponding element of other to check if they are the same to within a specified tolerance . More...  
boolean< T >  IsNearlySameOrientation (const RollPitchYaw< T > &other, double tolerance) const 
Compares each element of the RotationMatrix R1 produced by this to the corresponding element of the RotationMatrix R2 produced by other to check if they are the same to within a specified tolerance . More...  
boolean< T >  IsRollPitchYawInCanonicalRange () const 
Returns true if rollpitchyaw angles [r, p, y] are in the range π <= r <= π , π/2 <= p <= π/2 , π <= y <= π . More...  
boolean< T >  DoesPitchAngleViolateGimbalLockTolerance () const 
Returns true if the pitchangle p is within an internallydefined tolerance of gimballock. More...  
Matrix3< T >  CalcRotationMatrixDt (const Vector3< T > &rpyDt) const 
Forms Ṙ, the ordinary derivative of the RotationMatrix R with respect to an independent variable t (t usually denotes time) and R is the RotationMatrix formed by this RollPitchYaw. More...  
Vector3< T >  CalcAngularVelocityInParentFromRpyDt (const Vector3< T > &rpyDt) const 
Calculates angular velocity from this RollPitchYaw whose rollpitchyaw angles [r; p; y] relate the orientation of two generic frames A and D. More...  
Vector3< T >  CalcAngularVelocityInChildFromRpyDt (const Vector3< T > &rpyDt) const 
Calculates angular velocity from this RollPitchYaw whose rollpitchyaw angles [r; p; y] relate the orientation of two generic frames A and D. More...  
Vector3< T >  CalcRpyDtFromAngularVelocityInParent (const Vector3< T > &w_AD_A) const 
Uses angular velocity to compute the 1ˢᵗ timederivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D. More...  
const Matrix3< T >  CalcMatrixRelatingRpyDtToAngularVelocityInParent () const 
For this RollPitchYaw with rollpitchyaw angles [r; p; y] which relate the orientation of two generic frames A and D, returns the 3x3 matrix M that contains the partial derivatives of [ṙ, ṗ, ẏ] with respect to [wx; wy; wz]ₐ (which is w_AD_A expressed in A). More...  
Vector3< T >  CalcRpyDDtFromRpyDtAndAngularAccelInParent (const Vector3< T > &rpyDt, const Vector3< T > &alpha_AD_A) const 
Uses angular acceleration to compute the 2ⁿᵈ timederivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D. More...  
Vector3< T >  CalcRpyDDtFromAngularAccelInChild (const Vector3< T > &rpyDt, const Vector3< T > &alpha_AD_D) const 
Uses angular acceleration to compute the 2ⁿᵈ timederivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D. More...  
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable  
RollPitchYaw (const RollPitchYaw &)=default  
RollPitchYaw &  operator= (const RollPitchYaw &)=default 
RollPitchYaw (RollPitchYaw &&)=default  
RollPitchYaw &  operator= (RollPitchYaw &&)=default 
Static Public Member Functions  
static boolean< T >  DoesCosPitchAngleViolateGimbalLockTolerance (const T &cos_pitch_angle) 
Returns true if the pitchangle p is close to gimballock, which means cos(p) ≈ 0 or p ≈ (n*π + π/2) where n = 0, ±1, ±2, ... . More...  
static double  GimbalLockPitchAngleTolerance () 
Returns the internallydefined allowable closeness (in radians) of the pitch angle p to gimballock, i.e., the allowable proximity of p to (n*π + π/2) where n = 0, ±1, ±2, ... . More...  
static boolean< T >  IsValid (const Vector3< T > &rpy) 
Returns true if rpy contains valid roll, pitch, yaw angles. More...  
Related Functions  
(Note that these are not member functions.)  
using  RollPitchYawd = RollPitchYaw< double > 
Abbreviation (alias/typedef) for a RollPitchYaw double scalar type. More...  

default 

default 

explicit 
Constructs a RollPitchYaw from a 3x1 array of angles.
[in]  rpy  3x1 array with roll, pitch, yaw angles (units of radians). 
std::exception  in debug builds if !IsValid(rpy). 
RollPitchYaw  (  const T &  roll, 
const T &  pitch,  
const T &  yaw  
) 
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
[in]  roll  xdirected angle in SpaceXYZ rotation sequence. 
[in]  pitch  ydirected angle in SpaceXYZ rotation sequence. 
[in]  yaw  zdirected angle in SpaceXYZ rotation sequence. 
std::exception  in debug builds if !IsValid(Vector3<T>(roll, pitch, yaw)). 

explicit 
Uses a RotationMatrix to construct a RollPitchYaw with rollpitchyaw angles [r, p, y]
in the range π <= r <= π
, π/2 <= p <= π/2
, π <= y <= π
.
[in]  R  a RotationMatrix. 

explicit 
Uses a Quaternion to construct a RollPitchYaw with rollpitchyaw angles [r, p, y]
in the range π <= r <= π
, π/2 <= p <= π/2
, π <= y <= π
.
[in]  quaternion  unit Quaternion. 
std::exception  in debug builds if !IsValid(rpy). 
Calculates angular velocity from this
RollPitchYaw whose rollpitchyaw angles [r; p; y]
relate the orientation of two generic frames A and D.
[in]  rpyDt  Timederivative of [r; p; y] , i.e., [ṙ; ṗ; ẏ] . 
Calculates angular velocity from this
RollPitchYaw whose rollpitchyaw angles [r; p; y]
relate the orientation of two generic frames A and D.
[in]  rpyDt  Timederivative of [r; p; y] , i.e., [ṙ; ṗ; ẏ] . 
const Matrix3<T> CalcMatrixRelatingRpyDtToAngularVelocityInParent  (  )  const 
For this
RollPitchYaw with rollpitchyaw angles [r; p; y]
which relate the orientation of two generic frames A and D, returns the 3x3 matrix M that contains the partial derivatives of [ṙ, ṗ, ẏ] with respect to [wx; wy; wz]ₐ
(which is w_AD_A expressed in A).
In other words, rpyDt = M * w_AD_A
.
[in]  function_name  name of the calling function/method. 
std::exception  if cos(p) ≈ 0 (p is near gimballock). 
p
is zero [i.e., cos(p) = 0
]. This problem (called "gimbal lock") occurs when p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., when cos(p) ≈ 0
. Forms Ṙ, the ordinary derivative of the RotationMatrix R
with respect to an independent variable t
(t
usually denotes time) and R
is the RotationMatrix formed by this
RollPitchYaw.
The rollpitchyaw angles r, p, y are regarded as functions of t
[i.e., r(t), p(t), y(t)].
[in]  rpyDt  Ordinary derivative of rpy with respect to an independent variable t (t usually denotes time, but not necessarily). 
R
with respect to t
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ. Vector3<T> CalcRpyDDtFromAngularAccelInChild  (  const Vector3< T > &  rpyDt, 
const Vector3< T > &  alpha_AD_D  
)  const 
Uses angular acceleration to compute the 2ⁿᵈ timederivative of this
RollPitchYaw whose angles [r; p; y]
orient two generic frames A and D.
[in]  rpyDt  timederivative of [r; p; y] , i.e., [ṙ; ṗ; ẏ] . 
[in]  alpha_AD_D,frame  D's angular acceleration in frame A, expressed in frame D. 
[r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative of this
RollPitchYaw. std::exception  if cos(p) ≈ 0 (p is near gimballock). 
p
is zero [i.e., cos(p) = 0
]. This problem (called "gimbal lock") occurs when p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., when cos(p) ≈ 0
. Vector3<T> CalcRpyDDtFromRpyDtAndAngularAccelInParent  (  const Vector3< T > &  rpyDt, 
const Vector3< T > &  alpha_AD_A  
)  const 
Uses angular acceleration to compute the 2ⁿᵈ timederivative of this
RollPitchYaw whose angles [r; p; y]
orient two generic frames A and D.
[in]  rpyDt  timederivative of [r; p; y] , i.e., [ṙ; ṗ; ẏ] . 
[in]  alpha_AD_A,frame  D's angular acceleration in frame A, expressed in frame A. 
[r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative of this
RollPitchYaw. std::exception  if cos(p) ≈ 0 (p is near gimballock). 
p
is zero [i.e., cos(p) = 0
]. This problem (called "gimbal lock") occurs when p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., when cos(p) ≈ 0
. Uses angular velocity to compute the 1ˢᵗ timederivative of this
RollPitchYaw whose angles [r; p; y]
orient two generic frames A and D.
[in]  w_AD_A,frame  D's angular velocity in frame A, expressed in A. 
[ṙ; ṗ; ẏ]
, the 1ˢᵗ timederivative of this
RollPitchYaw. std::exception  if cos(p) ≈ 0 (p is near gimballock). 
p
is zero [i.e., cos(p) = 0
]. This problem (called "gimbal lock") occurs when p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., when cos(p) ≈ 0
.

static 
Returns true if the pitchangle p
is close to gimballock, which means cos(p) ≈ 0
or p ≈ (n*π + π/2)
where n = 0, ±1, ±2, ...
.
More specifically, returns true if abs(cos_pitch_angle)
is less than an internallydefined tolerance of gimballock.
[in]  cos_pitch_angle  cosine of the pitch angle, i.e., cos(p) . 
boolean<T> DoesPitchAngleViolateGimbalLockTolerance  (  )  const 
Returns true if the pitchangle p
is within an internallydefined tolerance of gimballock.
In other words, this method returns true if p ≈ (n*π + π/2)
where n = 0, ±1, ±2, ...
.

static 
Returns the internallydefined allowable closeness (in radians) of the pitch angle p
to gimballock, i.e., the allowable proximity of p
to (n*π + π/2)
where n = 0, ±1, ±2, ...
.
boolean<T> IsNearlyEqualTo  (  const RollPitchYaw< T > &  other, 
double  tolerance  
)  const 
Compares each element of this
to the corresponding element of other
to check if they are the same to within a specified tolerance
.
[in]  other  RollPitchYaw to compare to this . 
[in]  tolerance  maximum allowable absolute difference between the matrix elements in this and other . 
true
if ‖this  other‖∞ <= tolerance
. boolean<T> IsNearlySameOrientation  (  const RollPitchYaw< T > &  other, 
double  tolerance  
)  const 
Compares each element of the RotationMatrix R1 produced by this
to the corresponding element of the RotationMatrix R2 produced by other
to check if they are the same to within a specified tolerance
.
[in]  other  RollPitchYaw to compare to this . 
[in]  tolerance  maximum allowable absolute difference between R1, R2. 
true
if ‖R1  R2‖∞ <= tolerance
. boolean<T> IsRollPitchYawInCanonicalRange  (  )  const 
Returns true if rollpitchyaw angles [r, p, y]
are in the range π <= r <= π
, π/2 <= p <= π/2
, π <= y <= π
.
Returns true if rpy
contains valid roll, pitch, yaw angles.
[in]  rpy  allegedly valid roll, pitch, yaw angles. 

default 

default 
const T& pitch_angle  (  )  const 
Returns the pitchangle underlying this
RollPitchYaw.
const T& roll_angle  (  )  const 
Returns the rollangle underlying this
RollPitchYaw.
RollPitchYaw<T>& set  (  const Vector3< T > &  rpy  ) 
Sets this
RollPitchYaw from a 3x1 array of angles.
[in]  rpy  3x1 array with roll, pitch, yaw angles (units of radians). 
std::exception  in debug builds if !IsValid(rpy). 
RollPitchYaw<T>& set  (  const T &  roll, 
const T &  pitch,  
const T &  yaw  
) 
Sets this
RollPitchYaw from roll, pitch, yaw angles (units of radians).
[in]  roll  xdirected angle in SpaceXYZ rotation sequence. 
[in]  pitch  ydirected angle in SpaceXYZ rotation sequence. 
[in]  yaw  zdirected angle in SpaceXYZ rotation sequence. 
std::exception  in debug builds if !IsValid(Vector3<T>(roll, pitch, yaw)). 
void set_pitch_angle  (  const T &  p  ) 
Set the pitchangle underlying this
RollPitchYaw.
[in]  p  pitch angle (in units of radians). 
void set_roll_angle  (  const T &  r  ) 
Set the rollangle underlying this
RollPitchYaw.
[in]  r  roll angle (in units of radians). 
void set_yaw_angle  (  const T &  y  ) 
Set the yawangle underlying this
RollPitchYaw.
[in]  y  yaw angle (in units of radians). 
void SetFromQuaternion  (  const Eigen::Quaternion< T > &  quaternion  ) 
Uses a Quaternion to set this
RollPitchYaw with rollpitchyaw angles [r, p, y]
in the range π <= r <= π
, π/2 <= p <= π/2
, π <= y <= π
.
[in]  quaternion  unit Quaternion. 
std::exception  in debug builds if !IsValid(rpy). 
void SetFromRotationMatrix  (  const RotationMatrix< T > &  R  ) 
Uses a highaccuracy efficient algorithm to set the rollpitchyaw angles [r, p, y]
that underlie this
@RollPitchYaw, even when the pitch angle p is very near a singularity (when p is within 1E6 of π/2 or π/2).
After calling this method, the underlying rollpitchyaw [r, p, y]
has range π <= r <= π
, π/2 <= p <= π/2
, π <= y <= π
.
[in]  R  a RotationMatrix. 
Matrix3<T> ToMatrix3ViaRotationMatrix  (  )  const 
Returns the 3x3 matrix representation of the RotationMatrix that corresponds to this
RollPitchYaw.
This is a convenient "sugar" method that is exactly equivalent to RotationMatrix(rpy).ToMatrix3().
Eigen::Quaternion<T> ToQuaternion  (  )  const 
Returns a quaternion representation of this
RollPitchYaw.
RotationMatrix<T> ToRotationMatrix  (  )  const 
Returns the RotationMatrix representation of this
RollPitchYaw.
const Vector3<T>& vector  (  )  const 
Returns the Vector3 underlying this
RollPitchYaw.
const T& yaw_angle  (  )  const 
Returns the yawangle underlying this
RollPitchYaw.

related 
Abbreviation (alias/typedef) for a RollPitchYaw double scalar type.