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);
32 typedef typename Vector3Like::Scalar Scalar;
33 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(
typename QuaternionLike::Coefficients)::Options };
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;
45 quat_else.vec() = (Scalar(1)/Scalar(2) - t2/48) * v;
46 quat_else.w() = Scalar(1) - t2/8;
48 using ::pinocchio::internal::if_then_else;
49 for(Eigen::DenseIndex k = 0; k < 4; ++k)
51 quat_out.coeffs().coeffRef(k) = if_then_else(::pinocchio::internal::GT, t2, ts_prec,
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;
70 ReturnType res;
exp3(v,res);
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,
85 typename QuaternionLike::Scalar & theta)
87 typedef typename QuaternionLike::Scalar Scalar;
88 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(
typename QuaternionLike::Vector3)::Options };
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);
98 using ::pinocchio::internal::if_then_else;
99 using ::pinocchio::internal::GE;
100 using ::pinocchio::internal::LT;
102 const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0),
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());
114 theta = if_then_else(LT, norm_squared, ts_prec,
115 Scalar(2.)*(Scalar(1) - y_x_sq / Scalar(3)) * y_x,
118 const Scalar th2_2 = theta * theta / Scalar(4);
119 const Scalar inv_sinc = if_then_else(LT, norm_squared, ts_prec,
120 Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7)/Scalar(360) * th2_2*th2_2),
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)
146 typename QuaternionLike::Scalar theta;
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.");
162 Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);
164 typedef typename Vector3Like::Scalar Scalar;
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)
198 typedef typename QuaternionLike::Scalar Scalar;
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__