Detailed Description

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

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 distance-preserving 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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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.

Note

This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as X_AB * X_BC, but not X_AB * X_CB.

This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().

An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry.

RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation.
In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (non-NAN) position vector. Eigen::Isometry does not.
RigidTransform catches bugs that are undetected by Eigen::Isometry.
RigidTransform has additional functionality and ease-of-use, resulting in shorter, easier to write, and easier to read code.
The name Isometry is unfamiliar to many roboticists and dynamicists and for them Isometry.linear() is (for example) a counter-intuitive method name to return a rotation matrix.

One of the constructors in this class provides an implicit conversion from an Eigen Translation to RigidTransform.

Authors: Paul Mitiguy (2018) Original author.; Drake team (see https://drake.mit.edu/credits).

Template Parameters

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 requires is_default_scalar< U >
	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 >	IsNearlyIdentity (double translation_tolerance) const
	Returns true if `this` is within tolerance of the identity RigidTransform. More...

boolean< T >	IsExactlyEqualTo (const RigidTransform< T > &other) const
	Returns true if `this` is exactly equal to `other`. 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...

RigidTransform< T >	inverse () const
	Returns X_BA = X_AB⁻¹, the inverse of `this` RigidTransform. More...

RigidTransform< T >	InvertAndCompose (const RigidTransform< T > &other) const
	Calculates the product of `this` inverted and another RigidTransform. 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...

RigidTransform< T > &	operator *= (const RigidTransform< T > &other)
	In-place 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 >	operator * (const Eigen::Translation< T, 3 > &X_BBq) const
	Multiplies `this` RigidTransform `X_AB` by the translation-only 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...

Vector4< T >	operator * (const Vector4< T > &vec_B) const
	Multiplies `this` RigidTransform `X_AB` by the 4-element vector `vec_B`, equivalent to `X_AB.GetAsMatrix4() * vec_B`. 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...

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 RigidTransform< T >	MakeUnchecked (const Eigen::Matrix< T, 3, 4 > &pose)
	(Advanced) Constructs a RigidTransform from a 3x4 matrix, without any validity checks nor orthogonalization. More...

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 translation-only transform `X_AAq` by the RigidTransform `X_AqB` and returns the RigidTransform `X_AB = X_AAq * X_AqB`. More...

template<class HashAlgorithm >
void	hash_append (HashAlgorithm &hasher, const RigidTransform &X) noexcept
	Implements the hash_append generic hashing concept. 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...

(Internal use only) methods for specialized transforms
(Internal use only) If we have foreknowledge that a RigidTransform has some particular simplifications, we can exploit that knowledge to reduce the amount of computation required to form it and use it. The simplifications and associated notations for the rigid transform X_BC are: a "rotation transform" that contains a general rotation and zero (identity) translation (rX_BC), a "translation transform" that contains a general translation and an identity rotation (tX_BC), and an "axial rotation transform" that contains an axial rotation (see below) and zero translation (arX_BC), and an "axial translation transform" that contains an axial translation and an identity rotation (atX_BC). See Axial rotations for a definition. Similarly, an axial translation transform is a RigidTransform containing only a translation along one of the basis vectors (coordinate axes) +x, +y, +z. Notation: we use "a" for a general axis but substitute "x", "y", or "z" if we know the particular axis, e.g. zrX_BC (ztX_BC) would be an axial rotation (translation) transform containing a rotation about the z axis. Specialized transforms have the property that of the twelve elements in their matrix representation some are "inactive", meaning that they have known values and never change during a simulation. For best performance, the algorithms in this section are permitted to assume those values without looking. However, those elements are still required to have the expected values, which are the values they would have in an identity transform (that is, 1 on the diagonal and 0 elsewhere). (Even when T is symbolic::Expression, we insist that the inactive elements have the correct numerical values and don't require an Environment for evaluation.) This ensures that general purpose (non-specialized) code can still use the resulting transforms. With general transforms (twelve active elements), transforming a vector takes 18 floating point operations, composing two transforms takes 63, and updating requires writing all twelve elements. The methods in this section take advantage of the specialized structures to reduce the number of operations required. Axial methods are templatized by the axis number 0, 1, or 2 (+x, +y, or +z axis, respectively). The number of operations required is documented with each method. Warning We depend on the caller's promise that specialized transform arguments actually have the indicated specialized structure; for performance reasons we do not verify that in Release builds although we may verify in Debug builds. Note There are no Python bindings for these methods since there is nothing to be gained by using them in Python. Use the general equivalent RigidTransform methods instead.
template<int axis>
void	PostMultiplyByAxialRotation (const RigidTransform< T > &arX_BC, RigidTransform< T > *X_AC) const
	(Internal use only) With `this` a general transform X_AB, and given an axial rotation transform arX_BC, efficiently calculates `X_AC = X_AB * arX_BC`. More...

template<int axis>
void	PreMultiplyByAxialRotation (const RigidTransform< T > &arX_AB, RigidTransform< T > *X_AC) const
	(Internal use only) With `this` a general transform X_BC, and given an axial rotation transform arX_AB, efficiently calculates `X_AC = arX_AB * X_BC`. More...

template<int axis>
void	PostMultiplyByAxialTranslation (const RigidTransform< T > &atX_BC, RigidTransform< T > *X_AC) const
	(Internal use only) Composes `this` general transform X_AB with a given axial translation transform atX_BC to efficiently calculate `X_AC = X_AB * atX_BC`. More...

template<int axis>
void	PreMultiplyByAxialTranslation (const RigidTransform< T > &atX_AB, RigidTransform< T > *X_AC) const
	(Internal use only) With `this` a general transform X_BC, and given an axial translation transform atX_AB, efficiently calculates `X_AC = atX_AB * X_BC`. More...

void	PostMultiplyByRotation (const RigidTransform< T > &rX_BC, RigidTransform< T > *X_AC) const
	(Internal use only) Composes `this` general transform X_AB with a given rotation-only transform rX_BC to efficiently calculate `X_AC = X_AB * rX_BC`. More...

void	PostMultiplyByTranslation (const RigidTransform< T > &tX_BC, RigidTransform< T > *X_AC) const
	(Internal use only) Composes `this` general transform X_AB with a given translation-only transform tX_BC to efficiently calculate `X_AC = X_AB * tX_BC`. More...

template<int axis>
static RigidTransform< T >	MakeAxialRotation (const T &theta)
	(Internal use only) Creates an axial rotation transform arX_AB consisting of only an axial rotation of `theta` radians about x, y, or z and no translation. More...

template<int axis>
static Vector3< T >	ApplyAxialRotation (const RigidTransform< T > &arX_BC, const Vector3< T > &p_C)
	(Internal use only) Given an axial rotation transform arX_BC (just an axial rotation and no translation), uses it to efficiently re-express a given vector. More...

template<int axis>
static void	UpdateAxialRotation (const T &theta, RigidTransform< T > *arX_BC)
	(Internal use only) Given a new rotation angle θ, updates the axial rotation transform arX_BC to represent the new rotation angle. More...

template<int axis>
static void	UpdateAxialRotation (const T &sin_theta, const T &cos_theta, RigidTransform< T > *arX_BC)
	(Internal use only) Given sin(θ) and cos(θ), where θ is a new rotation angle, updates the axial rotation transform arX_BC to represent the new rotation angle. More...

template<int axis>
static void	UpdateAxialTranslation (const T &distance, RigidTransform< T > *atX_BC)
	(Internal use only) Given a new `distance`, updates the axial translation transform atX_BC to represent the new translation by that amount. More...

Constructor & Destructor Documentation

◆ RigidTransform() [1/14]

RigidTransform ( const RigidTransform< T > & )

default

◆ RigidTransform() [2/14]

RigidTransform ( RigidTransform< T > && )

default

◆ RigidTransform() [3/14]

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() [4/14]

RigidTransform	(	const RotationMatrix< T > &	R,
		const Vector3< T > &	p
	)

Constructs a RigidTransform from a rotation matrix and a position vector.

Parameters

[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() [5/14]

RigidTransform	(	const RollPitchYaw< T > &	rpy,
		const Vector3< T > &	p
	)

Constructs a RigidTransform from a RollPitchYaw and a position vector.

Parameters

[in] rpy a RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z rotation with "roll-pitch-yaw" angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with "yaw-pitch-roll" angles [y, p, r].

See also: RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)

Parameters

[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() [6/14]

RigidTransform	(	const Eigen::Quaternion< T > &	quaternion,
		const Vector3< T > &	p
	)

Constructs a RigidTransform from a Quaternion and a position vector.

Parameters

[in]	quaternion	a non-zero, 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`.

Exceptions

std::exception in debug builds if the rotation matrix that is built from quaternion is invalid.

See also: RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)

◆ RigidTransform() [7/14]

RigidTransform	(	const Eigen::AngleAxis< T > &	theta_lambda,
		const Vector3< T > &	p
	)

Constructs a RigidTransform from an AngleAxis and a position vector.

Parameters

[in]	theta_lambda	an Eigen::AngleAxis whose associated axis (vector direction herein called `lambda`) is non-zero 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

Exceptions

std::exception in debug builds if the rotation matrix that is built from theta_lambda is invalid.

See also: RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)

◆ RigidTransform() [8/14]

RigidTransform ( const RotationMatrix< T > & R )

explicit

Constructs a RigidTransform with a given RotationMatrix and a zero position vector.

Parameters

[in] R rotation matrix relating frames A and B (e.g., R_AB).

◆ RigidTransform() [9/14]

RigidTransform ( const Vector3< T > & p )

explicit

Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector p.

Parameters

[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() [10/14]

RigidTransform ( const Eigen::Translation< T, 3 > & translation )

Constructs a RigidTransform that contains an identity RotationMatrix and the position vector underlying the given translation.

Parameters

[in] translation translation-only transform that stores p_AoQ_A, the position vector from frame A's origin to a point Q, expressed in frame A.

Note: The constructed RigidTransform 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.; This constructor provides an implicit conversion from Translation to RigidTransform.

◆ RigidTransform() [11/14]

RigidTransform ( const Isometry3< T > & pose )

explicit

Constructs a RigidTransform from an Eigen Isometry3.

Parameters

[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.

Exceptions

std::exception in debug builds if R_AB is not a proper orthonormal 3x3 rotation matrix.

Note: No attempt is made to orthogonalize the 3x3 rotation matrix part of pose. As needed, use RotationMatrix::ProjectToRotationMatrix().

◆ RigidTransform() [12/14]

RigidTransform ( const Eigen::Matrix< T, 3, 4 > & pose )

explicit

Constructs a RigidTransform from a 3x4 matrix whose structure is below.

Parameters

[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 │
 └                ┘

Exceptions

std::exception in debug builds if the R_AB part of pose is not a proper orthonormal 3x3 rotation matrix.

Note: No attempt is made to orthogonalize the 3x3 rotation matrix part of pose. As needed, use RotationMatrix::ProjectToRotationMatrix().

◆ RigidTransform() [13/14]

RigidTransform ( const Matrix4< T > & pose )

explicit

Constructs a RigidTransform from a 4x4 matrix whose structure is below.

Parameters

[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     │
 └                ┘

Exceptions

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].

Note: No attempt is made to orthogonalize the 3x3 rotation matrix part of pose. As needed, use RotationMatrix::ProjectToRotationMatrix().

◆ RigidTransform() [14/14]

RigidTransform ( const Eigen::MatrixBase< Derived > & pose )

explicit

Constructs a RigidTransform from an appropriate Eigen expression.

Parameters

[in] pose Generic Eigen matrix expression.

Exceptions

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].

Note: No attempt is made to orthogonalize the 3x3 rotation matrix part of pose. As needed, use RotationMatrix::ProjectToRotationMatrix().; This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
const Vector3<double> position(4, 5, 6);
const RigidTransform<double> X1(3 * position);
----------------------------------------------
const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3));
Eigen::Matrix<double, 3, 4> pose34;
pose34 << R.matrix(), position;
const RigidTransform<double> X2(1.0 * pose34);
----------------------------------------------
Eigen::Matrix<double, 4, 4> pose4;
pose4 << R.matrix(), position,
0, 0, 0, 1;
const RigidTransform<double> X3(pose4 * pose4);

Member Function Documentation

◆ ApplyAxialRotation()

static Vector3<T> ApplyAxialRotation	(	const RigidTransform< T > &	arX_BC,
		const Vector3< T > &	p_C
	)

static

(Internal use only) Given an axial rotation transform arX_BC (just an axial rotation and no translation), uses it to efficiently re-express a given vector.

This takes only 6 floating point operations.

Parameters

[in]	arX_BC	the axial transform to be applied.
[in]	p_C	a position vector expressed in frame C, to be re-expressed in frame B since there is zero translation.

Return values

p_B	the input position vector p_C, now re-expressed in frame B.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

Precondition: arX_BC is an Axial rotation transform.

◆ cast()

RigidTransform<U> cast ( ) const

Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T.

For example,

RigidTransform<double> source = RigidTransform<double>::Identity();

RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();

Template Parameters

U	Scalar type on which the returned RigidTransform is templated.

Note: 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 vice-versa.

◆ GetAsIsometry3()

Isometry3<T> GetAsIsometry3 ( ) const

Returns the isometry in ℜ³ that is equivalent to a RigidTransform.

◆ GetAsMatrix34()

Eigen::Matrix<T, 3, 4> GetAsMatrix34 ( ) const

Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.

 ┌                ┐
 │ R_AB  p_AoBo_A │
 └                ┘

◆ GetAsMatrix4()

Matrix4<T> GetAsMatrix4 ( ) const

Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.

 ┌                ┐
 │ R_AB  p_AoBo_A │
 │                │
 │   0      1     │
 └                ┘

◆ GetMaximumAbsoluteDifference()

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

Parameters

[in] other RigidTransform to subtract from this.

Returns: ‖this - other‖∞

◆ GetMaximumAbsoluteTranslationDifference()

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.

Parameters

[in] other RigidTransform whose position vector is subtracted from the position vector in this.

◆ Identity()

static const RigidTransform<T>& Identity ( )

static

Returns the identity RigidTransform (corresponds to coincident frames).

Returns: 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 returned RigidTransform contains a 3x3 identity matrix and a zero position vector.

◆ inverse()

RigidTransform<T> inverse ( ) const

Returns X_BA = X_AB⁻¹, the inverse of this RigidTransform.

Note: The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector p_BoAo_B_ (position from B's origin Bo to A's origin Ao, expressed in frame B).; : The square-root of a RigidTransform's condition number is roughly the magnitude of the position vector. The accuracy of the calculation for the inverse of a RigidTransform drops off with the sqrt condition number.

◆ InvertAndCompose()

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.

Parameters

[in] other RigidTransform that post-multiplies this inverted.

Return values

X_BC	where X_BC = this⁻¹ * other.

Note: It is possible (albeit improbable) to create an invalid rigid transform by accumulating round-off error with a large number of multiplies.

◆ IsExactlyEqualTo()

boolean<T> IsExactlyEqualTo ( const RigidTransform< T > & other ) const

Returns true if this is exactly equal to other.

Parameters

[in] other RigidTransform to compare to this.

Returns: true if each element of this is exactly equal to the corresponding element of other.

◆ IsExactlyIdentity()

boolean<T> IsExactlyIdentity ( ) const

Returns true if this is exactly the identity RigidTransform.

See also: IsNearlyIdentity().

◆ IsNearlyEqualTo()

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.

Parameters

[in]	other	RigidTransform to compare to `this`.
[in]	tolerance	maximum allowable absolute difference between the elements in `this` and `other`.

Returns: true if ‖this.matrix() - other.matrix()‖∞ <= tolerance.

Note: Consider scaling tolerance with the largest of magA and magB, where magA and magB denoted the magnitudes of this position vector and other position vectors, respectively.

◆ IsNearlyIdentity()

boolean<T> IsNearlyIdentity ( double translation_tolerance ) const

Returns true if this is within tolerance of the identity RigidTransform.

Parameters

[in] translation_tolerance a non-negative 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()).

Returns: true if the RotationMatrix portion of this satisfies RotationMatrix::IsNearlyIdentity() and if the position vector portion of this is equal to zero vector within translation_tolerance.

See also: IsExactlyIdentity().

◆ MakeAxialRotation()

static RigidTransform<T> MakeAxialRotation ( const T & theta )

static

(Internal use only) Creates an axial rotation transform arX_AB consisting of only an axial rotation of theta radians about x, y, or z and no translation.

Of the 12 entries in the transform matrix, only 4 are active; the rest will be set to 0 or 1. This structure can be exploited for efficient updating and operating with this transform.

Parameters

[in] theta the rotation angle.

Return values

arX_BC the axial transform (also known as X_BC(theta)).

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

See also: Specialized transforms.

◆ MakeUnchecked()

static RigidTransform<T> MakeUnchecked ( const Eigen::Matrix< T, 3, 4 > & pose )

static

(Advanced) Constructs a RigidTransform from a 3x4 matrix, without any validity checks nor orthogonalization.

Parameters

[in]

pose

3x4 matrix that contains a 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 │
 └                ┘

◆ operator *() [1/5]

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.

◆ operator *() [2/5]

RigidTransform<T> operator * ( const Eigen::Translation< T, 3 > & X_BBq ) const

Multiplies this RigidTransform X_AB by the translation-only transform X_BBq and returns the RigidTransform X_ABq = X_AB * X_BBq.

Note: The rotation matrix in the returned RigidTransform X_ABq is equal to the rotation matrix in X_AB. X_ABq and X_AB only differ by origin location.

◆ operator *() [3/5]

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.

Parameters

[in] p_BoQ_B position vector from Bo to Q, expressed in frame B.

Return values

p_AoQ_A position vector from Ao to Q, expressed in frame A.

◆ operator *() [4/5]

Vector4<T> operator * ( const Vector4< T > & vec_B ) const

Multiplies this RigidTransform X_AB by the 4-element vector vec_B, equivalent to X_AB.GetAsMatrix4() * vec_B.

Parameters

[in] vec_B 4-element vector whose first 3 elements are the position vector p_BoQ_B from Bo (frame B's origin) to an arbitrary point Q, expressed in frame B and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo or Q) and whose 4ᵗʰ element is 0.

Return values

vec_A 4-element vector whose first 3 elements are the position vector p_AoQ_A from Ao (frame A's origin) to Q, expressed in frame A and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo and Q) and whose 4ᵗʰ element is 0.

Exceptions

std::exception if the 4ᵗʰ element of vec_B is not 0 or 1.

◆ operator *() [5/5]

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.

Parameters

[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.

Return values

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.

Note: As needed, use parentheses. This operator* is not associative. To see this, let 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).
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3);
const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3));
Eigen::Matrix<double, 3, 2> p_BoQ_B;
p_BoQ_B.col(0) = Vector3d(4, 5, 6);
p_BoQ_B.col(1) = Vector3d(9, 8, 7);
const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;

◆ operator *=()

RigidTransform<T>& operator *= ( const RigidTransform< T > & other )

In-place multiply of this RigidTransform X_AB by other RigidTransform X_BC.

Parameters

[in] other RigidTransform that post-multiplies this.

Returns: this RigidTransform which has been multiplied by other. On return, this = X_AC, where X_AC = X_AB * X_BC.

◆ operator=() [1/2]

RigidTransform& operator= ( RigidTransform< T > && )

default

◆ operator=() [2/2]

RigidTransform& operator= ( const RigidTransform< T > & )

default

◆ PostMultiplyByAxialRotation()

void PostMultiplyByAxialRotation	(	const RigidTransform< T > &	arX_BC,
		RigidTransform< T > *	X_AC
	)		const

(Internal use only) With this a general transform X_AB, and given an axial rotation transform arX_BC, efficiently calculates X_AC = X_AB * arX_BC.

This requires only 18 floating point operations.

Parameters

[in]	arX_BC	An axial rotation transform about the indicated `axis`.
[out]	X_AC	Preallocated space for the result, which will be a general transform. Must not overlap with `this` in memory.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

Precondition: arX_BC is an Axial rotation transform; X_AC does not overlap with this in memory.

◆ PostMultiplyByAxialTranslation()

void PostMultiplyByAxialTranslation	(	const RigidTransform< T > &	atX_BC,
		RigidTransform< T > *	X_AC
	)		const

(Internal use only) Composes this general transform X_AB with a given axial translation transform atX_BC to efficiently calculate X_AC = X_AB * atX_BC.

This requires only 6 floating point operations.

Parameters

[in]	atX_BC	An axial translation transform about the indicated `axis`.
[out]	X_AC	Preallocated space for the result, which will be a general transform.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

Precondition: atX_BC is an Axial translation transform.

◆ PostMultiplyByRotation()

void PostMultiplyByRotation	(	const RigidTransform< T > &	rX_BC,
		RigidTransform< T > *	X_AC
	)		const

(Internal use only) Composes this general transform X_AB with a given rotation-only transform rX_BC to efficiently calculate X_AC = X_AB * rX_BC.

This requires 45 floating point operations.

Parameters

[in]	rX_BC	the rotation-only transform.
[out]	X_AC	Preallocated space for the result.

Precondition: rX_BC is a Rotation transform.

◆ PostMultiplyByTranslation()

void PostMultiplyByTranslation	(	const RigidTransform< T > &	tX_BC,
		RigidTransform< T > *	X_AC
	)		const

(Internal use only) Composes this general transform X_AB with a given translation-only transform tX_BC to efficiently calculate X_AC = X_AB * tX_BC.

This requires only 18 floating point operations.

Parameters

[in]	tX_BC	the translation-only transform.
[out]	X_AC	preallocated space for the result.

Precondition: tX_BC is a Translation transform.

◆ PreMultiplyByAxialRotation()

void PreMultiplyByAxialRotation	(	const RigidTransform< T > &	arX_AB,
		RigidTransform< T > *	X_AC
	)		const

(Internal use only) With this a general transform X_BC, and given an axial rotation transform arX_AB, efficiently calculates X_AC = arX_AB * X_BC.

This requires only 24 floating point operations.

Parameters

[in]	arX_AB	An axial rotation transform about the indicated `axis`.
[out]	X_AC	Preallocated space for the result, which will be a general transform. Must not overlap with arX_BC or `this` in memory.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

Precondition: arX_AB is an Axial rotation transform; X_AC does not overlap with arX_AB or this in memory.

◆ PreMultiplyByAxialTranslation()

void PreMultiplyByAxialTranslation	(	const RigidTransform< T > &	atX_AB,
		RigidTransform< T > *	X_AC
	)		const

(Internal use only) With this a general transform X_BC, and given an axial translation transform atX_AB, efficiently calculates X_AC = atX_AB * X_BC.

This requires just 1 floating point operation.

Parameters

[in]	atX_AB	An axial translation transform along the indicated `axis`.
[out]	X_AC	Preallocated space for the result, which will be a general transform.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z translation axis.

Precondition: atX_AB is an Axial translation transform

◆ rotation()

const RotationMatrix<T>& rotation ( ) const

Returns R_AB, the rotation matrix portion of this RigidTransform.

Return values

R_AB	the rotation matrix portion of `this` RigidTransform.

◆ set()

void set	(	const RotationMatrix< T > &	R,
		const Vector3< T > &	p
	)

Sets this RigidTransform from a RotationMatrix and a position vector.

Parameters

[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`.

◆ set_rotation() [1/4]

void set_rotation ( const RotationMatrix< T > & R )

Sets the RotationMatrix portion of this RigidTransform.

Parameters

[in] R rotation matrix relating frames A and B (e.g., R_AB).

◆ set_rotation() [2/4]

void set_rotation ( const RollPitchYaw< T > & rpy )

Sets the rotation part of this RigidTransform from a RollPitchYaw.

Parameters

[in] rpy "roll-pitch-yaw" angles.

See also: RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.

◆ set_rotation() [3/4]

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

Sets the rotation part of this RigidTransform from a Quaternion.

Parameters

[in] quaternion a quaternion which may or may not have unit length.

See also: RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.

◆ set_rotation() [4/4]

void set_rotation ( const Eigen::AngleAxis< T > & theta_lambda )

Sets the rotation part of this RigidTransform from an AngleAxis.

Parameters

[in] theta_lambda an angle theta (in radians) and vector lambda.

See also: RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.

◆ set_translation()

void set_translation ( const Vector3< T > & p )

Sets the position vector portion of this RigidTransform.

Parameters

[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.

◆ SetFromIsometry3()

void SetFromIsometry3 ( const Isometry3< T > & pose )

Sets this RigidTransform from an Eigen Isometry3.

Parameters

[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.

Exceptions

std::exception in debug builds if R_AB is not a proper orthonormal 3x3 rotation matrix.

Note: No attempt is made to orthogonalize the 3x3 rotation matrix part of pose. As needed, use RotationMatrix::ProjectToRotationMatrix().

◆ SetIdentity()

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.

◆ translation()

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

◆ UpdateAxialRotation() [1/2]

static void UpdateAxialRotation	(	const T &	theta,
		RigidTransform< T > *	arX_BC
	)

static

(Internal use only) Given a new rotation angle θ, updates the axial rotation transform arX_BC to represent the new rotation angle.

We expect that arX_BC was already such a transform (about the given x, y, or z axis). Only the 4 active elements are modified; the other 8 remain unchanged.

Parameters

[in]	theta	the new rotation angle in radians.
[in,out]	arX_BC	the axial rotation transform matrix to be updated.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

See also: the overloaded signature if you already have sin(θ) and cos(θ).

Precondition: arX_BC is an Axial rotation transform.

◆ UpdateAxialRotation() [2/2]

static void UpdateAxialRotation	(	const T &	sin_theta,
		const T &	cos_theta,
		RigidTransform< T > *	arX_BC
	)

static

(Internal use only) Given sin(θ) and cos(θ), where θ is a new rotation angle, updates the axial rotation transform arX_BC to represent the new rotation angle.

We expect that arX_BC was already such a transform (about the given x, y, or z axis). Only the 4 active elements are modified; the other 8 remain unchanged.

Parameters

[in]	sin_theta	sin(θ), where θ is the new rotation angle.
[in]	cos_theta	cos(θ), where θ is the new rotation angle.
[in,out]	arX_BC	the axial rotation transform matrix to be updated.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z rotation axis.

See also: the overloaded signature if you just have the angle θ.

Precondition: arX_BC is an Axial rotation transform.; sin_theta and cos_theta are sine and cosine of the same angle.

◆ UpdateAxialTranslation()

static void UpdateAxialTranslation	(	const T &	distance,
		RigidTransform< T > *	atX_BC
	)

static

(Internal use only) Given a new distance, updates the axial translation transform atX_BC to represent the new translation by that amount.

We expect that atX_BC was already such a transform (about the given x, y, or z axis). Only the 1 active element is modified; the other 11 remain unchanged. No floating point operations are needed.

Parameters

[in]	distance	the component of p_BC along the `axis` direction.
[in,out]	atX_BC	the axial translation transform matrix to be updated.

Template Parameters

axis	0, 1, or 2 corresponding to +x, +y, or +z translation axis.

Precondition: atX_BC is an Axial translation transform.

Friends And Related Function Documentation

◆ hash_append

void hash_append	(	HashAlgorithm &	hasher,
		const RigidTransform< T > &	X
	)

friend

Implements the hash_append generic hashing concept.

Precondition: T implements the hash_append concept.

◆ operator *

RigidTransform<T> operator *	(	const Eigen::Translation< T, 3 > &	X_AAq,
		const RigidTransform< T > &	X_AqB
	)

friend

Multiplies the translation-only transform X_AAq by the RigidTransform X_AqB and returns the RigidTransform X_AB = X_AAq * X_AqB.

Note: The rotation matrix in the returned RigidTransform X_AB is equal to the rotation matrix in X_AqB. X_AB and X_AqB only differ by origin location.

◆ operator<<()

std::ostream & operator<<	(	std::ostream &	out,
		const RigidTransform< T > &	X
	)

Especially useful for debugging.

◆ RigidTransform

friend class RigidTransform

friend

◆ RigidTransformd

using RigidTransformd = RigidTransform<double>

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

drake/math/rigid_transform.h

Detailed Description

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

Public Member Functions

Static Public Member Functions

Friends

Related Functions

(Internal use only) methods for specialized transforms

Constructor & Destructor Documentation

◆ RigidTransform() [1/14]

◆ RigidTransform() [2/14]

◆ RigidTransform() [3/14]

◆ RigidTransform() [4/14]

◆ RigidTransform() [5/14]

◆ RigidTransform() [6/14]

◆ RigidTransform() [7/14]

◆ RigidTransform() [8/14]

◆ RigidTransform() [9/14]

◆ RigidTransform() [10/14]

◆ RigidTransform() [11/14]

◆ RigidTransform() [12/14]

◆ RigidTransform() [13/14]

◆ RigidTransform() [14/14]

Member Function Documentation

◆ ApplyAxialRotation()

◆ cast()

◆ GetAsIsometry3()

◆ GetAsMatrix34()

◆ GetAsMatrix4()

◆ GetMaximumAbsoluteDifference()

◆ GetMaximumAbsoluteTranslationDifference()

◆ Identity()

◆ inverse()

◆ InvertAndCompose()

◆ IsExactlyEqualTo()

◆ IsExactlyIdentity()

◆ IsNearlyEqualTo()

◆ IsNearlyIdentity()

◆ MakeAxialRotation()

◆ MakeUnchecked()

◆ operator *() [1/5]

◆ operator *() [2/5]

◆ operator *() [3/5]

◆ operator *() [4/5]

◆ operator *() [5/5]

◆ operator *=()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ PostMultiplyByAxialRotation()

◆ PostMultiplyByAxialTranslation()

◆ PostMultiplyByRotation()

◆ PostMultiplyByTranslation()

◆ PreMultiplyByAxialRotation()

◆ PreMultiplyByAxialTranslation()

◆ rotation()

◆ set()

◆ set_rotation() [1/4]

◆ set_rotation() [2/4]

◆ set_rotation() [3/4]

◆ set_rotation() [4/4]

◆ set_translation()

◆ SetFromIsometry3()

◆ SetIdentity()

◆ translation()

◆ UpdateAxialRotation() [1/2]

◆ UpdateAxialRotation() [2/2]

◆ UpdateAxialTranslation()

Friends And Related Function Documentation

◆ hash_append

◆ operator *

◆ operator<<()

◆ RigidTransform

◆ RigidTransformd

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