Math related to orientation calculations. More...

Functions
EIGEN_DEVICE_FUNC	Quaternion (const arma::vec &arma)
	Constructor of Eigen Quaternion with armadillo vector in [w, x, y, z]. More...

template<class OtherDerived >
EIGEN_DEVICE_FUNC Matrix < Scalar, 3, 1 >	error (const QuaternionBase< OtherDerived > &desired) const
	Returns the orientation error between two quaternions. More...

arma::vec	arl::math::getOrientationError (const arma::vec &q, const arma::vec &qd)
	Returns the orientation error between two quaternions using Armadillo. More...

KDL::Vector	arl::math::getOrientationError (const std::vector< double > &quat, const std::vector< double > &quat_d)
	Calculate the orientation error between two quaternions using STL and KDL. More...

arma::mat	arl::math::quatToRot (const arma::vec &q)
	Transforms a quaternion to a rotation matrix. More...

arma::vec	arl::math::rotToQuat (const arma::mat &R)
	Transforms a rotation matrix to a quaternion. More...

arma::vec	arl::math::getQuatConjugate (const arma::vec &q)
	Calculates the quaternion conjugate. More...

arma::vec	arl::math::getQuatInverse (const arma::vec &q)
	Calculates the inverse of a given quaternion. More...

arma::vec	arl::math::getQuatProduct (const arma::vec &q1, const arma::vec &q2)
	Calculates the product of two quaternions. More...

arma::vec	arl::math::getQuatDifference (const arma::vec &q, const arma::vec &qd)
	Calculates the difference of two quaternions. More...

arma::vec	arl::math::rotateQuat (const arma::vec &w, const arma::vec &q)
	Integrates the orientation in quaternion. More...

arma::vec	arl::math::getQuatLog (const arma::vec &Q, double eps=std::numeric_limits< double >::epsilon())
	Calculate the log of a quaternion. More...

arma::vec	arl::math::getQuatExp (const arma::vec &w, double eps=std::numeric_limits< double >::epsilon())
	Calculate the exp of a quaternion. More...

bool	arl::math::rotationHasUnitColumns (const KDL::Rotation &rot, double eps=1e-8)
	Tests if the columns of the rotation matrix are unit vectors. More...

bool	arl::math::rotationHasOrthogonalColumns (const KDL::Rotation &rot, double eps=1e-8)
	Tests if the columns of the rotation matrix are orthogonal. More...

bool	arl::math::rotationIsRightHanded (const KDL::Rotation &rot, double eps=1e-8)
	Tests if the columns of the rotation matrix form a right handed system. More...

bool	arl::math::rotationIsOk (const KDL::Rotation &rot, double eps=1e-8)
	Tests if a 3x3 matrix R is a legit rotation matrix, i.e. if R belongs to SO(3). More...

arma::mat	arl::math::rotX (double theta)
	Provides the basic rotation around the X-axis. More...

arma::mat	arl::math::rotY (double theta)
	Provides the basic rotation around the Y-axis. More...

arma::mat	arl::math::rotZ (double theta)
	Provides the basic rotation around the Z-axis. More...

Detailed Description

Math related to orientation calculations.

Function Documentation

template<class OtherDerived >

EIGEN_DEVICE_FUNC Matrix<Scalar, 3, 1> error ( const QuaternionBase< OtherDerived > & desired ) const

inline

Returns the orientation error between two quaternions.

The returned quaternion error is given by Eq. 3.91 at p. 140 of book "Robotics: Modelling, Planning and Control of Siciliano", with different sequence.

Parameters

desired The desired quaternion

Returns: The quaternion error as a 3D vector

,

arma::vec arl::math::getOrientationError	(	const arma::vec &	q,
		const arma::vec &	qd
	)

Returns the orientation error between two quaternions using Armadillo.

The quaternions are following the convention w, x, y, z. The returned quaternion error is given by Eq. 3.91 at p. 140 of book "Robotics: Modelling, Planning and Control of Siciliano", with different sequence.

Parameters

q	The current orientation
qd	The desired orientation

Returns: The quaternion error

See Also: getOrientationError(const std::vector<double>& quat, const std::vector<double>& quat_d);

Deprecated:: Use error(const QuaternionBase<OtherDerived>& desired) const instead

Here is the call graph for this function:

KDL::Vector arl::math::getOrientationError	(	const std::vector< double > &	quat,
		const std::vector< double > &	quat_d
	)

Calculate the orientation error between two quaternions using STL and KDL.

The quaternions are following the convention w, x, y, z. The returned quaternion error is given by Eq. 3.91 at p. 140 of book "Robotics: Modelling, Planning and Control of Siciliano", with different sequence.

Parameters

quat	The current orientation
quat_d	The desired orientation

Returns: The quaternion error as a KDL Vector

See Also: getOrientationError(const arma::vec& q, const arma::vec& qd)

Deprecated:: Use error(const QuaternionBase<OtherDerived>& desired) const instead

Here is the call graph for this function:

arma::vec arl::math::getQuatConjugate ( const arma::vec & q )

Calculates the quaternion conjugate.

The quaternions as input and output follows the convention w, x, y, z

Parameters

q	The given quaternion.

Returns: The quaternion conjugate

arma::vec arl::math::getQuatDifference	(	const arma::vec &	q,
		const arma::vec &	qd
	)

Calculates the difference of two quaternions.

The quaternions as input and output follows the convention w, x, y, z

Parameters

q	Quaternion input 1
qd	Quaternion input 2

Returns: The Quaternion difference

Here is the call graph for this function:

arma::vec arl::math::getQuatExp	(	const arma::vec &	w,
		double	eps = `std::numeric_limits<double>::epsilon()`
	)

Calculate the exp of a quaternion.

Parameters

w	The angular velocity

Returns: The quaternion exponential (with zero scalar part) as described in (25) of "Orientation in Cartesian Space Dynamic Movement Primitives", Ales Ude1, Bojan Nemec1, Tadej

arma::vec arl::math::getQuatInverse ( const arma::vec & q )

Calculates the inverse of a given quaternion.

The quaternions as input and output follows the convention w, x, y, z

Parameters

q	The given quaternion

Returns: The inverse of the quaternion

Here is the call graph for this function:

arma::vec arl::math::getQuatLog	(	const arma::vec &	Q,
		double	eps = `std::numeric_limits<double>::epsilon()`
	)

Calculate the log of a quaternion.

arma::vec arl::math::getQuatProduct	(	const arma::vec &	q1,
		const arma::vec &	q2
	)

Calculates the product of two quaternions.

The quaternions as input and output follows the convention w, x, y, z

Parameters

q1	Quaternion input 1
q1	Quaternion input 2

Returns: The quaternion product

Here is the call graph for this function:

EIGEN_DEVICE_FUNC Quaternion ( const arma::vec & arma )

inline

Constructor of Eigen Quaternion with armadillo vector in [w, x, y, z].

arma The quaternion in the arma vector

,

arma::mat arl::math::quatToRot ( const arma::vec & q )

Transforms a quaternion to a rotation matrix.

The quaternion should be given following the convention: w, x, y, z

Parameters

q	The quaternion to be transformed

Returns: The resulting rotation matrix

arma::vec arl::math::rotateQuat	(	const arma::vec &	w,
		const arma::vec &	q
	)

Integrates the orientation in quaternion.

Parameters

w	The angular deviation
q	The current quaternion in w, x, y, z

Returns: The rotated quaternion as described in (24) of "Orientation in Cartesian Space Dynamic Movement Primitives", Ales Ude1, Bojan Nemec1, Tadej Petric1, and Jun Morimoto.

Here is the call graph for this function:

bool arl::math::rotationHasOrthogonalColumns	(	const KDL::Rotation &	rot,
		double	eps = `1e-8`
	)

Tests if the columns of the rotation matrix are orthogonal.

Parameters

rot	The rotation matrix to be tested
eps	Parameter for the tolerances to rounding errors

Returns: True if the rotation are orthogonal, false otherwise

bool arl::math::rotationHasUnitColumns	(	const KDL::Rotation &	rot,
		double	eps = `1e-8`
	)

Tests if the columns of the rotation matrix are unit vectors.

Parameters

rot	The rotation matrix to be tested
eps	Parameter for the tolerances to rounding errors

Returns: True if the rotation are unit vectors, false otherwise

bool arl::math::rotationIsOk	(	const KDL::Rotation &	rot,
		double	eps = `1e-8`
	)

Tests if a 3x3 matrix R is a legit rotation matrix, i.e. if R belongs to SO(3).

This function tests if the columns of the rotation matrix:

Are unit vectors
Are orthogonal to each other
Forms a righthanded system

Parameters

rot	The rotation matrix to be tested
eps	Parameter for the tolerances to rounding errors

Returns: True if the rotation is ok, false otherwise

Here is the call graph for this function:

bool arl::math::rotationIsRightHanded	(	const KDL::Rotation &	rot,
		double	eps = `1e-8`
	)

Tests if the columns of the rotation matrix form a right handed system.

Parameters

rot	The rotation matrix to be tested
eps	Parameter for the tolerances to rounding errors

Returns: True if the rotation is right-handed, false otherwise

arma::vec arl::math::rotToQuat ( const arma::mat & R )

Transforms a rotation matrix to a quaternion.

The quaternion will returned following the convention: w, x, y, z

Parameters

R	A 3x3 armadillo matrix as the rotation matrix

Returns: The quaternion as output

arma::mat arl::math::rotX ( double theta )

Provides the basic rotation around the X-axis.

Parameters

theta The angle for the rotation in rad

arma::mat arl::math::rotY ( double theta )

Provides the basic rotation around the Y-axis.

Parameters

theta The angle for the rotation in rad

arma::mat arl::math::rotZ ( double theta )

Provides the basic rotation around the Z-axis.

Parameters

theta The angle for the rotation in rad

Functions

Detailed Description

Function Documentation