pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
explog-quaternion.hpp
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_spatial_explog_quaternion_hpp__
6 #define __pinocchio_spatial_explog_quaternion_hpp__
7 
8 #include "pinocchio/math/quaternion.hpp"
9 #include "pinocchio/spatial/explog.hpp"
10 #include "pinocchio/utils/static-if.hpp"
11 
12 namespace pinocchio
13 {
14  namespace quaternion
15  {
16 
25  template<typename Vector3Like, typename QuaternionLike>
26  void
27  exp3(const Eigen::MatrixBase<Vector3Like> & v, Eigen::QuaternionBase<QuaternionLike> & quat_out)
28  {
29  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
30  assert(v.size() == 3);
31 
32  typedef typename Vector3Like::Scalar Scalar;
33  enum
34  {
35  Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options
36  };
37  typedef Eigen::Quaternion<typename QuaternionLike::Scalar, Options> QuaternionPlain;
38  const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
39 
40  const Scalar t2 = v.squaredNorm();
41  const Scalar t = math::sqrt(t2 + eps * eps);
42 
43  static const Scalar ts_prec =
44  TaylorSeriesExpansion<Scalar>::template precision<3>(); // Precision for the Taylor series
45  // expansion.
46 
47  Eigen::AngleAxis<Scalar> aa(t, v / t);
48  QuaternionPlain quat_then(aa);
49 
50  // order 4 Taylor expansion in theta / (order 2 in t2)
51  QuaternionPlain quat_else;
52  const Scalar t2_2 = t2 / 4; // theta/2 squared
53  quat_else.vec() =
54  Scalar(0.5) * (Scalar(1) - t2_2 / Scalar(6) + t2_2 * t2_2 / Scalar(120)) * v;
55  quat_else.w() = Scalar(1) - t2_2 / 2 + t2_2 * t2_2 / 24;
56 
57  using ::pinocchio::internal::if_then_else;
58  for (Eigen::DenseIndex k = 0; k < 4; ++k)
59  {
60  quat_out.coeffs().coeffRef(k) = if_then_else(
61  ::pinocchio::internal::GT, t2, ts_prec, quat_then.coeffs().coeffRef(k),
62  quat_else.coeffs().coeffRef(k));
63  }
64  }
65 
72  template<typename Vector3Like>
73  Eigen::
74  Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
75  exp3(const Eigen::MatrixBase<Vector3Like> & v)
76  {
77  typedef Eigen::Quaternion<
78  typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
79  ReturnType;
80  ReturnType res;
81  exp3(v, res);
82  return res;
83  }
84 
91  template<typename MotionDerived, typename Config_t>
92  void exp6(const MotionDense<MotionDerived> & motion, Eigen::MatrixBase<Config_t> & qout)
93  {
94  enum
95  {
96  Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t)::Options
97  };
98  typedef typename Config_t::Scalar Scalar;
99  typedef typename MotionDerived::Vector3 Vector3;
100  typedef Eigen::Quaternion<Scalar, Options> Quaternion_t;
101  const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
102 
103  const typename MotionDerived::ConstAngularType & w = motion.angular();
104  const typename MotionDerived::ConstLinearType & v = motion.linear();
105 
106  const Scalar t2 = w.squaredNorm() + eps * eps;
107  const Scalar t = math::sqrt(t2);
108 
109  Scalar ct, st;
110  SINCOS(t, &st, &ct);
111 
112  const Scalar inv_t2 = Scalar(1) / t2;
113  const Scalar ts_prec =
114  TaylorSeriesExpansion<Scalar>::template precision<3>(); // Taylor expansion precision
115 
116  using ::pinocchio::internal::if_then_else;
117  using ::pinocchio::internal::LT;
118 
119  const Scalar alpha_wxv = if_then_else(
120  LT, t, ts_prec,
121  Scalar(0.5) - t2 / Scalar(24), // then: use Taylor expansion
122  (Scalar(1) - ct) * inv_t2 // else
123  );
124 
125  const Scalar alpha_w2 = if_then_else(
126  LT, t, ts_prec, Scalar(1) / Scalar(6) - t2 / Scalar(120), (t - st) * inv_t2 / t);
127 
128  // linear part
129  Eigen::Map<Vector3> trans_(qout.derived().template head<3>().data());
130  trans_.noalias() = v + alpha_wxv * w.cross(v) + alpha_w2 * w.cross(w.cross(v));
131 
132  // quaternion part
133  typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
134  QuaternionMap_t quat_(qout.derived().template tail<4>().data());
135  exp3(w, quat_);
136  }
137 
143  template<typename MotionDerived>
144  Eigen::Matrix<
145  typename MotionDerived::Scalar,
146  7,
147  1,
148  PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
149  exp6(const MotionDense<MotionDerived> & motion)
150  {
151  typedef typename MotionDerived::Scalar Scalar;
152  enum
153  {
154  Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options
155  };
156  typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType;
157 
158  ReturnType qout;
159  exp6(motion, qout);
160  return qout;
161  }
162 
169  template<typename Vector6Like, typename Config_t>
170  void exp6(const Eigen::MatrixBase<Vector6Like> & vec6, Eigen::MatrixBase<Config_t> & qout)
171  {
172  MotionRef<const Vector6Like> nu(vec6.derived());
173  ::pinocchio::quaternion::exp6(nu, qout);
174  }
175 
181  template<typename Vector6Like>
182  Eigen::
183  Matrix<typename Vector6Like::Scalar, 7, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
184  exp6(const Eigen::MatrixBase<Vector6Like> & vec6)
185  {
186  typedef typename Vector6Like::Scalar Scalar;
187  enum
188  {
189  Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options
190  };
191  typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType;
192 
193  ReturnType qout;
194  ::pinocchio::quaternion::exp6(vec6, qout);
195  return qout;
196  }
197 
205  template<typename QuaternionLike>
206  Eigen::Matrix<
207  typename QuaternionLike::Scalar,
208  3,
209  1,
210  PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
211  log3(
212  const Eigen::QuaternionBase<QuaternionLike> & quat, typename QuaternionLike::Scalar & theta)
213  {
214  typedef typename QuaternionLike::Scalar Scalar;
215  enum
216  {
217  Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options
218  };
219  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
220 
221  Vector3 res;
222  const Scalar norm_squared = quat.vec().squaredNorm();
223 
224  static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
225  static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
226  const Scalar norm = math::sqrt(norm_squared + eps * eps);
227 
228  using ::pinocchio::internal::GE;
229  using ::pinocchio::internal::if_then_else;
230  using ::pinocchio::internal::LT;
231 
232  const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));
233 
234  Eigen::Quaternion<Scalar, Options> quat_pos;
235  quat_pos.w() = pos_neg * quat.w();
236  quat_pos.vec() = pos_neg * quat.vec();
237 
238  const Scalar theta_2 = math::atan2(norm, quat_pos.w()); // in [0,pi]
239  const Scalar y_x = norm / quat_pos.w(); // nonnegative
240  const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());
241 
242  theta = if_then_else(
243  LT, norm_squared, ts_prec, Scalar(2.) * (Scalar(1) - y_x_sq / Scalar(3)) * y_x,
244  Scalar(2.) * theta_2);
245 
246  const Scalar th2_2 = theta * theta / Scalar(4);
247  const Scalar inv_sinc = if_then_else(
248  LT, norm_squared, ts_prec,
249  Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7) / Scalar(360) * th2_2 * th2_2),
250  theta / math::sin(theta_2));
251 
252  for (Eigen::DenseIndex k = 0; k < 3; ++k)
253  {
254  // res[k] = if_then_else(LT, norm_squared, ts_prec,
255  // Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq /
256  // Scalar(9)) * pos_neg * quat.vec()[k], inv_sinc * pos_neg *
257  // quat.vec()[k]);
258  res[k] = inv_sinc * quat_pos.vec()[k];
259  }
260  return res;
261  }
262 
272  template<typename QuaternionLike>
273  Eigen::Matrix<
274  typename QuaternionLike::Scalar,
275  3,
276  1,
277  PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
278  log3(const Eigen::QuaternionBase<QuaternionLike> & quat)
279  {
280  typename QuaternionLike::Scalar theta;
281  return log3(quat.derived(), theta);
282  }
283 
291  template<typename Vector3Like, typename Matrix43Like>
292  void Jexp3CoeffWise(
293  const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<Matrix43Like> & Jexp)
294  {
295  // EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
296  assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
297  Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like, Jexp);
298 
299  typedef typename Vector3Like::Scalar Scalar;
300 
301  const Scalar n2 = v.squaredNorm();
302  const Scalar n = math::sqrt(n2);
303  const Scalar theta = Scalar(0.5) * n;
304  const Scalar theta2 = Scalar(0.25) * n2;
305 
306  if (n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
307  {
308  Scalar c, s;
309  SINCOS(theta, &s, &c);
310  Jout.template topRows<3>().noalias() =
311  ((Scalar(0.5) / n2) * (c - 2 * s / n)) * v * v.transpose();
312  Jout.template topRows<3>().diagonal().array() += s / n;
313  Jout.template bottomRows<1>().noalias() = -s / (2 * n) * v.transpose();
314  }
315  else
316  {
317  Jout.template topRows<3>().noalias() =
318  (-Scalar(1) / Scalar(12) + n2 / Scalar(480)) * v * v.transpose();
319  Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2 / 6);
320  Jout.template bottomRows<1>().noalias() =
321  (Scalar(-0.25) * (Scalar(1) - theta2 / 6)) * v.transpose();
322  }
323  }
324 
331  template<typename QuaternionLike, typename Matrix3Like>
332  void Jlog3(
333  const Eigen::QuaternionBase<QuaternionLike> & quat,
334  const Eigen::MatrixBase<Matrix3Like> & Jlog)
335  {
336  typedef typename QuaternionLike::Scalar Scalar;
337  typedef Eigen::Matrix<
338  Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options>
339  Vector3;
340 
341  Scalar t;
342  Vector3 w(log3(quat, t));
343  pinocchio::Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog));
344  }
345  } // namespace quaternion
346 } // namespace pinocchio
347 
348 #endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition: explog-quaternion.hpp:27
pinocchio::quaternion::exp6
void exp6(const MotionDense< MotionDerived > &motion, Eigen::MatrixBase< Config_t > &qout)
The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
Definition: explog-quaternion.hpp:92
pinocchio::quaternion::Jlog3
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
Definition: explog-quaternion.hpp:332
pinocchio::quaternion::log3
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.
Definition: explog-quaternion.hpp:211
pinocchio::quaternion::Jexp3CoeffWise
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
Definition: explog-quaternion.hpp:292
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::motion
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion.
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:240
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27