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

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

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

◆ 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= ( const RigidTransform< T > & )

default

◆ operator=() [2/2]

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

default

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

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

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

◆ cast()

◆ GetAsIsometry3()

◆ GetAsMatrix34()

◆ GetAsMatrix4()

◆ GetMaximumAbsoluteDifference()

◆ GetMaximumAbsoluteTranslationDifference()

◆ Identity()

◆ inverse()

◆ InvertAndCompose()

◆ IsExactlyEqualTo()

◆ IsExactlyIdentity()

◆ IsNearlyEqualTo()

◆ IsNearlyIdentity()

◆ operator *() [1/5]

◆ operator *() [2/5]

◆ operator *() [3/5]

◆ operator *() [4/5]

◆ operator *() [5/5]

◆ operator *=()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ rotation()

◆ set()

◆ set_rotation() [1/4]

◆ set_rotation() [2/4]

◆ set_rotation() [3/4]

◆ set_rotation() [4/4]

◆ set_translation()

◆ SetFromIsometry3()

◆ SetIdentity()

◆ translation()

Friends And Related Function Documentation

◆ hash_append

◆ operator *

◆ operator<<()

◆ RigidTransform

◆ RigidTransformd

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