Drake
RollPitchYaw< T > Class Template Reference

Detailed Description

template<typename T>
class drake::math::RollPitchYaw< T >

This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by "roll-pitch-yaw" angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by "yaw-pitch-roll" angles [y, p, r].

The rotation matrix R_AD associated with this roll-pitch-yaw [r, p, y] rotation sequence is equal to the matrix multiplication shown below.

⎡cos(y) -sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤
R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) -sin(r)⎥
⎣ 0 0 1⎦ ⎣-sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦
= R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so 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 right-handed rotations relative to A.
  • 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a roll angle r about Dx = Cx. Note: D and C are no longer aligned.
  • 2nd rotation R_BC: Frames D, C (collectively – as if welded together) rotate relative to frame B, A by a pitch angle p about Cy = By. Note: C and B are no longer aligned.
  • 3rd rotation R_AB: Frames D, C, B (collectively – as if welded) rotate relative to frame A by a roll angle 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.
See also
Multibody Quantities for monogram notation for dynamics and a discussion on rotation matrices.
Note
This class does not store the frames associated with this rotation sequence.
Template Parameters
TThe underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

#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 roll-pitch-yaw 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 roll-pitch-yaw 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 roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`. More...
 
void SetFromRotationMatrix (const RotationMatrix< T > &R)
 Uses a RotationMatrix to set this RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`. More...
 
void SetFromQuaternionAndRotationMatrix (const Eigen::Quaternion< T > &quaternion, const RotationMatrix< T > &R)
 Uses a quaternion and its associated rotation matrix R to accurately set this @RollPitchYaw roll-pitch-yaw angles (SpaceXYZ Euler angles), even when the pitch angle is within 1E-6 of π/2 or -π/2. More...
 
const Vector3< T > & vector () const
 Returns the Vector3 underlying this RollPitchYaw. More...
 
const T & roll_angle () const
 Returns the roll-angle underlying this RollPitchYaw. More...
 
const T & pitch_angle () const
 Returns the pitch-angle underlying this RollPitchYaw. More...
 
const T & yaw_angle () const
 Returns the yaw-angle underlying this RollPitchYaw. More...
 
void set_roll_angle (const T &r)
 Set the roll-angle underlying this RollPitchYaw. More...
 
void set_pitch_angle (const T &p)
 Set the pitch-angle underlying this RollPitchYaw. More...
 
void set_yaw_angle (const T &y)
 Set the yaw-angle 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 roll-pitch-yaw angles [r, p, y] are in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`. More...
 
boolean< T > DoesPitchAngleViolateGimbalLockTolerance () const
 Returns true if the pitch-angle p is within an internally-defined tolerance of gimbal-lock. 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 roll-pitch-yaw 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 roll-pitch-yaw 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ˢᵗ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D. More...
 
Vector3< T > CalcRpyDDtFromRpyDtAndAngularAccelInParent (const Vector3< T > &rpyDt, const Vector3< T > &alpha_AD_A) const
 Uses angular acceleration to compute the 2ⁿᵈ time-derivative 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ⁿᵈ time-derivative 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
 
RollPitchYawoperator= (const RollPitchYaw &)=default
 
 RollPitchYaw (RollPitchYaw &&)=default
 
RollPitchYawoperator= (RollPitchYaw &&)=default
 

Static Public Member Functions

static boolean< T > DoesCosPitchAngleViolateGimbalLockTolerance (const T &cos_pitch_angle)
 Returns true if the pitch-angle p is close to gimbal-lock, which means cos(p) ≈ 0 or p ≈ (n*π + π/2) where n = 0, ±1, ±2, .... More...
 
static double GimbalLockPitchAngleTolerance ()
 Returns the internally-defined allowable closeness (in radians) of the pitch angle p to gimbal-lock, 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...
 

Constructor & Destructor Documentation

◆ RollPitchYaw() [1/6]

RollPitchYaw ( const RollPitchYaw< T > &  )
default

◆ RollPitchYaw() [2/6]

RollPitchYaw ( RollPitchYaw< T > &&  )
default

◆ RollPitchYaw() [3/6]

RollPitchYaw ( const Vector3< T > &  rpy)
explicit

Constructs a RollPitchYaw from a 3x1 array of angles.

Parameters
[in]rpy3x1 array with roll, pitch, yaw angles (units of radians).
Exceptions
std::logic_errorin debug builds if !IsValid(rpy).

◆ RollPitchYaw() [4/6]

RollPitchYaw ( const T &  roll,
const T &  pitch,
const T &  yaw 
)

Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).

Parameters
[in]rollx-directed angle in SpaceXYZ rotation sequence.
[in]pitchy-directed angle in SpaceXYZ rotation sequence.
[in]yawz-directed angle in SpaceXYZ rotation sequence.
Exceptions
std::logic_errorin debug builds if !IsValid(Vector3<T>(roll, pitch, yaw)).

◆ RollPitchYaw() [5/6]

RollPitchYaw ( const RotationMatrix< T > &  R)
explicit

Uses a RotationMatrix to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`.

Parameters
[in]Ra RotationMatrix.
Note
This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

◆ RollPitchYaw() [6/6]

RollPitchYaw ( const Eigen::Quaternion< T > &  quaternion)
explicit

Uses a Quaternion to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`.

Parameters
[in]quaternionunit Quaternion.
Note
This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.
Exceptions
std::logic_errorin debug builds if !IsValid(rpy).

Member Function Documentation

◆ CalcAngularVelocityInChildFromRpyDt()

Vector3<T> CalcAngularVelocityInChildFromRpyDt ( const Vector3< T > &  rpyDt) const

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameters
[in]rpyDtTime-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns
w_AD_D, frame D's angular velocity in frame A, expressed in D.

◆ CalcAngularVelocityInParentFromRpyDt()

Vector3<T> CalcAngularVelocityInParentFromRpyDt ( const Vector3< T > &  rpyDt) const

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameters
[in]rpyDtTime-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns
w_AD_A, frame D's angular velocity in frame A, expressed in A.

◆ CalcRotationMatrixDt()

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.

The roll-pitch-yaw angles r, p, y are regarded as functions of t [i.e., r(t), p(t), y(t)].

Parameters
[in]rpyDtOrdinary derivative of rpy with respect to an independent variable t (t usually denotes time, but not necessarily).
Returns
Ṙ, the ordinary derivative of 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 * ẏ.

◆ CalcRpyDDtFromAngularAccelInChild()

Vector3<T> CalcRpyDDtFromAngularAccelInChild ( const Vector3< T > &  rpyDt,
const Vector3< T > &  alpha_AD_D 
) const

Uses angular acceleration to compute the 2ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameters
[in]rpyDttime-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
[in]alpha_AD_D,frameD's angular acceleration in frame A, expressed in frame D.
Returns
[r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw.
Exceptions
std::logic_errorif cos(p) ≈ 0 (p is near gimbal-lock).
Note
This method has a divide-by-zero error (singularity) when the cosine of the pitch angle 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.

◆ CalcRpyDDtFromRpyDtAndAngularAccelInParent()

Vector3<T> CalcRpyDDtFromRpyDtAndAngularAccelInParent ( const Vector3< T > &  rpyDt,
const Vector3< T > &  alpha_AD_A 
) const

Uses angular acceleration to compute the 2ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameters
[in]rpyDttime-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
[in]alpha_AD_A,frameD's angular acceleration in frame A, expressed in frame A.
Returns
[r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw.
Exceptions
std::logic_errorif cos(p) ≈ 0 (p is near gimbal-lock).
Note
This method has a divide-by-zero error (singularity) when the cosine of the pitch angle 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.

◆ CalcRpyDtFromAngularVelocityInParent()

Vector3<T> CalcRpyDtFromAngularVelocityInParent ( const Vector3< T > &  w_AD_A) const

Uses angular velocity to compute the 1ˢᵗ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameters
[in]w_AD_A,frameD's angular velocity in frame A, expressed in A.
Returns
[ṙ; ṗ; ẏ], the 1ˢᵗ time-derivative of this RollPitchYaw.
Exceptions
std::logic_errorif cos(p) ≈ 0 (p is near gimbal-lock).
Note
This method has a divide-by-zero error (singularity) when the cosine of the pitch angle 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.

◆ DoesCosPitchAngleViolateGimbalLockTolerance()

static boolean<T> DoesCosPitchAngleViolateGimbalLockTolerance ( const T &  cos_pitch_angle)
static

Returns true if the pitch-angle p is close to gimbal-lock, 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 internally-defined tolerance of gimbal-lock.

Parameters
[in]cos_pitch_anglecosine of the pitch angle, i.e., cos(p).
Note
Pitch-angles close to gimbal-lock can can cause problems with numerical precision and numerical integration.

◆ DoesPitchAngleViolateGimbalLockTolerance()

boolean<T> DoesPitchAngleViolateGimbalLockTolerance ( ) const

Returns true if the pitch-angle p is within an internally-defined tolerance of gimbal-lock.

In other words, this method returns true if p ≈ (n*π + π/2) where n = 0, ±1, ±2, ....

Note
To improve efficiency when cos(pitch_angle()) is already calculated, instead use the function DoesCosPitchAngleViolateGimbalLockTolerance().
See also
DoesCosPitchAngleViolateGimbalLockTolerance()

◆ GimbalLockPitchAngleTolerance()

static double GimbalLockPitchAngleTolerance ( )
static

Returns the internally-defined allowable closeness (in radians) of the pitch angle p to gimbal-lock, i.e., the allowable proximity of p to (n*π + π/2) where n = 0, ±1, ±2, ....

◆ IsNearlyEqualTo()

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.

Parameters
[in]otherRollPitchYaw to compare to this.
[in]tolerancemaximum allowable absolute difference between the matrix elements in this and other.
Returns
true if ‖this - other‖∞ <= tolerance.

◆ IsNearlySameOrientation()

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.

Parameters
[in]otherRollPitchYaw to compare to this.
[in]tolerancemaximum allowable absolute difference between R1, R2.
Returns
true if ‖R1 - R2‖∞ <= tolerance.

◆ IsRollPitchYawInCanonicalRange()

boolean<T> IsRollPitchYawInCanonicalRange ( ) const

Returns true if roll-pitch-yaw angles [r, p, y] are in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`.

◆ IsValid()

static boolean<T> IsValid ( const Vector3< T > &  rpy)
static

Returns true if rpy contains valid roll, pitch, yaw angles.

Parameters
[in]rpyallegedly valid roll, pitch, yaw angles.
Note
an angle is invalid if it is NaN or infinite.

◆ operator=() [1/2]

RollPitchYaw& operator= ( const RollPitchYaw< T > &  )
default

◆ operator=() [2/2]

RollPitchYaw& operator= ( RollPitchYaw< T > &&  )
default

◆ pitch_angle()

const T& pitch_angle ( ) const

Returns the pitch-angle underlying this RollPitchYaw.

◆ roll_angle()

const T& roll_angle ( ) const

Returns the roll-angle underlying this RollPitchYaw.

◆ set() [1/2]

RollPitchYaw<T>& set ( const Vector3< T > &  rpy)

Sets this RollPitchYaw from a 3x1 array of angles.

Parameters
[in]rpy3x1 array with roll, pitch, yaw angles (units of radians).
Exceptions
std::logic_errorin debug builds if !IsValid(rpy).

◆ set() [2/2]

RollPitchYaw<T>& set ( const T &  roll,
const T &  pitch,
const T &  yaw 
)

Sets this RollPitchYaw from roll, pitch, yaw angles (units of radians).

Parameters
[in]rollx-directed angle in SpaceXYZ rotation sequence.
[in]pitchy-directed angle in SpaceXYZ rotation sequence.
[in]yawz-directed angle in SpaceXYZ rotation sequence.
Exceptions
std::logic_errorin debug builds if !IsValid(Vector3<T>(roll, pitch, yaw)).

◆ set_pitch_angle()

void set_pitch_angle ( const T &  p)

Set the pitch-angle underlying this RollPitchYaw.

Parameters
[in]ppitch angle (in units of radians).

◆ set_roll_angle()

void set_roll_angle ( const T &  r)

Set the roll-angle underlying this RollPitchYaw.

Parameters
[in]rroll angle (in units of radians).

◆ set_yaw_angle()

void set_yaw_angle ( const T &  y)

Set the yaw-angle underlying this RollPitchYaw.

Parameters
[in]yyaw angle (in units of radians).

◆ SetFromQuaternion()

void SetFromQuaternion ( const Eigen::Quaternion< T > &  quaternion)

Uses a Quaternion to set this RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`.

Parameters
[in]quaternionunit Quaternion.
Note
This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.
Exceptions
std::logic_errorin debug builds if !IsValid(rpy).

◆ SetFromQuaternionAndRotationMatrix()

void SetFromQuaternionAndRotationMatrix ( const Eigen::Quaternion< T > &  quaternion,
const RotationMatrix< T > &  R 
)

Uses a quaternion and its associated rotation matrix R to accurately set this @RollPitchYaw roll-pitch-yaw angles (SpaceXYZ Euler angles), even when the pitch angle is within 1E-6 of π/2 or -π/2.

Parameters
[in]quaternionunit quaternion with elements [e0, e1, e2, e3].
[in]RThe RotationMatrix corresponding to quaternion.
Returns
RollPitchYaw containing angles [r, p, y] with range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`.
Exceptions
std::logic_errorin debug builds if rpy fails IsValid(rpy).
Note
Aborts in debug builds if quaternion does not correspond to R.

◆ SetFromRotationMatrix()

void SetFromRotationMatrix ( const RotationMatrix< T > &  R)

Uses a RotationMatrix to set this RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π`.

Parameters
[in]Ra RotationMatrix.
Note
This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

◆ ToMatrix3ViaRotationMatrix()

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().

◆ ToQuaternion()

Eigen::Quaternion<T> ToQuaternion ( ) const

Returns a quaternion representation of this RollPitchYaw.

◆ ToRotationMatrix()

RotationMatrix< T > ToRotationMatrix ( ) const

Returns the RotationMatrix representation of this RollPitchYaw.

◆ vector()

const Vector3<T>& vector ( ) const

Returns the Vector3 underlying this RollPitchYaw.

◆ yaw_angle()

const T& yaw_angle ( ) const

Returns the yaw-angle underlying this RollPitchYaw.

Friends And Related Function Documentation

◆ RollPitchYawd

using RollPitchYawd = RollPitchYaw<double>
related

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


The documentation for this class was generated from the following files: