pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
explog-quaternion.hpp
1 //
2 // Copyright (c) 2018-2020 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 exp3(const Eigen::MatrixBase<Vector3Like> & v,
27  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 { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options };
34  typedef Eigen::Quaternion<typename QuaternionLike::Scalar,Options> QuaternionPlain;
35 
36  const Scalar t2 = v.squaredNorm();
37  const Scalar t = math::sqrt(t2);
38 
39  static const Scalar ts_prec = math::sqrt(Eigen::NumTraits<Scalar>::epsilon()); // Precision for the Taylor series expansion.
40 
41  Eigen::AngleAxis<Scalar> aa(t,v/t);
42  QuaternionPlain quat_then(aa);
43 
44  QuaternionPlain quat_else;
45  quat_else.vec() = (Scalar(1)/Scalar(2) - t2/48) * v;
46  quat_else.w() = Scalar(1) - t2/8;
47 
48  using ::pinocchio::internal::if_then_else;
49  for(Eigen::DenseIndex k = 0; k < 4; ++k)
50  {
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));
54  }
55 
56  }
57 
65  template<typename Vector3Like>
66  Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
67  exp3(const Eigen::MatrixBase<Vector3Like> & v)
68  {
69  typedef Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
70  ReturnType res; exp3(v,res);
71  return res;
72  }
73 
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)
86  {
87  typedef typename QuaternionLike::Scalar Scalar;
88  typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options> Vector3;
89 
90  Vector3 res;
91  const Scalar norm_squared = quat.vec().squaredNorm();
92  const Scalar norm = math::sqrt(norm_squared);
93  static const Scalar ts_prec = math::sqrt(Eigen::NumTraits<Scalar>::epsilon());
94 
95  const Scalar y_x = norm / quat.w();
96  static const Scalar PI_value = PI<Scalar>();
97  using ::pinocchio::internal::if_then_else;
98  using ::pinocchio::internal::GE;
99  using ::pinocchio::internal::LT;
100 
101  const Scalar theta_2 = if_then_else(GE, quat.w(), Scalar(0),
102  math::atan2(norm,quat.w()),
103  PI_value - math::atan2(norm,quat.w()));
104 
105  const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0),
106  Scalar(+1),
107  Scalar(-1));
108 
109 
110  theta = if_then_else(LT, norm_squared, ts_prec,
111  (Scalar(1) - y_x * y_x / Scalar(3)) * y_x,
112  Scalar(2.)*theta_2);
113  for(Eigen::DenseIndex k = 0; k < 3; ++k)
114  {
115  res[k] = if_then_else(LT, norm_squared, ts_prec,
116  (Scalar(1) + norm_squared / (Scalar(6) * quat.w() * quat.w())) * quat.vec()[k],
117  pos_neg*(theta / math::sin(theta_2)) * quat.vec()[k]);
118  }
119  return res;
120  }
121 
131  template<typename QuaternionLike>
132  Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
133  log3(const Eigen::QuaternionBase<QuaternionLike> & quat)
134  {
135  typename QuaternionLike::Scalar theta;
136  return log3(quat.derived(),theta);
137  }
138 
145  template<typename Vector3Like, typename Matrix43Like>
146  void Jexp3CoeffWise(const Eigen::MatrixBase<Vector3Like> & v,
147  const Eigen::MatrixBase<Matrix43Like> & Jexp)
148  {
149 // EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
150  assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
151  Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);
152 
153  typedef typename Vector3Like::Scalar Scalar;
154 
155  const Scalar n2 = v.squaredNorm();
156  const Scalar n = math::sqrt(n2);
157  const Scalar theta = Scalar(0.5) * n;
158  const Scalar theta2 = Scalar(0.25) * n2;
159 
160  if(n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
161  {
162  Scalar c, s;
163  SINCOS(theta,&s,&c);
164  Jout.template topRows<3>().noalias() = ((Scalar(0.5)/n2) * (c - 2*s/n)) * v * v.transpose();
165  Jout.template topRows<3>().diagonal().array() += s/n;
166  Jout.template bottomRows<1>().noalias() = -s/(2*n) * v.transpose();
167  }
168  else
169  {
170  Jout.template topRows<3>().noalias() = (-Scalar(1)/Scalar(12) + n2/Scalar(480)) * v * v.transpose();
171  Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2/6);
172  Jout.template bottomRows<1>().noalias() = (Scalar(-0.25) * (Scalar(1) - theta2/6)) * v.transpose();
173 
174  }
175  }
176 
183  template<typename QuaternionLike, typename Matrix3Like>
184  void Jlog3(const Eigen::QuaternionBase<QuaternionLike> & quat,
185  const Eigen::MatrixBase<Matrix3Like> & Jlog)
186  {
187  typedef typename QuaternionLike::Scalar Scalar;
188  typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> Vector3;
189 
190  Scalar t;
191  Vector3 w(log3(quat,t));
192  pinocchio::Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
193  }
194  } // namespace quaternion
195 }
196 
197 #endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: explog.hpp:193
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.
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)
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
Main pinocchio namespace.
Definition: treeview.dox:24