5 #ifndef __pinocchio_spatial_explog_quaternion_hpp__ 6 #define __pinocchio_spatial_explog_quaternion_hpp__ 8 #include "pinocchio/math/quaternion.hpp" 9 #include "pinocchio/spatial/explog.hpp" 10 #include "pinocchio/utils/static-if.hpp" 25 template<
typename Vector3Like,
typename QuaternionLike>
26 void exp3(
const Eigen::MatrixBase<Vector3Like> &
v,
27 Eigen::QuaternionBase<QuaternionLike> & quat_out)
29 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
30 assert(v.size() == 3);
34 typedef Eigen::Quaternion<typename QuaternionLike::Scalar,Options> QuaternionPlain;
36 const Scalar t2 = v.squaredNorm();
37 const Scalar
t = math::sqrt(t2);
39 static const Scalar ts_prec = math::sqrt(Eigen::NumTraits<Scalar>::epsilon());
41 Eigen::AngleAxis<Scalar> aa(t,v/t);
42 QuaternionPlain quat_then(aa);
44 QuaternionPlain quat_else;
46 quat_else.w() =
Scalar(1) - t2/8;
49 for(Eigen::DenseIndex k = 0; k < 4; ++k)
52 quat_then.coeffs().coeffRef(k),
53 quat_else.coeffs().coeffRef(k));
65 template<
typename Vector3Like>
66 Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
67 exp3(
const Eigen::MatrixBase<Vector3Like> &
v)
69 typedef Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
82 template<
typename QuaternionLike>
83 Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
84 log3(
const Eigen::QuaternionBase<QuaternionLike> & quat,
89 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
92 const Scalar norm_squared = quat.vec().squaredNorm();
94 static const Scalar
eps = Eigen::NumTraits<Scalar>::epsilon();
96 const Scalar norm = math::sqrt(norm_squared + eps * eps);
106 Eigen::Quaternion<Scalar, Options> quat_pos;
107 quat_pos.w() = pos_neg * quat.w();
108 quat_pos.vec() = pos_neg * quat.vec();
110 const Scalar theta_2 = math::atan2(norm,quat_pos.w());
111 const Scalar y_x = norm / quat_pos.w();
112 const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());
118 const Scalar th2_2 = theta * theta /
Scalar(4);
121 theta / math::sin(theta_2));
123 for(Eigen::DenseIndex k = 0; k < 3; ++k)
128 res[k] = inv_sinc * quat_pos.vec()[k];
142 template<
typename QuaternionLike>
143 Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
144 log3(
const Eigen::QuaternionBase<QuaternionLike> & quat)
147 return log3(quat.derived(),theta);
156 template<
typename Vector3Like,
typename Matrix43Like>
158 const Eigen::MatrixBase<Matrix43Like> & Jexp)
161 assert(Jexp.rows() == 4 && Jexp.cols() == 3 &&
"Jexp does have the right size.");
166 const Scalar n2 = v.squaredNorm();
167 const Scalar
n = math::sqrt(n2);
168 const Scalar theta =
Scalar(0.5) *
n;
169 const Scalar theta2 =
Scalar(0.25) * n2;
171 if(n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
175 Jout.template topRows<3>().noalias() = ((
Scalar(0.5)/n2) * (c - 2*s/n)) * v * v.transpose();
176 Jout.template topRows<3>().diagonal().array() += s/
n;
177 Jout.template bottomRows<1>().noalias() = -s/(2*
n) * v.transpose();
181 Jout.template topRows<3>().noalias() = (-
Scalar(1)/
Scalar(12) + n2/
Scalar(480)) * v * v.transpose();
182 Jout.template topRows<3>().diagonal().array() +=
Scalar(0.5) * (1 - theta2/6);
183 Jout.template bottomRows<1>().noalias() = (
Scalar(-0.25) * (
Scalar(1) - theta2/6)) * v.transpose();
194 template<
typename QuaternionLike,
typename Matrix3Like>
195 void Jlog3(
const Eigen::QuaternionBase<QuaternionLike> & quat,
196 const Eigen::MatrixBase<Matrix3Like> & Jlog)
199 typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> Vector3;
202 Vector3
w(
log3(quat,t));
208 #endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
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.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Main pinocchio namespace.
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType if_then_else(const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value)