RobWorkProject  23.9.11-
Public Types | Public Member Functions | Friends | Related Functions | List of all members
Quaternion< T > Class Template Reference

A Quaternion \( \mathbf{q}\in \mathbb{R}^4 \) a complex number used to describe rotations in 3-dimensional space. \( q_w+{\bf i}\ q_x+ {\bf j} q_y+ {\bf k}\ q_z \). More...

#include <Quaternion.hpp>

Inherits Rotation3DVector< double >.

Public Types

typedef T value_type
 Value type.
 

Public Member Functions

 Quaternion ()
 constuct Quaterinion of {0,0,0,1}
 
 Quaternion (T qx, T qy, T qz, T qw)
 Creates a Quaternion. More...
 
 Quaternion (const Quaternion< T > &quat)
 Creates a Quaternion from another Quaternion. More...
 
 Quaternion (const rw::math::Rotation3DVector< T > &rot)
 Creates a Quaternion from another Rotation3DVector type. More...
 
 Quaternion (const rw::math::Rotation3D< T > &rot)
 Extracts a Quaternion from Rotation matrix using setRotation(const Rotation3D<R>& rot) More...
 
 Quaternion (const Eigen::Quaternion< T > &r)
 Creates a Quaternion from a Eigen quaternion. More...
 
template<class R >
 Quaternion (const Eigen::MatrixBase< R > &r)
 Creates a Quaternion from vector_expression. More...
 
getQx () const
 get method for the x component More...
 
getQy () const
 get method for the y component More...
 
getQz () const
 get method for the z component More...
 
getQw () const
 get method for the w component More...
 
operator() (size_t i) const
 Returns reference to Quaternion element. More...
 
T & operator() (size_t i)
 Returns reference to Quaternion element. More...
 
T & operator[] (size_t i)
 Returns reference to Quaternion element. More...
 
operator[] (size_t i) const
 Returns reference to Quaternion element. More...
 
const rw::math::Rotation3D< T > toRotation3D () const
 Calculates the \( 3\times 3 \) Rotation matrix. More...
 
template<class R >
void setRotation (const rw::math::Rotation3D< R > &rot)
 Converts a Rotation3D to a Quaternion and saves the Quaternion in this. More...
 
size_t size () const
 The dimension of the quaternion (i.e. 4). This method is provided to help support generic algorithms using size() and operator[].
 
Eigen::Quaternion< T > & e ()
 Convert to an Eigen Quaternion. More...
 
const Eigen::Quaternion< T > & e () const
 Convert to an Eigen Quaternion. More...
 
Eigen::Matrix< T, 4, 1 > toEigenVector () const
 convert to Eigen Vector More...
 
Quaternion< T > operator- () const
 Unary minus.
 
Quaternion< T > operator+ () const
 Unary plus.
 
const Quaternion< T > operator- (const Quaternion< T > &v)
 Subtraction.
 
Quaternion< T > operator* (const Quaternion< T > &r) const
 Multiply-from operator.
 
const Quaternion< T > operator+ (const Quaternion< T > &v) const
 Addition of two quaternions.
 
const Quaternion< T > operator* (T s) const
 Scalar multiplication.
 
const Quaternion< T > operator/ (T s) const
 Scalar Devision.
 
Quaternion< T > elemDivide (const T &lhs) const
 element whise division More...
 
getLength () const
 get length of quaternion \( \sqrt{q_x^2+q_y^2+q_z^2+q_w^2} \) More...
 
getLengthSquared () const
 get squared length of quaternion \( q_x^2+q_y^2+q_z^2+q_w^2 \) More...
 
void normalize ()
 normalizes this quaternion so that \( normalze(Q)=\frac{Q}{\sqrt{q_x^2+q_y^2+q_z^2+q_w^2}} \)
 
const Quaternion< T > slerp (const Quaternion< T > &v, const T t) const
 Calculates a slerp interpolation between this and v. More...
 
Quaternion< T > exp () const
 
Quaternion< T > inverse () const
 Calculate the inverse Quaternion. More...
 
Quaternion< T > ln () const
 calculates the natural logerithm of this quaternion More...
 
Quaternion< T > pow (double power) const
 calculates the quaternion lifted to the power of power More...
 
void operator= (const Eigen::Quaternion< T > &r)
 copy a boost quaternion to this Quaternion More...
 
const Quaternion< T > operator*= (T s)
 Scalar multiplication.
 
const Quaternion< T > operator*= (const Quaternion< T > &r)
 Multiply with operator.
 
const Quaternion< T > operator+= (const Quaternion< T > &r)
 Add-to operator.
 
const Quaternion< T > operator-= (const Quaternion< T > &r)
 Subtract-from operator.
 
Quaternion< T > & operator= (const rw::math::Rotation3D<> &rhs)
 copyfrom rotaion matrix, same as setRotation. More...
 
bool operator== (const Quaternion< T > &r) const
 Comparison (equals) operator.
 
bool operator!= (const Quaternion< T > &r) const
 Comparison (not equals) operator.
 
- Public Member Functions inherited from Rotation3DVector< double >
virtual ~Rotation3DVector ()
 Virtual destructor.
 

Friends

const Quaternion< T > operator* (T s, const Quaternion< T > &v)
 Scalar multiplication.
 

Related Functions

(Note that these are not member functions.)

template<class T >
std::ostream & operator<< (std::ostream &out, const Quaternion< T > &v)
 Streaming operator.
 
template<>
void write (const rw::math::Quaternion< double > &sobject, rw::common::OutputArchive &oarchive, const std::string &id)
 
template<>
void write (const rw::math::Quaternion< float > &sobject, rw::common::OutputArchive &oarchive, const std::string &id)
 
template<>
void read (rw::math::Quaternion< double > &sobject, rw::common::InputArchive &iarchive, const std::string &id)
 
template<>
void read (rw::math::Quaternion< float > &sobject, rw::common::InputArchive &iarchive, const std::string &id)
 

Additional Inherited Members

- Protected Member Functions inherited from Rotation3DVector< double >
 Rotation3DVector (const Rotation3DVector &)
 Copy Constructor. More...
 
 Rotation3DVector ()
 Default Constructor.
 
Rotation3DVectoroperator= (const Rotation3DVector &)
 Assignment operator is protected to force subclasses to implement it by themself.
 

Detailed Description

template<class T = double>
class rw::math::Quaternion< T >

A Quaternion \( \mathbf{q}\in \mathbb{R}^4 \) a complex number used to describe rotations in 3-dimensional space. \( q_w+{\bf i}\ q_x+ {\bf j} q_y+ {\bf k}\ q_z \).

Quaternions can be added and multiplied in a similar way as usual algebraic numbers. Though there are differences. Quaternion multiplication is not commutative which means \( Q\cdot P \neq P\cdot Q \)

Constructor & Destructor Documentation

◆ Quaternion() [1/6]

Quaternion ( qx,
qy,
qz,
qw 
)
inline

Creates a Quaternion.

Parameters
qx[in] \( q_x \)
qy[in] \( q_y \)
qz[in] \( q_z \)
qw[in] \( q_w \)

◆ Quaternion() [2/6]

Quaternion ( const Quaternion< T > &  quat)
inline

Creates a Quaternion from another Quaternion.

Parameters
quat[in] Quaternion

◆ Quaternion() [3/6]

Quaternion ( const rw::math::Rotation3DVector< T > &  rot)
inline

Creates a Quaternion from another Rotation3DVector type.

Parameters
rot[in] The Rotation3DVector type

◆ Quaternion() [4/6]

Quaternion ( const rw::math::Rotation3D< T > &  rot)
inline

Extracts a Quaternion from Rotation matrix using setRotation(const Rotation3D<R>& rot)

Parameters
rot[in] A 3x3 rotation matrix \( \mathbf{rot} \)

◆ Quaternion() [5/6]

Quaternion ( const Eigen::Quaternion< T > &  r)
inline

Creates a Quaternion from a Eigen quaternion.

Parameters
r[in] a boost quaternion

◆ Quaternion() [6/6]

Quaternion ( const Eigen::MatrixBase< R > &  r)
inlineexplicit

Creates a Quaternion from vector_expression.

Parameters
r[in] an Eigen Vector

Member Function Documentation

◆ e() [1/2]

Eigen::Quaternion<T>& e ( )
inline

Convert to an Eigen Quaternion.

Returns
Eigen Quaternion representation.

◆ e() [2/2]

const Eigen::Quaternion<T>& e ( ) const
inline

Convert to an Eigen Quaternion.

Returns
Eigen Quaternion representation.

◆ elemDivide()

Quaternion<T> elemDivide ( const T &  lhs) const

element whise division

Parameters
lhs[in] the scalar to devide with
Returns
the result of elementwise devision

◆ getLength()

T getLength ( ) const
inline

get length of quaternion \( \sqrt{q_x^2+q_y^2+q_z^2+q_w^2} \)

Returns
the length og this quaternion

◆ getLengthSquared()

T getLengthSquared ( ) const
inline

get squared length of quaternion \( q_x^2+q_y^2+q_z^2+q_w^2 \)

Returns
the length og this quaternion

◆ getQw()

T getQw ( ) const
inline

get method for the w component

Returns
the w component of the quaternion

◆ getQx()

T getQx ( ) const
inline

get method for the x component

Returns
the x component of the quaternion

◆ getQy()

T getQy ( ) const
inline

get method for the y component

Returns
the y component of the quaternion

◆ getQz()

T getQz ( ) const
inline

get method for the z component

Returns
the z component of the quaternion

◆ inverse()

Quaternion<T> inverse ( ) const

Calculate the inverse Quaternion.

Returns
the inverse quaternion

◆ ln()

Quaternion<T> ln ( ) const

calculates the natural logerithm of this quaternion

Returns
natural logetihm

◆ operator()() [1/2]

T& operator() ( size_t  i)
inline

Returns reference to Quaternion element.

Parameters
i[in] index in the quaternion \(i\in \{0,1,2,3\} \)
Returns
reference to element

◆ operator()() [2/2]

T operator() ( size_t  i) const
inline

Returns reference to Quaternion element.

Parameters
i[in] index in the quaternion \(i\in \{0,1,2,3\} \)
Returns
const reference to element

◆ operator=() [1/2]

void operator= ( const Eigen::Quaternion< T > &  r)
inline

copy a boost quaternion to this Quaternion

Parameters
r[in] - boost quaternion

◆ operator=() [2/2]

Quaternion<T>& operator= ( const rw::math::Rotation3D<> &  rhs)
inline

copyfrom rotaion matrix, same as setRotation.

Parameters
rhs[in] the rotation that will be copyed

◆ operator[]() [1/2]

T& operator[] ( size_t  i)
inline

Returns reference to Quaternion element.

Parameters
i[in] index in the quaternion \(i\in \{0,1,2,3\} \)
Returns
reference to element

◆ operator[]() [2/2]

T operator[] ( size_t  i) const
inline

Returns reference to Quaternion element.

Parameters
i[in] index in the quaternion \(i\in \{0,1,2,3\} \)
Returns
reference to element

◆ pow()

Quaternion<T> pow ( double  power) const

calculates the quaternion lifted to the power of power

Parameters
power[in] the power the quaternion is lifted to
Returns
\( Quaternion^power \)

◆ setRotation()

void setRotation ( const rw::math::Rotation3D< R > &  rot)
inline

Converts a Rotation3D to a Quaternion and saves the Quaternion in this.

Parameters
rot[in] A 3x3 rotation matrix \( \mathbf{R} \)

\( \begin{array}{c} q_x\\ q_y\\ q_z\\ q_w \end{array} = \left[ \begin{array}{c} \\ \\ \end{array} \right] \)

The conversion method is proposed by Henrik Gordon Petersen. The switching between different cases occur well before numerical instabilities, hence the solution should be more robust, than many of the methods proposed elsewhere.

◆ slerp()

const Quaternion<T> slerp ( const Quaternion< T > &  v,
const T  t 
) const
inline

Calculates a slerp interpolation between this and v.

The slerp interpolation ensures a constant velocity across the interpolation. For \( t=0\) the result is this and for \( t=1\) it is v.

Note
Algorithm and implementation is thanks to euclideanspace.com

◆ toEigenVector()

Eigen::Matrix<T, 4, 1> toEigenVector ( ) const
inline

convert to Eigen Vector

Returns
eigen Vector of quaternion

◆ toRotation3D()

const rw::math::Rotation3D<T> toRotation3D ( ) const
inlinevirtual

Calculates the \( 3\times 3 \) Rotation matrix.

Returns
A 3x3 rotation matrix \( \mathbf{rot} \) \( \mathbf{rot} = \left[ \begin{array}{ccc} 1-2(q_y^2-q_z^2) & 2(q_x\ q_y+q_z\ q_w)& 2(q_x\ q_z-q_y\ q_w) \\ 2(q_x\ q_y-q_z\ q_w) & 1-2(q_x^2-q_z^2) & 2(q_y\ q_z+q_x\ q_w)\\ 2(q_x\ q_z+q_y\ q_w) & 2(q_y\ q_z-q_x\ q_z) & 1-2(q_x^2-q_y^2) \end{array} \right] \)

Implements Rotation3DVector< double >.

Friends And Related Function Documentation

◆ read() [1/2]

void read ( rw::math::Quaternion< double > &  sobject,
rw::common::InputArchive iarchive,
const std::string &  id 
)
related

Enable read-serialization of class T by overloading this method. Data is read from iarchive and filled into sobject.

Parameters
sobject[out] the object in which the data should be streamed into
iarchive[in] the InputArchive from which to read data.
id[in] The id of the serialized sobject.
Note
the id can be empty in which case the overloaded method should provide a default identifier. E.g. the Vector3D class defined "Vector3D" as its default id.

◆ read() [2/2]

void read ( rw::math::Quaternion< float > &  sobject,
rw::common::InputArchive iarchive,
const std::string &  id 
)
related

Enable read-serialization of class T by overloading this method. Data is read from iarchive and filled into sobject.

Parameters
sobject[out] the object in which the data should be streamed into
iarchive[in] the InputArchive from which to read data.
id[in] The id of the serialized sobject.
Note
the id can be empty in which case the overloaded method should provide a default identifier. E.g. the Vector3D class defined "Vector3D" as its default id.

◆ write() [1/2]

void write ( const rw::math::Quaternion< double > &  sobject,
rw::common::OutputArchive oarchive,
const std::string &  id 
)
related

Enable write-serialization of class T by overloading this method. Data is written to oarchive from the sobject.

Parameters
sobject[in] the object from which the data should be streamed.
oarchive[out] the OutputArchive in which data should be written.
id[in] The id of the serialized sobject.
Note
the id can be empty in which case the overloaded method should provide a default identifier. E.g. the Vector3D class defined "Vector3D" as its default id.

◆ write() [2/2]

void write ( const rw::math::Quaternion< float > &  sobject,
rw::common::OutputArchive oarchive,
const std::string &  id 
)
related

Enable write-serialization of class T by overloading this method. Data is written to oarchive from the sobject.

Parameters
sobject[in] the object from which the data should be streamed.
oarchive[out] the OutputArchive in which data should be written.
id[in] The id of the serialized sobject.
Note
the id can be empty in which case the overloaded method should provide a default identifier. E.g. the Vector3D class defined "Vector3D" as its default id.

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