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

Class representation the rotation of an orientation frame with respect to a reference orientation frame, expressed as a Rotation matrix.

The semantics for this class is based on the OrientationCoord in:

De Laet T, Bellens S, Smits R, Aertbeliën E, Bruyninckx H, and De Schutter J (2013), Geometric Relations between Rigid Bodies: Semantics for Standardization, IEEE Robotics & Automation Magazine, Vol. 20, No. 1, pp. 84-93. URL : http://people.mech.kuleuven.be/~tdelaet/geometric_relations_semantics/geometric_relations_semantics_theory.pdf

Storage for the Orientation:

The rotation matrix representation of the orientation, stored in row major order, inside a Matrix3x3 parent object.

Base classes

template<unsigned int nRows, unsigned int nCols>
class MatrixFixSize<3, 3>
Class providing a simple form of matrix with dynamic size.

Public static functions

static auto compose(const Rotation& op1, const Rotation& op2) -> Rotation
static auto inverse2(const Rotation& orient) -> Rotation

Constructors, destructors, conversion operators

Rotation()
Default constructor.
Rotation(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz)
Constructor from 9 doubles: initialize elements of the rotation matrix.
Rotation(const Rotation& other)
Copy constructor: create a Rotation from another Rotation.
Rotation(iDynTree::MatrixView<const double> other)
Create a Rotation from a MatrixView.
Rotation(const double* in_data, const unsigned int in_rows, const unsigned int in_cols)
Constructor from a buffer of 9 doubles, stored as a C-style array (i.e.

Public functions

auto changeOrientFrame(const Rotation& newOrientFrame) -> const Rotation&
Geometric operations.
auto changeRefOrientFrame(const Rotation& newRefOrientFrame) -> const Rotation&
auto changeCoordinateFrame(const Rotation& newCoordinateFrame) -> const Rotation&
auto changeCoordFrameOf(const Position& other) const -> Position
auto changeCoordFrameOf(const SpatialMotionVector& other) const -> SpatialMotionVector
auto changeCoordFrameOf(const SpatialForceVector& other) const -> SpatialForceVector
auto changeCoordFrameOf(const Twist& other) const -> Twist
auto changeCoordFrameOf(const SpatialAcc& other) const -> SpatialAcc
auto changeCoordFrameOf(const SpatialMomentum& other) const -> SpatialMomentum
auto changeCoordFrameOf(const Wrench& other) const -> Wrench
auto changeCoordFrameOf(const Direction& other) const -> Direction
auto changeCoordFrameOf(const Axis& other) const -> Axis
auto changeCoordFrameOf(const ClassicalAcc& other) const -> ClassicalAcc
auto changeCoordFrameOf(const RotationalInertia& other) const -> RotationalInertia
auto operator=(const Rotation& other) -> Rotation&
overloaded operators
auto operator*(const Rotation& other) const -> Rotation
auto inverse() const -> Rotation
auto operator*(const Position& other) const -> Position
auto operator*(const SpatialForceVector& other) const -> SpatialForceVector
auto operator*(const Twist& other) const -> Twist
auto operator*(const Wrench& other) const -> Wrench
auto operator*(const Direction& other) const -> Direction
auto operator*(const Axis& other) const -> Axis
auto operator*(const SpatialAcc& other) const -> SpatialAcc
auto operator*(const SpatialMomentum& other) const -> SpatialMomentum
auto operator*(const ClassicalAcc& other) const -> ClassicalAcc
auto operator*(const RotationalInertia& other) const -> RotationalInertia
auto log() const -> AngularMotionVector3
Log mapping between a generic element of SO(3) (iDynTree::Rotation) to the corresponding element of so(3) (iDynTree::AngularMotionVector).
void fromQuaternion(const iDynTree::Vector4& quaternion)
Set the rotation matrix as the passed rotation expressed in quaternion.

Conversion to others represention of matrices.

void getRPY(double& r, double& p, double& y) const
Get a roll, pitch and yaw corresponding to this rotation.
auto asRPY() const -> iDynTree::Vector3
Get a roll, pitch and yaw corresponding to this rotation, as for getRPY, but return a vector with the output parameters.
auto getQuaternion(iDynTree::Vector4& quaternion) const -> bool
Get a unit quaternion corresponding to this rotation.
auto getQuaternion(double& s, double& r1, double& r2, double& r3) const -> bool
Get a unit quaternion corresponding to this rotation.
auto asQuaternion() const -> iDynTree::Vector4
Get a unit quaternion corresponding to this rotation.

Output helpers.

Output helpers.

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

Initialization helpers.

static auto RotX(const double angle) -> Rotation
Return a Rotation around axis X of given angle.
static auto RotY(const double angle) -> Rotation
Return a Rotation around axis Y of given angle.
static auto RotZ(const double angle) -> Rotation
Return a Rotation around axis Z of given angle.
static auto RotAxis(const Direction& direction, const double angle) -> Rotation
Return a Rotation around axis given by direction of given angle.
static auto RotAxisDerivative(const Direction& direction, const double angle) -> Matrix3x3
Return the derivative of the RotAxis function with respect to the angle argument.
static auto RPY(const double roll, const double pitch, const double yaw) -> Rotation
Return a rotation object given Roll, Pitch and Yaw values.
static auto RPYRightTrivializedDerivative(const double roll, const double pitch, const double yaw) -> Matrix3x3
Return the right-trivialized derivative of the RPY function.
static auto RPYRightTrivializedDerivativeRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot) -> Matrix3x3
Return the rate of change of the right-trivialized derivative of the RPY function.
static auto RPYRightTrivializedDerivativeInverse(const double roll, const double pitch, const double yaw) -> Matrix3x3
Return the inverse of the right-trivialized derivative of the RPY function.
static auto RPYRightTrivializedDerivativeInverseRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot) -> Matrix3x3
Return the rate of change of the inverse of the right-trivialized derivative of the RPY function.
static auto QuaternionRightTrivializedDerivative(Vector4 quaternion) -> MatrixFixSize<4, 3>
Return the right-trivialized derivative of the Quaternion function.
static auto QuaternionRightTrivializedDerivativeInverse(Vector4 quaternion) -> MatrixFixSize<3, 4>
Return the inverse of the right-trivialized derivative of the Quaternion function.
static auto Identity() -> Rotation
Return an identity rotation.
static auto RotationFromQuaternion(const iDynTree::Vector4& quaternion) -> Rotation
Construct a rotation matrix from the given unit quaternion representation.
static auto leftJacobian(const iDynTree::AngularMotionVector3& omega) -> Matrix3x3
Get the left Jacobian of rotation matrix.
static auto leftJacobianInverse(const iDynTree::AngularMotionVector3& omega) -> Matrix3x3
Get the left Jacobian inverse of rotation matrix.

Function documentation

iDynTree::Rotation::Rotation()

Default constructor.

The data is not reset to the identity matrix for perfomance reason. Please initialize the data in the vector before any use.

iDynTree::Rotation::Rotation(const double* in_data, const unsigned int in_rows, const unsigned int in_cols)

Constructor from a buffer of 9 doubles, stored as a C-style array (i.e.

row major).

const Rotation& iDynTree::Rotation::changeOrientFrame(const Rotation& newOrientFrame)

Geometric operations.

For the inverse2() operation, both the forward and the inverse geometric relations have to be expressed in the reference orientation frame!!

void iDynTree::Rotation::fromQuaternion(const iDynTree::Vector4& quaternion)

Set the rotation matrix as the passed rotation expressed in quaternion.

Parameters
quaternion the rotation expressed in quaternion

void iDynTree::Rotation::getRPY(double& r, double& p, double& y) const

Get a roll, pitch and yaw corresponding to this rotation.

Parameters
r out roll rotation angle
p out pitch rotation angle
y out yaw rotation angle

Get $ (r,p,y) \in ( (-\pi, \pi] \times (-\frac{\pi}{2}, \frac{\pi}{2}) \times (-\pi, \pi] ) \cup ( \{0\} \times \{-\frac{\pi}{2}\} \times (-\pi,\pi] ) \cup ( \{0\} \times \{\frac{\pi}{2}\} \times [-\pi,\pi) )$ such that *this == RotZ(y)*RotY(p)*RotX(r)

iDynTree::Vector3 iDynTree::Rotation::asRPY() const

Get a roll, pitch and yaw corresponding to this rotation, as for getRPY, but return a vector with the output parameters.

Returns the output vector with the r, p and y parameters.

This function is more suitable for bindings.

bool iDynTree::Rotation::getQuaternion(iDynTree::Vector4& quaternion) const

Get a unit quaternion corresponding to this rotation.

Parameters
quaternion out the output quaternion

The quaternion is defined as [s, r] where $s \in \mathbb{R}$ is the real and $r \in \mathbb{R}^3$ is the imaginary part.

The returned quaternion is such that *this is equal to RotationFromQuaternion(quaternion).

bool iDynTree::Rotation::getQuaternion(double& s, double& r1, double& r2, double& r3) const

Get a unit quaternion corresponding to this rotation.

Parameters
s out the real part
r1 out the first component of the imaginary part (i.e. i base)
r2 out the second component of the imaginary part (i.e. j base)
r3 out the third component of the imaginary part (i.e. k base)

The unit quaternion is defined as [s, r] where $s \in \mathbb{R}$ is the real and $r \in \mathbb{R}^3$ is the imaginary part.

The returned quaternion is such that *this is equal to RotationFromQuaternion(quaternion).

iDynTree::Vector4 iDynTree::Rotation::asQuaternion() const

Get a unit quaternion corresponding to this rotation.

Returns the output quaternion

The quaternion is defined as [s, r] where $s \in \mathbb{R}$ is the costituent and $r \in \mathbb{R}^3$ is the imaginary part.

The returned quaternion is such that *this is equal to RotationFromQuaternion(quaternion).

static Rotation iDynTree::Rotation::RotX(const double angle)

Return a Rotation around axis X of given angle.

Parameters
angle the angle (in Radians) of the rotation arount the X axis

If $ \theta $ is the input angle, this function returns the $ R_x(\theta) $ rotation matrix such that :

\[ R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\theta) & - \sin(\theta) \\ 0 & \sin(\theta) & \cos(\theta) \\ \end{bmatrix} \]

static Rotation iDynTree::Rotation::RotY(const double angle)

Return a Rotation around axis Y of given angle.

Parameters
angle the angle (in Radians) of the rotation arount the Y axis

If $ \theta $ is the input angle, this function returns the $ R_y(\theta) $ rotation matrix such that :

\[ R_y(\theta) = \begin{bmatrix} \cos(\theta) & 0 & \sin(\theta) \\ 0 & 1 & 0 \\ -\sin(\theta) & 0 & \cos(\theta) \\ \end{bmatrix} \]

static Rotation iDynTree::Rotation::RotZ(const double angle)

Return a Rotation around axis Z of given angle.

Parameters
angle the angle (in Radians) of the rotation arount the Z axis

If $ \theta $ is the input angle, this function returns the $ R_z(\theta) $ rotation matrix such that :

\[ R_z(\theta) = \begin{bmatrix} \cos(\theta) & -\sin(\theta) & 0 \\ \sin(\theta) & \cos(\theta) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \]

static Rotation iDynTree::Rotation::RotAxis(const Direction& direction, const double angle)

Return a Rotation around axis given by direction of given angle.

Parameters
direction the Direction around with to rotate
angle the angle (in Radians) of the rotation arount the given axis

If we indicate with $ d \in \mathbb{R}^3 $ the unit norm of the direction, and with $ \theta $ the input angle, the return rotation matrix $ R $ can be computed using the Rodrigues' rotation formula [1] :

\[ R = I_{3\times3} + d^{\wedge} \sin(\theta) + {d^{\wedge}}^2 (1-\cos(\theta)) \]

[1] : http://mathworld.wolfram.com/RodriguesRotationFormula.html

static Matrix3x3 iDynTree::Rotation::RotAxisDerivative(const Direction& direction, const double angle)

Return the derivative of the RotAxis function with respect to the angle argument.

Parameters
direction the Direction around with to rotate
angle the angle (in Radians) of the rotation arount the given axis

If we indicate with $ d \in \mathbb{R}^3 $ the unit norm of the direction, and with $ \theta $ the input angle, the derivative of the rotation matrix $ \frac{\partial R}{\partial \theta} $ can be computed using the derivative of the Rodrigues' rotation formula [1] :

\[ \frac{\partial R}{\partial \theta} = d^{\vee} \cos(\theta) + {d^{\vee}}^2 \sin(\theta) \]

[1] : http://mathworld.wolfram.com/RodriguesRotationFormula.html

static Rotation iDynTree::Rotation::RPY(const double roll, const double pitch, const double yaw)

Return a rotation object given Roll, Pitch and Yaw values.

static Matrix3x3 iDynTree::Rotation::RPYRightTrivializedDerivative(const double roll, const double pitch, const double yaw)

Return the right-trivialized derivative of the RPY function.

If we indicate with $ rpy \in \mathbb{R}^3 $ the roll pitch yaw vector, and with $ RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) $ the function implemented in the Rotation::RPY method, this method returns the right-trivialized partial derivative of Rotation::RPY, i.e. :

\[ (RPY(rpy) \frac{\partial RPY(rpy)}{\partial rpy})^\vee \]

static Matrix3x3 iDynTree::Rotation::RPYRightTrivializedDerivativeRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot)

Return the rate of change of the right-trivialized derivative of the RPY function.

If we indicate with $ rpy \in \mathbb{R}^3 $ the roll pitch yaw vector, and with $ RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) $ the function implemented in the Rotation::RPY method, this method returns the right-trivialized partial derivative of Rotation::RPY, i.e. :

\[ (RPY(rpy) \frac{d}{d t}\frac{\partial RPY(rpy)}{\partial rpy})^\vee \]

static Matrix3x3 iDynTree::Rotation::RPYRightTrivializedDerivativeInverse(const double roll, const double pitch, const double yaw)

Return the inverse of the right-trivialized derivative of the RPY function.

See RPYRightTrivializedDerivative for a detailed description of the method.

static Matrix3x3 iDynTree::Rotation::RPYRightTrivializedDerivativeInverseRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot)

Return the rate of change of the inverse of the right-trivialized derivative of the RPY function.

See RPYRightTrivializedDerivativeRateOfChange for a detailed description of the method.

static MatrixFixSize<4, 3> iDynTree::Rotation::QuaternionRightTrivializedDerivative(Vector4 quaternion)

Return the right-trivialized derivative of the Quaternion function.

If we indicate with $ quat \in \mathbb{Q} $ the quaternion, and with $ QUAT(quat) : \mathbb{Q} \mapsto SO(3) $ the function implemented in the Rotation::RotationFromQuaternion method, this method returns the right-trivialized partial derivative of Rotation::RotationFromQuaternion, i.e. :

\[ (QUAT(quat) \frac{\partial QUAT(quat)}{\partial quat})^\vee \]

static MatrixFixSize<3, 4> iDynTree::Rotation::QuaternionRightTrivializedDerivativeInverse(Vector4 quaternion)

Return the inverse of the right-trivialized derivative of the Quaternion function.

static Rotation iDynTree::Rotation::RotationFromQuaternion(const iDynTree::Vector4& quaternion)

Construct a rotation matrix from the given unit quaternion representation.

Parameters
quaternion a quaternion representing a rotation
Returns The rotation matrix

The quaternion is expected to be ordered in the following way:

  • $s \in \mathbb{R}$ the real part of the quaterion
  • $r \in \mathbb{R}^3$ the imaginary part of the quaternion

The returned rotation matrix is given by the following formula:

\[ R(s,r) = I_{3\times3} + 2s r^{\wedge} + 2{r^\wedge}^2, \]

where $ r^{\wedge} $ is the skew-symmetric matrix such that:

\[ r \times v = r^\wedge v \]

static Matrix3x3 iDynTree::Rotation::leftJacobian(const iDynTree::AngularMotionVector3& omega)

Get the left Jacobian of rotation matrix.

Parameters
omega in angular motion vector
Returns $ 3 \times 3 $ left Jacobian matrix

$ \omega \in \mathbb{R}^3 $ is the angular motion vector $ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) $ where $ \mathfrak{so}(3) $ is the set of skew symmetric matrices or the Lie algebra of $ SO(3) $

\[ J_{l_{SO(3)}} = \sum_{n = 0}^{\infty} \frac{1}{(n+1)!} [\omega_\times]^n = (I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} [\omega _{\times}] + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} [\omega _{\times}]^{2} \]

When simplified further,

\[ J_{l_{SO(3)}} = \frac{\text{sin}(||\omega||)}{||\omega||}I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||} [\phi _{\times}] + \bigg(1 - \frac{\text{sin}(||\omega||)}{||\omega||}\bigg) \phi\phi^T \]

where $ \phi = \frac{\omega}{||\omega||} $

static Matrix3x3 iDynTree::Rotation::leftJacobianInverse(const iDynTree::AngularMotionVector3& omega)

Get the left Jacobian inverse of rotation matrix.

Parameters
omega in angular motion vector
Returns $ 3 \times 3 $ left Jacobian inverse matrix

$ \omega \in \mathbb{R}^3 $ is the angular motion vector $ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) $ where $ \mathfrak{so}(3) $ is the set of skew symmetric matrices or the Lie algebra of $ SO(3) $

\[ J^{-1} _{l _{SO(3)}} = \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) I _3 + \bigg( 1 - \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) \bigg) \phi \phi^T - \frac{||\omega||}{2} [\phi _{\times}] \]

where $ \phi = \frac{\omega}{||\omega||} $