iDynTree::Transform class
#include <iDynTree/Transform.h>

Class representation the relative displacement between two different frames.

Image

This class is designed to be an easy to use proxy to perform change of frame of expression for iDynTree::Position, iDynTree::Twist, iDynTree::Wrench and other data structure in iDynTree core data structures. For this reason the class is called "Transform", because it will be mainly used to transform quantities between frames.

Given that this class it may used to represent homogeneous transform matrix as well as adjoint matrix, no raw access to the underline storage ( data() method ) is provided, because it does not have a canonical representation. Instead, access is given to the two elements of a transform: The position vector ${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}$ and the rotation matrix ${}^{\texttt{refOrient}} R_{\texttt{orient}}$ .

We will indicate this tranform as $( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} )$

The frame whose origin is the reference point and whose orientation the reference orientation is often indicated as $ \texttt{refFrame} = (\texttt{refPoint},\texttt{refOrient}) $ .

Similarly the frame whose origin is the point and whose orientation is the orientation is indicated as $ \texttt{frame} = (\texttt{point},\texttt{orient}) $ .

This transform object can be obtained as the ${}^{\texttt{refFrame}} H_{\texttt{frame}}$ 4x4 homogeneous matrix using the asHomogeneousTransform method, or as the ${}^{\texttt{refFrame}} X_{\texttt{frame}}$ 6x6 adjoint matrix using the asAdjointTransform .

Public static functions

static auto compose(const Transform& op1, const Transform& op2) -> Transform
static auto inverse2(const Transform& trans) -> Transform
static auto Identity() -> Transform
Return an identity transfom.

Constructors, destructors, conversion operators

Transform()
Default constructor.
Transform(const Rotation& _rot, const Position& origin)
Constructor from a rotation and a point.
Transform(const Matrix4x4& transform)
Constructor from a Matrix4x4 object.
Transform(const Transform& other)
Copy constructor: create a Transform from another Transform.

Public functions

void fromHomogeneousTransform(const Matrix4x4& transform)
Set rotation and translation from a iDynTree::Matrix4x4 object.
auto operator=(const Transform& other) -> Transform&
Assigment operator.
auto getRotation() const -> const Rotation&
Get the rotation part of the transform.
auto getPosition() const -> const Position&
Get the translation part of the transform.
void setRotation(const Rotation& rotation)
Set the rotation part of the transform.
void setPosition(const Position& position)
Set the translation part of the transform.

Protected variables

Position pos
The position part of the transform.
Rotation rot
The rotation part of the transform.

Overloaded operators.

This operators are used to change the frame in which a quantity is expressed from the $\texttt{frame}$ to the $\texttt{refFrame}$ of the transform.

auto operator*(const Transform& other) const -> Transform
Combine two transforms.
auto inverse() const -> Transform
Return the inverse of this Transform.
auto operator*(const Position& other) const -> Position
Change reference frame of a point.
auto operator*(const Wrench& other) const -> Wrench
Change frame in which a Wrench is expressed.
auto operator*(const Twist& other) const -> Twist
Change the frame in which a Twist is expressed.
auto operator*(const SpatialMomentum& other) const -> SpatialMomentum
Change the frame in which a SpatialMomentum is expressed.
auto operator*(const SpatialAcc& other) const -> SpatialAcc
Change the frame in which a SpatialAcc is expressed.
auto operator*(const SpatialMotionVector& other) const -> SpatialMotionVector
Change the frame in which a SpatialMotionVector is expressed.
auto operator*(const SpatialForceVector& other) const -> SpatialForceVector
Change the frame in which a SpatialForceVector is expressed.
auto operator*(const SpatialInertia& other) const -> SpatialInertia
Change the frame in which a SpatialInertia is expressed.
auto operator*(const ArticulatedBodyInertia& other) const -> ArticulatedBodyInertia
Change the frame in which a ArticulatedBodyInertia is expressed.
auto operator*(const Direction& other) const -> Direction
Change the frame in which a Direction is expressed.
auto operator*(const Axis& other) const -> Axis
Change the frame in which a Axis is expressed.

Raw data accessors.

For several applications it may be useful to access the fransform contents in the raw form of homogeneous matrix or an adjoint matrix.

auto asHomogeneousTransform() const -> Matrix4x4
Return the 4x4 homogeneous matrix representing the transform.
auto asAdjointTransform() const -> Matrix6x6
Return the 6x6 adjoint matrix representing this transform.
auto asAdjointTransformWrench() const -> Matrix6x6
Return the 6x6 adjoint matrix (for wrench) representing this transform.
auto log() const -> SpatialMotionVector

Output helpers.

Function to print out the Transform.

auto toString() const -> std::string
auto reservedToString() const -> std::string

Function documentation

static Transform iDynTree::Transform::Identity()

Return an identity transfom.

Returns an identity transform.

Set the rotation to the identity and the translation to 0 :

\[ (0_{3 \times 1}, I_{3 \times 3}) \]

iDynTree::Transform::Transform()

Default constructor.

The data is not reset to the default for perfomance reason. Please initialize the data in the class before any use.

iDynTree::Transform::Transform(const Matrix4x4& transform)

Constructor from a Matrix4x4 object.

Parameters
transform The input homogeneous matrix

It is equivalent of calling fromHomogeneousTransform()

void iDynTree::Transform::fromHomogeneousTransform(const Matrix4x4& transform)

Set rotation and translation from a iDynTree::Matrix4x4 object.

Parameters
transform The input homogeneous matrix

Transform& iDynTree::Transform::operator=(const Transform& other)

Assigment operator.

Parameters
other the rhs
Returns *this equal to the other object

const Rotation& iDynTree::Transform::getRotation() const

Get the rotation part of the transform.

Get the rotation matrix ${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}$ , that is the rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in a 3d vector expressed in the reference orientation frame.

const Position& iDynTree::Transform::getPosition() const

Get the translation part of the transform.

Get 3d vector ${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}$ , that is the vector between the point and the reference point, pointing toward the point and expressed in the reference orientation frame.

void iDynTree::Transform::setRotation(const Rotation& rotation)

Set the rotation part of the transform.

Set the rotation matrix ${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}$ , that is the rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in a 3d vector expressed in the reference orientation frame.

void iDynTree::Transform::setPosition(const Position& position)

Set the translation part of the transform.

Set 3d vector ${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}$ , that is the vector between the point and the reference point, pointing toward the point and expressed in the reference orientation frame.

Transform iDynTree::Transform::operator*(const Transform& other) const

Combine two transforms.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

and the argument is

\[ (p',R') = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} , {}^{\texttt{orient}} R_{\texttt{newOrient}} ) \]

.

The resulting transform will be:

\[ (p+Rp', RR') = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} , {}^{\texttt{refOrient}} R_{\texttt{newOrient}} ) \]

.

Notice that this is equivalent to multiply the associated homogemeous matrices:

\[ {}^{\texttt{refFrame}} H_{\texttt{newFrame}} = {}^{\texttt{refFrame}} H_{\texttt{frame}} {}^{\texttt{frame}} H_{\texttt{newFrame}} \]

or the associated adjoint matrices :

\[ {}^{\texttt{refFrame}} X_{\texttt{newFrame}} = {}^{\texttt{refFrame}} X_{\texttt{frame}} {}^{\texttt{frame}} X_{\texttt{newFrame}} \]

Transform iDynTree::Transform::inverse() const

Return the inverse of this Transform.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

this function will return:

\[ (-R^\top p , R^\top) = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{refPoint}} , {}^{\texttt{orient}} R_{\texttt{refOrient}} ) \]

Position iDynTree::Transform::operator*(const Position& other) const

Change reference frame of a point.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

And the Position argument represent a point:

\[ p' = {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} \]

The result of the operation is:

\[ Rp' + p = {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} \]

Wrench iDynTree::Transform::operator*(const Wrench& other) const

Change frame in which a Wrench is expressed.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

And the argument is a wrench:

\[ {}_{\texttt{frame}} F = \begin{bmatrix} f \\ \tau \end{bmatrix} \]

The result of this operation is :

\[ {}_{\texttt{refFrame}} F = {}_{\texttt{refFrame}}X^{\texttt{frame}} {}_{\texttt{frame}} F = \begin{bmatrix} R & 0_{3\times3} \\ p \times R & R \end{bmatrix} \begin{bmatrix} f \\ \tau \end{bmatrix} = \begin{bmatrix} Rf \\ p \times R f + R\tau \end{bmatrix} \]

Twist iDynTree::Transform::operator*(const Twist& other) const

Change the frame in which a Twist is expressed.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

And the argument is a twist:

\[ {}^{\texttt{frame}} V = \begin{bmatrix} v \\ \omega \end{bmatrix} \]

The result of this operation is :

\[ {}^{\texttt{refFrame}} V = {}^{\texttt{refFrame}}X_{\texttt{frame}} {}^{\texttt{frame}} V = \begin{bmatrix} R & p \times R\\ 0_{3\times3} & R \end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix} = \begin{bmatrix} R v + p \times R \omega \\ R\omega \end{bmatrix} \]

SpatialMomentum iDynTree::Transform::operator*(const SpatialMomentum& other) const

Change the frame in which a SpatialMomentum is expressed.

Check the operator*(const Wrench & other) documentation for the mathematical details.

SpatialAcc iDynTree::Transform::operator*(const SpatialAcc& other) const

Change the frame in which a SpatialAcc is expressed.

Check the operator*(const Twist & other) documentation for the mathematical details.

SpatialMotionVector iDynTree::Transform::operator*(const SpatialMotionVector& other) const

Change the frame in which a SpatialMotionVector is expressed.

Check the operator*(const Twist & other) documentation for the mathematical details.

SpatialForceVector iDynTree::Transform::operator*(const SpatialForceVector& other) const

Change the frame in which a SpatialForceVector is expressed.

Check the operator*(const Wrench & other) documentation for the mathematical details.

Direction iDynTree::Transform::operator*(const Direction& other) const

Change the frame in which a Direction is expressed.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

And the argument is a direction represented by a unit norm 3d vector :

\[ {}^{\texttt{orient}} d \]

The result of this operation is:

\[ {}^{\texttt{refOrient}} d = R {}^{\texttt{orient}} d \]

Axis iDynTree::Transform::operator*(const Axis& other) const

Change the frame in which a Axis is expressed.

If this object is

\[ (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) \]

And the argument is an axis, specified by the axis origin and the axis direction:

\[ {}^{\texttt{frame}} A = ({}^{\texttt{orient}} p_{\texttt{point},\texttt{axisOrigin}} , {}^{\texttt{orient}} d) = (p',d) \]

The result of this operation is:

\[ {}^{\texttt{refFrame}} A = ({}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{axisOrigin}} , {}^{\texttt{refOrient}} d) = ( Rp' + p , Rd) \]

Matrix4x4 iDynTree::Transform::asHomogeneousTransform() const

Return the 4x4 homogeneous matrix representing the transform.

The returned matrix is the one with this structure:

\[ {}^{\texttt{refFrame}} H_{\texttt{frame}} = \begin{bmatrix} {}^{\texttt{refOrient}} R_{\texttt{orient}} & {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \\ 0_{1\times3} & 1 \end{bmatrix} \]

Where ${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}$ is the rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in a 3d vector expressed in the reference orientation frame.

${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3$ is the vector between the point and the reference point, pointing toward the point and expressed in the reference orientation frame.

Matrix6x6 iDynTree::Transform::asAdjointTransform() const

Return the 6x6 adjoint matrix representing this transform.

The returned matrix is the one with this structure:

\[ {}^{\texttt{refFrame}} X_{\texttt{frame}} = \begin{bmatrix} R & p \times R \\ 0_{3\times3} & R \end{bmatrix} \]

Where $R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}$ is the rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in a 3d vector expressed in the reference orientation frame.

$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3$ is the vector between the point and the reference point, pointing toward the point and expressed in the reference orientation frame $p \times$ is the skew simmetric matrix such that $(p \times) v = p \times v$ .

Matrix6x6 iDynTree::Transform::asAdjointTransformWrench() const

Return the 6x6 adjoint matrix (for wrench) representing this transform.

The returned matrix is the one with this structure:

\[ {}_{\texttt{refFrame}} X^{\texttt{frame}} = \begin{bmatrix} R & 0_{3\times3} \\ p \times R & R \end{bmatrix} \]

Where $R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}$ is the rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in a 3d vector expressed in the reference orientation frame.

$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3$ is the vector between the point and the reference point, pointing toward the point and expressed in the reference orientation frame $p \times$ is the skew simmetric matrix such that $(p \times) v = p \times v$ .

Variable documentation

Position iDynTree::Transform::pos protected

The position part of the transform.

The 3d vector ${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}$ , that is the vector between the point and the reference point, pointing toward the point and expressed in the reference orientation frame.

Rotation iDynTree::Transform::rot protected

The rotation part of the transform.

Set the rotation matrix ${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}$ , that is the rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in a 3d vector expressed in the reference orientation frame.