Quaternion operations. More...

Functions
template<typename D1 , typename D2 >
D1::Scalar	angleBetweenQuaternions (const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
	Compute the minimal angle between q1 and q2. More...

template<typename D1 , typename D2 >
bool	defineSameRotation (const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
	Check if two quaternions define the same rotations. More...

template<typename Vector3Like , typename QuaternionLike >
void	exp3 (const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
	Exp: so3 -> SO3 (quaternion) More...

template<typename Vector3Like >
Eigen::Quaternion< typename Vector3Like::Scalar, Vector3Like::Options >	exp3 (const Eigen::MatrixBase< Vector3Like > &v)
	Exp: so3 -> SO3 (quaternion) More...

template<typename D >
void	firstOrderNormalize (const Eigen::QuaternionBase< D > &q)

template<typename Vector3Like , typename Matrix43Like >
void	Jexp3CoeffWise (const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
	Derivative of \( q = \exp{\mathbf{v} + \delta\mathbf{v}} \) where \( \delta\mathbf{v} \) is a small perturbation of \( \mathbf{v} \) at identity. More...

template<typename QuaternionLike , typename Matrix3Like >
void	Jlog3 (const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
	Computes the Jacobian of log3 operator for a unit quaternion. More...

template<typename QuaternionLike >
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, typename QuaternionLike::Vector3::Options >	log3 (const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
	Same as log3 but with a unit quaternion as input. More...

template<typename QuaternionLike >
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, typename QuaternionLike::Vector3::Options >	log3 (const Eigen::QuaternionBase< QuaternionLike > &quat)
	Log: SO3 -> so3. More...

template<typename Derived >
void	uniformRandom (const Eigen::QuaternionBase< Derived > &q)
	Uniformly random quaternion sphere.

Detailed Description

Quaternion operations.

Function Documentation

D1::Scalar pinocchio::quaternion::angleBetweenQuaternions	(	const Eigen::QuaternionBase< D1 > &	q1,
		const Eigen::QuaternionBase< D2 > &	q2
	)

Compute the minimal angle between q1 and q2.

Parameters

[in]	q1	input quaternion.
[in]	q2	input quaternion.

Returns: angle between the two quaternions

Definition at line 24 of file quaternion.hpp.

Referenced by pinocchio::angleBetweenQuaternions().

bool pinocchio::quaternion::defineSameRotation	(	const Eigen::QuaternionBase< D1 > &	q1,
		const Eigen::QuaternionBase< D2 > &	q2,
		const typename D1::RealScalar &	prec = `Eigen::NumTraits<typename D1::Scalar>::dummy_precision()`
	)

Check if two quaternions define the same rotations.

Note: Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2.

Parameters

[in]	q1	input quaternion.
[in]	q2	input quaternion.

Returns: Return true if the two input quaternions define the same rotation.

Definition at line 48 of file quaternion.hpp.

Referenced by pinocchio::defineSameRotation(), and SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv().

void pinocchio::quaternion::exp3	(	const Eigen::MatrixBase< Vector3Like > &	v,
		Eigen::QuaternionBase< QuaternionLike > &	quat_out
	)

Exp: so3 -> SO3 (quaternion)

Returns: the integral of the velocity vector as a queternion.

Parameters

[in]	v	The angular velocity vector.
[out]	qout	The quanternion where the result is stored.

Definition at line 24 of file explog-quaternion.hpp.

Referenced by exp3(), and SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv().

Eigen::Quaternion<typename Vector3Like::Scalar, Vector3Like ::Options> pinocchio::quaternion::exp3 ( const Eigen::MatrixBase< Vector3Like > & v )

Exp: so3 -> SO3 (quaternion)

Returns: the integral of the velocity vector as a queternion.

Parameters

[in] v The angular velocity vector.

Definition at line 58 of file explog-quaternion.hpp.

References exp3().

void pinocchio::quaternion::firstOrderNormalize ( const Eigen::QuaternionBase< D > & q )

Approximately normalize by applying the first order limited development of the normalization function.

Only additions and multiplications are required. Neither square root nor division are used (except a division by 2). Let \( \delta = ||q||^2 - 1 \). Using the following limited development:

\[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \]

The output is

\[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \]

The output quaternion is guaranted to statisfy the following:

\[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \]

where \( M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \) and \( \epsilon \) is the maximum tolerance of \( ||q_{in}||^2 - 1 \).

Warning: \( ||q||^2 - 1 \) should already be close to zero.

Note: See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3 to know the reason why the argument is const.

Definition at line 77 of file quaternion.hpp.

Referenced by pinocchio::firstOrderNormalize(), SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv(), and SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nv().

void pinocchio::quaternion::Jexp3CoeffWise	(	const Eigen::MatrixBase< Vector3Like > &	v,
		const Eigen::MatrixBase< Matrix43Like > &	Jexp
	)

Derivative of \( q = \exp{\mathbf{v} + \delta\mathbf{v}} \) where \( \delta\mathbf{v} \) is a small perturbation of \( \mathbf{v} \) at identity.

Returns: The Jacobian of the quaternion components variation.

Definition at line 137 of file explog-quaternion.hpp.

References pinocchio::SINCOS().

Referenced by SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv(), and SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nv().

void pinocchio::quaternion::Jlog3	(	const Eigen::QuaternionBase< QuaternionLike > &	quat,
		const Eigen::MatrixBase< Matrix3Like > &	Jlog
	)

Computes the Jacobian of log3 operator for a unit quaternion.

Parameters

[in]	quat	A unit quaternion representing the input rotation.
[out]	Jlog	The resulting Jacobian of the log operator.

Definition at line 175 of file explog-quaternion.hpp.

References log3().

Eigen::Matrix<typename QuaternionLike::Scalar,3,1, typename QuaternionLike::Vector3 ::Options> pinocchio::quaternion::log3	(	const Eigen::QuaternionBase< QuaternionLike > &	quat,
		typename QuaternionLike::Scalar &	theta
	)

Same as log3 but with a unit quaternion as input.

Parameters

[in]	quat	the unit quaternion.
[out]	theta	the angle value (resuling from compurations).

Returns: The angular velocity vector associated to the rotation matrix.

Definition at line 75 of file explog-quaternion.hpp.

Referenced by Jlog3(), log3(), SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv(), and SpecialEuclideanOperationTpl< 3, _Scalar, _Options >::nv().

Eigen::Matrix<typename QuaternionLike::Scalar,3,1, typename QuaternionLike::Vector3 ::Options> pinocchio::quaternion::log3 ( const Eigen::QuaternionBase< QuaternionLike > & quat )

Log: SO3 -> so3.

Pseudo-inverse of log from \( SO3 -> { v \in so3, ||v|| \le pi } \).

Parameters

[in] quat The unit quaternion representing a certain rotation.

Returns: The angular velocity vector associated to the quaternion.

Definition at line 124 of file explog-quaternion.hpp.

References log3().

Functions

Detailed Description

Function Documentation