Quaternion operations. More...
Namespaces | |
internal | |
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 D , typename Matrix3 > | |
void | assignQuaternion (Eigen::QuaternionBase< D > &quat, const Eigen::MatrixBase< Matrix3 > &R) |
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, PINOCCHIO_EIGEN_PLAIN_TYPE(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 Quaternion > | |
bool | isNormalized (const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec=Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision()) |
Check whether the input quaternion is Normalized within the given precision. More... | |
template<typename Vector3Like , typename Matrix43Like > | |
void | Jexp3CoeffWise (const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp) |
Derivative of where is a small perturbation of 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, PINOCCHIO_EIGEN_PLAIN_TYPE(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, PINOCCHIO_EIGEN_PLAIN_TYPE(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. More... | |
Quaternion operations.
D1::Scalar pinocchio::quaternion::angleBetweenQuaternions | ( | const Eigen::QuaternionBase< D1 > & | q1, |
const Eigen::QuaternionBase< D2 > & | q2 | ||
) |
Compute the minimal angle between q1 and q2.
[in] | q1 | input quaternion. |
[in] | q2 | input quaternion. |
Definition at line 35 of file math/quaternion.hpp.
void pinocchio::quaternion::assignQuaternion | ( | Eigen::QuaternionBase< D > & | quat, |
const Eigen::MatrixBase< Matrix3 > & | R | ||
) |
Definition at line 209 of file math/quaternion.hpp.
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.
[in] | q1 | input quaternion. |
[in] | q2 | input quaternion. |
Definition at line 59 of file math/quaternion.hpp.
void pinocchio::quaternion::exp3 | ( | const Eigen::MatrixBase< Vector3Like > & | v, |
Eigen::QuaternionBase< QuaternionLike > & | quat_out | ||
) |
Exp: so3 -> SO3 (quaternion)
[in] | v | The angular velocity vector. |
[out] | qout | The quanternion where the result is stored. |
Definition at line 26 of file explog-quaternion.hpp.
Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> pinocchio::quaternion::exp3 | ( | const Eigen::MatrixBase< Vector3Like > & | v | ) |
Exp: so3 -> SO3 (quaternion)
[in] | v | The angular velocity vector. |
Definition at line 67 of file explog-quaternion.hpp.
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 . Using the following limited development:
The output is
The output quaternion is guaranted to statisfy the following:
where and is the maximum tolerance of .
Definition at line 88 of file math/quaternion.hpp.
|
inline |
Check whether the input quaternion is Normalized within the given precision.
[in] | quat | Input quaternion |
[in] | prec | Required precision |
Definition at line 225 of file math/quaternion.hpp.
void pinocchio::quaternion::Jexp3CoeffWise | ( | const Eigen::MatrixBase< Vector3Like > & | v, |
const Eigen::MatrixBase< Matrix43Like > & | Jexp | ||
) |
Derivative of where is a small perturbation of at identity.
Definition at line 157 of file explog-quaternion.hpp.
void pinocchio::quaternion::Jlog3 | ( | const Eigen::QuaternionBase< QuaternionLike > & | quat, |
const Eigen::MatrixBase< Matrix3Like > & | Jlog | ||
) |
Computes the Jacobian of log3 operator for a unit quaternion.
[in] | quat | A unit quaternion representing the input rotation. |
[out] | Jlog | The resulting Jacobian of the log operator. |
Definition at line 195 of file explog-quaternion.hpp.
Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(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.
[in] | quat | the unit quaternion. |
[out] | theta | the angle value (resuling from compurations). |
Definition at line 84 of file explog-quaternion.hpp.
Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options> pinocchio::quaternion::log3 | ( | const Eigen::QuaternionBase< QuaternionLike > & | quat | ) |
Log: SO3 -> so3.
Pseudo-inverse of log from .
[in] | quat | The unit quaternion representing a certain rotation. |
Definition at line 144 of file explog-quaternion.hpp.
void pinocchio::quaternion::uniformRandom | ( | const Eigen::QuaternionBase< Derived > & | q | ) |
Uniformly random quaternion sphere.
Definition at line 108 of file math/quaternion.hpp.