Drake

This class represents a proper rigid transform between two frames which can be regarded in two ways.
A rigid transform describes the "pose" between two frames A and B (i.e., the relative orientation and position of A to B). Alternately, it can be regarded as a distancepreserving operator that can rotate and/or translate a rigid body without changing its shape or size (rigid) and without mirroring/reflecting the body (proper), e.g., it can add one position vector to another and express the result in a particular basis as p_AoQ_A = X_AB * p_BoQ_B
(Q is any point). In many ways, this rigid transform class is conceptually similar to using a homogeneous matrix as a linear operator. See operator* documentation for an exception.
The class stores a RotationMatrix that relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The class also stores a position vector from Ao (the origin of frame A) to Bo (the origin of frame B). The position vector is expressed in frame A. The monogram notation for the transform relating frame A to B is X_AB
. The monogram notation for the rotation matrix relating A to B is R_AB
. The monogram notation for the position vector from Ao to Bo is p_AoBo_A
. See Multibody Quantities for monogram notation for dynamics.
X_AB * X_BC
, but not X_AB * X_CB
.T  The scalar type, which must be one of the default scalars. 
#include <drake/math/rigid_transform.h>
Public Member Functions  
RigidTransform ()  
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. More...  
RigidTransform (const RotationMatrix< T > &R, const Vector3< T > &p)  
Constructs a RigidTransform from a rotation matrix and a position vector. More...  
RigidTransform (const RollPitchYaw< T > &rpy, const Vector3< T > &p)  
Constructs a RigidTransform from a RollPitchYaw and a position vector. More...  
RigidTransform (const Eigen::Quaternion< T > &quaternion, const Vector3< T > &p)  
Constructs a RigidTransform from a Quaternion and a position vector. More...  
RigidTransform (const Eigen::AngleAxis< T > &theta_lambda, const Vector3< T > &p)  
Constructs a RigidTransform from an AngleAxis and a position vector. More...  
RigidTransform (const RotationMatrix< T > &R)  
Constructs a RigidTransform with a given RotationMatrix and a zero position vector. More...  
RigidTransform (const Vector3< T > &p)  
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector p . More...  
RigidTransform (const Eigen::Translation< T, 3 > &translation)  
Constructs a RigidTransform that contains an identity RotationMatrix and the position vector underlying the given translation . More...  
RigidTransform (const Isometry3< T > &pose)  
Constructs a RigidTransform from an Eigen Isometry3. More...  
RigidTransform (const Eigen::Matrix< T, 3, 4 > pose)  
Constructs a RigidTransform from a 3x4 matrix whose structure is below. More...  
RigidTransform (const Matrix4< T > &pose)  
Constructs a RigidTransform from a 4x4 matrix whose structure is below. More...  
template<typename Derived >  
RigidTransform (const Eigen::MatrixBase< Derived > &pose)  
Constructs a RigidTransform from an appropriate Eigen expression. More...  
void  set (const RotationMatrix< T > &R, const Vector3< T > &p) 
Sets this RigidTransform from a RotationMatrix and a position vector. More...  
void  SetFromIsometry3 (const Isometry3< T > &pose) 
Sets this RigidTransform from an Eigen Isometry3. More...  
template<typename U >  
RigidTransform< U >  cast () const 
Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. More...  
const RotationMatrix< T > &  rotation () const 
Returns R_AB, the rotation matrix portion of this RigidTransform. More...  
void  set_rotation (const RotationMatrix< T > &R) 
Sets the RotationMatrix portion of this RigidTransform. More...  
void  set_rotation (const RollPitchYaw< T > &rpy) 
Sets the rotation part of this RigidTransform from a RollPitchYaw. More...  
void  set_rotation (const Eigen::Quaternion< T > &quaternion) 
Sets the rotation part of this RigidTransform from a Quaternion. More...  
void  set_rotation (const Eigen::AngleAxis< T > &theta_lambda) 
Sets the rotation part of this RigidTransform from an AngleAxis. More...  
const Vector3< T > &  translation () const 
Returns p_AoBo_A , the position vector portion of this RigidTransform, i.e., position vector from Ao (frame A's origin) to Bo (frame B's origin). More...  
void  set_translation (const Vector3< T > &p) 
Sets the position vector portion of this RigidTransform. More...  
Matrix4< T >  GetAsMatrix4 () const 
Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB. More...  
Eigen::Matrix< T, 3, 4 >  GetAsMatrix34 () const 
Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB. More...  
Isometry3< T >  GetAsIsometry3 () const 
Returns the isometry in ℜ³ that is equivalent to a RigidTransform. More...  
const RigidTransform< T > &  SetIdentity () 
Sets this RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. More...  
boolean< T >  IsExactlyIdentity () const 
Returns true if this is exactly the identity RigidTransform. More...  
boolean< T >  IsIdentityToEpsilon (double translation_tolerance) const 
Return true if this is within tolerance of the identity RigidTransform. More...  
RigidTransform< T >  inverse () const 
Returns X_BA = X_AB⁻¹, the inverse of this RigidTransform. More...  
RigidTransform< T > &  operator *= (const RigidTransform< T > &other) 
Inplace multiply of this RigidTransform X_AB by other RigidTransform X_BC . More...  
RigidTransform< T >  operator * (const RigidTransform< T > &other) const 
Multiplies this RigidTransform X_AB by the other RigidTransform X_BC and returns the RigidTransform X_AC = X_AB * X_BC . More...  
RigidTransform< T >  InvertAndCompose (const RigidTransform< T > &other) const 
Calculates the product of this inverted and another RigidTransform. More...  
RigidTransform< T >  operator * (const Eigen::Translation< T, 3 > &X_BBq) const 
Multiplies this RigidTransform X_AB by the translationonly transform X_BBq and returns the RigidTransform X_ABq = X_AB * X_BBq . More...  
Vector3< T >  operator * (const Vector3< T > &p_BoQ_B) const 
Multiplies this RigidTransform X_AB by the position vector p_BoQ_B which is from Bo (B's origin) to an arbitrary point Q. More...  
template<typename Derived >  
Eigen::Matrix< typename Derived::Scalar, 3, Derived::ColsAtCompileTime >  operator * (const Eigen::MatrixBase< Derived > &p_BoQ_B) const 
Multiplies this RigidTransform X_AB by the n position vectors p_BoQ1_B ... More...  
boolean< T >  IsNearlyEqualTo (const RigidTransform< 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 >  IsExactlyEqualTo (const RigidTransform< T > &other) const 
Returns true if this is exactly equal to other . More...  
T  GetMaximumAbsoluteDifference (const RigidTransform< T > &other) const 
Computes the infinity norm of this  other (i.e., the maximum absolute value of the difference between the elements of this and other ). More...  
T  GetMaximumAbsoluteTranslationDifference (const RigidTransform< T > &other) const 
Returns the maximum absolute value of the difference in the position vectors (translation) in this and other . More...  
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable  
RigidTransform (const RigidTransform &)=default  
RigidTransform &  operator= (const RigidTransform &)=default 
RigidTransform (RigidTransform &&)=default  
RigidTransform &  operator= (RigidTransform &&)=default 
Static Public Member Functions  
static const RigidTransform< T > &  Identity () 
Returns the identity RigidTransform (corresponds to coincident frames). More...  
Friends  
template<typename U >  
class  RigidTransform 
RigidTransform< T >  operator * (const Eigen::Translation< T, 3 > &X_AAq, const RigidTransform< T > &X_AqB) 
Multiplies the translationonly transform X_AAq by the RigidTransform X_AqB and returns the RigidTransform X_AB = X_AAq * X_AqB . More...  
Related Functions  
(Note that these are not member functions.)  
using  RigidTransformd = RigidTransform< double > 
Abbreviation (alias/typedef) for a RigidTransform double scalar type. More...  
template<typename T >  
std::ostream &  operator<< (std::ostream &out, const RigidTransform< T > &X) 
Stream insertion operator to write an instance of RigidTransform into a std::ostream . More...  

default 

default 
RigidTransform  (  ) 
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo.
Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
RigidTransform  (  const RotationMatrix< T > &  R, 
const Vector3< T > &  p  
) 
Constructs a RigidTransform from a rotation matrix and a position vector.
[in]  R  rotation matrix relating frames A and B (e.g., R_AB ). 
[in]  p  position vector from frame A's origin to frame B's origin, expressed in frame A. In monogram notation p is denoted p_AoBo_A . 
RigidTransform  (  const RollPitchYaw< T > &  rpy, 
const Vector3< T > &  p  
) 
Constructs a RigidTransform from a RollPitchYaw and a position vector.
[in]  rpy  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation with "rollpitchyaw" angles [r, p, y] or equivalently a Body fixed (intrinsic) ZYX rotation with "yawpitchroll" angles [y, p, r] . 
[in]  p  position vector from frame A's origin to frame B's origin, expressed in frame A. In monogram notation p is denoted p_AoBo_A . 
RigidTransform  (  const Eigen::Quaternion< T > &  quaternion, 
const Vector3< T > &  p  
) 
Constructs a RigidTransform from a Quaternion and a position vector.
[in]  quaternion  a nonzero, finite quaternion which may or may not have unit length [i.e., quaternion.norm() does not have to be 1]. 
[in]  p  position vector from frame A's origin to frame B's origin, expressed in frame A. In monogram notation p is denoted p_AoBo_A . 
std::exception  in debug builds if the rotation matrix that is built from quaternion is invalid. 
RigidTransform  (  const Eigen::AngleAxis< T > &  theta_lambda, 
const Vector3< T > &  p  
) 
Constructs a RigidTransform from an AngleAxis and a position vector.
[in]  theta_lambda  an Eigen::AngleAxis whose associated axis (vector direction herein called lambda ) is nonzero and finite, but which may or may not have unit length [i.e., lambda.norm() does not have to be 1]. 
[in]  p  position vector from frame A's origin to frame B's origin, expressed in frame A. In monogram notation p is denoted `p_AoBo_A 
std::exception  in debug builds if the rotation matrix that is built from theta_lambda is invalid. 

explicit 
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
[in]  R  rotation matrix relating frames A and B (e.g., R_AB ). 

explicit 
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector p
.
[in]  p  position vector from frame A's origin to frame B's origin, expressed in frame A. In monogram notation p is denoted p_AoBo_A . 
RigidTransform  (  const Eigen::Translation< T, 3 > &  translation  ) 
Constructs a RigidTransform that contains an identity RotationMatrix and the position vector underlying the given translation
.
[in]  translation  translationonly transform that stores p_AoQ_A, the position vector from frame A's origin to a point Q, expressed in frame A. 
X_AAq
relates frame A to a frame Aq whose basis unit vectors are aligned with Ax, Ay, Az and whose origin position is located at point Q.

explicit 
Constructs a RigidTransform from an Eigen Isometry3.
[in]  pose  Isometry3 that contains an allegedly valid rotation matrix R_AB and also contains a position vector p_AoBo_A from frame A's origin to frame B's origin. p_AoBo_A must be expressed in frame A. 
std::exception  in debug builds if R_AB is not a proper orthonormal 3x3 rotation matrix. 
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().

explicit 
Constructs a RigidTransform from a 3x4 matrix whose structure is below.
[in]  pose  3x4 matrix that contains an allegedly valid 3x3 rotation matrix R_AB and also a 3x1 position vector p_AoBo_A (the position vector from frame A's origin to frame B's origin, expressed in frame A). ┌ ┐ │ R_AB p_AoBo_A │ └ ┘ 
std::exception  in debug builds if the R_AB part of pose is not a proper orthonormal 3x3 rotation matrix. 
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().

explicit 
Constructs a RigidTransform from a 4x4 matrix whose structure is below.
[in]  pose  4x4 matrix that contains an allegedly valid 3x3 rotation matrix R_AB and also a 3x1 position vector p_AoBo_A (the position vector from frame A's origin to frame B's origin, expressed in frame A). ┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘ 
std::exception  in debug builds if the R_AB part of pose is not a proper orthonormal 3x3 rotation matrix or if pose is a 4x4 matrix whose final row is not [0, 0, 0, 1] . 
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().

explicit 
Constructs a RigidTransform from an appropriate Eigen expression.
[in]  pose  Generic Eigen matrix expression. 
std::exception  if the Eigen expression in pose does not resolve to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part of pose is not a proper orthonormal 3x3 rotation matrix or if pose is a 4x4 matrix whose final row is not [0, 0, 0, 1] . 
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). RigidTransform<U> cast  (  )  const 
Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T.
For example,
U  Scalar type on which the returned RigidTransform is templated. 
RigidTransform<From>::cast<To>()
creates a new RigidTransform<To>
from a RigidTransform<From>
but only if type To
is constructible from type From
. This cast method works in accordance with Eigen's cast method for Eigen's objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Isometry3<T> GetAsIsometry3  (  )  const 
Returns the isometry in ℜ³ that is equivalent to a RigidTransform.
Eigen::Matrix<T, 3, 4> GetAsMatrix34  (  )  const 
Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ └ ┘
Matrix4<T> GetAsMatrix4  (  )  const 
Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘
T GetMaximumAbsoluteDifference  (  const RigidTransform< T > &  other  )  const 
Computes the infinity norm of this
 other
(i.e., the maximum absolute value of the difference between the elements of this
and other
).
[in]  other  RigidTransform to subtract from this . 
this
 other
‖∞ T GetMaximumAbsoluteTranslationDifference  (  const RigidTransform< T > &  other  )  const 
Returns the maximum absolute value of the difference in the position vectors (translation) in this
and other
.
In other words, returns the infinity norm of the difference in the position vectors.
[in]  other  RigidTransform whose position vector is subtracted from the position vector in this . 

static 
Returns the identity RigidTransform (corresponds to coincident frames).
RigidTransform<T> inverse  (  )  const 
Returns X_BA = X_AB⁻¹, the inverse of this
RigidTransform.
p_BoAo_B_
(position from B's origin Bo to A's origin Ao, expressed in frame B). RigidTransform<T> InvertAndCompose  (  const RigidTransform< T > &  other  )  const 
Calculates the product of this
inverted and another RigidTransform.
If you consider this
to be the transform X_AB, and other
to be X_AC, then this method returns X_BC = X_AB⁻¹ * X_AC. For T==double, this method can be much faster than inverting first and then performing the composition, because it can take advantage of the special structure of a rigid transform to avoid unnecessary memory and floating point operations. On some platforms it can use SIMD instructions for further speedups.
[in]  other  RigidTransform that postmultiplies this inverted. 
X_BC  where X_BC = this⁻¹ * other. 
boolean<T> IsExactlyEqualTo  (  const RigidTransform< T > &  other  )  const 
Returns true if this
is exactly equal to other
.
[in]  other  RigidTransform to compare to this . 
true
if each element of this
is exactly equal to the corresponding element of other
. boolean<T> IsExactlyIdentity  (  )  const 
Returns true
if this
is exactly the identity RigidTransform.
boolean<T> IsIdentityToEpsilon  (  double  translation_tolerance  )  const 
Return true if this
is within tolerance of the identity RigidTransform.
true
if the RotationMatrix portion of this
satisfies RotationMatrix::IsIdentityToInternalTolerance() and if the position vector portion of this
is equal to zero vector within translation_tolerance
. [in]  translation_tolerance  a nonnegative number. One way to choose translation_tolerance is to multiply a characteristic length (e.g., the magnitude of a characteristic position vector) by an epsilon (e.g., RotationMatrix::get_internal_tolerance_for_orthonormality()). 
boolean<T> IsNearlyEqualTo  (  const RigidTransform< 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  RigidTransform to compare to this . 
[in]  tolerance  maximum allowable absolute difference between the elements in this and other . 
true
if ‖this.matrix()  other.matrix()‖∞ <= tolerance
. this
position vector and other
position vectors, respectively. RigidTransform<T> operator *  (  const RigidTransform< T > &  other  )  const 
Multiplies this
RigidTransform X_AB
by the other
RigidTransform X_BC
and returns the RigidTransform X_AC = X_AB * X_BC
.
RigidTransform<T> operator *  (  const Eigen::Translation< T, 3 > &  X_BBq  )  const 
Multiplies this
RigidTransform X_AB
by the translationonly transform X_BBq
and returns the RigidTransform X_ABq = X_AB * X_BBq
.
X_ABq
is equal to the rotation matrix in X_AB
. X_ABq
and X_AB
only differ by origin location. Multiplies this
RigidTransform X_AB
by the position vector p_BoQ_B
which is from Bo (B's origin) to an arbitrary point Q.
[in]  p_BoQ_B  position vector from Bo to Q, expressed in frame B. 
p_AoQ_A  position vector from Ao to Q, expressed in frame A. 
Eigen::Matrix<typename Derived::Scalar, 3, Derived::ColsAtCompileTime> operator *  (  const Eigen::MatrixBase< Derived > &  p_BoQ_B  )  const 
Multiplies this
RigidTransform X_AB
by the n position vectors p_BoQ1_B
...
p_BoQn_B
, where p_BoQi_B
is the iᵗʰ position vector from Bo (frame B's origin) to an arbitrary point Qi, expressed in frame B.
[in]  p_BoQ_B  3 x n matrix with n position vectors p_BoQi_B or an expression that resolves to a 3 x n matrix of position vectors. 
p_AoQ_A  3 x n matrix with n position vectors p_AoQi_A , i.e., n position vectors from Ao (frame A's origin) to Qi, expressed in frame A. Specifically, this operator* is defined so that X_AB * p_BoQ_B returns p_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B , where p_AoBo_A is the position vector from Ao to Bo expressed in A and R_AB is the rotation matrix relating the orientation of frames A and B. 
p = p_AoBo_A
, q = p_BoQ_B
and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7). RigidTransform<T>& operator *=  (  const RigidTransform< T > &  other  ) 
Inplace multiply of this
RigidTransform X_AB
by other
RigidTransform X_BC
.
[in]  other  RigidTransform that postmultiplies this . 
this
RigidTransform which has been multiplied by other
. On return, this = X_AC
, where X_AC = X_AB * X_BC
.

default 

default 
const RotationMatrix<T>& rotation  (  )  const 
Returns R_AB, the rotation matrix portion of this
RigidTransform.
R_AB  the rotation matrix portion of this RigidTransform. 
void set  (  const RotationMatrix< T > &  R, 
const Vector3< T > &  p  
) 
Sets this
RigidTransform from a RotationMatrix and a position vector.
[in]  R  rotation matrix relating frames A and B (e.g., R_AB ). 
[in]  p  position vector from frame A's origin to frame B's origin, expressed in frame A. In monogram notation p is denoted p_AoBo_A . 
void set_rotation  (  const RotationMatrix< T > &  R  ) 
Sets the RotationMatrix portion of this
RigidTransform.
[in]  R  rotation matrix relating frames A and B (e.g., R_AB ). 
void set_rotation  (  const RollPitchYaw< T > &  rpy  ) 
Sets the rotation part of this
RigidTransform from a RollPitchYaw.
[in]  rpy  "rollpitchyaw" angles. 
void set_rotation  (  const Eigen::Quaternion< T > &  quaternion  ) 
Sets the rotation part of this
RigidTransform from a Quaternion.
[in]  quaternion  a quaternion which may or may not have unit length. 
void set_rotation  (  const Eigen::AngleAxis< T > &  theta_lambda  ) 
Sets the rotation part of this
RigidTransform from an AngleAxis.
[in]  theta_lambda  an angle theta (in radians) and vector lambda . 
void set_translation  (  const Vector3< T > &  p  ) 
Sets the position vector portion of this
RigidTransform.
[in]  p  position vector from Ao (frame A's origin) to Bo (frame B's origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A. 
void SetFromIsometry3  (  const Isometry3< T > &  pose  ) 
Sets this
RigidTransform from an Eigen Isometry3.
[in]  pose  Isometry3 that contains an allegedly valid rotation matrix R_AB and also contains a position vector p_AoBo_A from frame A's origin to frame B's origin. p_AoBo_A must be expressed in frame A. 
std::exception  in debug builds if R_AB is not a proper orthonormal 3x3 rotation matrix. 
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). const RigidTransform<T>& SetIdentity  (  ) 
Sets this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo.
Hence, this
RigidTransform contains a 3x3 identity matrix and a zero position vector.
const Vector3<T>& translation  (  )  const 
Returns p_AoBo_A
, the position vector portion of this
RigidTransform, i.e., position vector from Ao (frame A's origin) to Bo (frame B's origin).

friend 
Multiplies the translationonly transform X_AAq
by the RigidTransform X_AqB
and returns the RigidTransform X_AB = X_AAq * X_AqB
.
X_AB
is equal to the rotation matrix in X_AqB
. X_AB
and X_AqB
only differ by origin location.

related 
Stream insertion operator to write an instance of RigidTransform into a std::ostream
.
Especially useful for debugging.

friend 

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