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.
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
 T The 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 Ṙ, 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...

◆ 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] rpy 3x1 array with roll, pitch, yaw angles (units of radians).
Exceptions
 std::logic_error in 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] roll x-directed angle in SpaceXYZ rotation sequence. [in] pitch y-directed angle in SpaceXYZ rotation sequence. [in] yaw z-directed angle in SpaceXYZ rotation sequence.
Exceptions
 std::logic_error in debug builds if !IsValid(Vector3(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] R a 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] quaternion unit 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_error in debug builds if !IsValid(rpy).

◆ CalcAngularVelocityInChildFromRpyDt()

 Vector3 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] rpyDt Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns
w_AD_D, frame D's angular velocity in frame A, expressed in D.

◆ CalcAngularVelocityInParentFromRpyDt()

 Vector3 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] rpyDt Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns
w_AD_A, frame D's angular velocity in frame A, expressed in A.

◆ CalcRotationMatrixDt()

 Matrix3 CalcRotationMatrixDt ( const Vector3< T > & rpyDt ) const

Forms Ṙ, 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] rpyDt Ordinary derivative of rpy with respect to an independent variable t (t usually denotes time, but not necessarily).
Returns
Ṙ, the ordinary derivative of R with respect to t, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.

◆ CalcRpyDDtFromAngularAccelInChild()

 Vector3 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] rpyDt time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ]. [in] alpha_AD_D,frame D's angular acceleration in frame A, expressed in frame D.
Returns
[r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw.
Exceptions
 std::logic_error if 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 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] rpyDt time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ]. [in] alpha_AD_A,frame D's angular acceleration in frame A, expressed in frame A.
Returns
[r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw.
Exceptions
 std::logic_error if 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 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,frame D's angular velocity in frame A, expressed in A.
Returns
[ṙ; ṗ; ẏ], the 1ˢᵗ time-derivative of this RollPitchYaw.
Exceptions
 std::logic_error if 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 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_angle cosine 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 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().
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 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] other RollPitchYaw to compare to this. [in] tolerance maximum 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] other RollPitchYaw to compare to this. [in] tolerance maximum allowable absolute difference between R1, R2.
Returns
true if ‖R1 - R2‖∞ <= tolerance.

◆ IsRollPitchYawInCanonicalRange()

 boolean IsRollPitchYawInCanonicalRange ( ) const

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

◆ IsValid()

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

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

Parameters
 [in] rpy allegedly 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& set ( const Vector3< T > & rpy )

Sets this RollPitchYaw from a 3x1 array of angles.

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

◆ set() [2/2]

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

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

Parameters
 [in] roll x-directed angle in SpaceXYZ rotation sequence. [in] pitch y-directed angle in SpaceXYZ rotation sequence. [in] yaw z-directed angle in SpaceXYZ rotation sequence.
Exceptions
 std::logic_error in debug builds if !IsValid(Vector3(roll, pitch, yaw)).

◆ set_pitch_angle()

 void set_pitch_angle ( const T & p )

Set the pitch-angle underlying this RollPitchYaw.

Parameters
 [in] p pitch angle (in units of radians).

◆ set_roll_angle()

 void set_roll_angle ( const T & r )

Set the roll-angle underlying this RollPitchYaw.

Parameters
 [in] r roll angle (in units of radians).

◆ set_yaw_angle()

 void set_yaw_angle ( const T & y )

Set the yaw-angle underlying this RollPitchYaw.

Parameters
 [in] y yaw 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] quaternion unit 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_error in 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] quaternion unit quaternion with elements [e0, e1, e2, e3]. [in] R The RotationMatrix corresponding to quaternion.
Returns
RollPitchYaw containing angles [r, p, y] with range -π <= r <= π, -π/2 <= p <= π/2,-π <= y <= π.
Exceptions
 std::logic_error in 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] R a 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 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 ToQuaternion ( ) const

Returns a quaternion representation of this RollPitchYaw.

◆ ToRotationMatrix()

 RotationMatrix< T > ToRotationMatrix ( ) const

Returns the RotationMatrix representation of this RollPitchYaw.

◆ vector()

 const Vector3& vector ( ) const

Returns the Vector3 underlying this RollPitchYaw.

◆ yaw_angle()

 const T& yaw_angle ( ) const

Returns the yaw-angle underlying this` RollPitchYaw.

◆ RollPitchYawd

 using RollPitchYawd = RollPitchYaw
related

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

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