explog-quaternion.hpp
Go to the documentation of this file.
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 
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  enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options };
89  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
90 
91  Vector3 res;
92  const Scalar norm_squared = quat.vec().squaredNorm();
93 
94  static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
95  static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
96  const Scalar norm = math::sqrt(norm_squared + eps * eps);
97 
101 
102  const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0),
103  Scalar(+1),
104  Scalar(-1));
105 
106  Eigen::Quaternion<Scalar, Options> quat_pos;
107  quat_pos.w() = pos_neg * quat.w();
108  quat_pos.vec() = pos_neg * quat.vec();
109 
110  const Scalar theta_2 = math::atan2(norm,quat_pos.w()); // in [0,pi]
111  const Scalar y_x = norm / quat_pos.w(); // nonnegative
112  const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());
113 
114  theta = if_then_else(LT, norm_squared, ts_prec,
115  Scalar(2.)*(Scalar(1) - y_x_sq / Scalar(3)) * y_x,
116  Scalar(2.)*theta_2);
117 
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));
122 
123  for(Eigen::DenseIndex k = 0; k < 3; ++k)
124  {
125  // res[k] = if_then_else(LT, norm_squared, ts_prec,
126  // Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq / Scalar(9)) * pos_neg * quat.vec()[k],
127  // inv_sinc * pos_neg * quat.vec()[k]);
128  res[k] = inv_sinc * quat_pos.vec()[k];
129  }
130  return res;
131  }
132 
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)
145  {
146  typename QuaternionLike::Scalar theta;
147  return log3(quat.derived(),theta);
148  }
149 
156  template<typename Vector3Like, typename Matrix43Like>
157  void Jexp3CoeffWise(const Eigen::MatrixBase<Vector3Like> & v,
158  const Eigen::MatrixBase<Matrix43Like> & Jexp)
159  {
160 // EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
161  assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
162  Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);
163 
164  typedef typename Vector3Like::Scalar Scalar;
165 
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;
170 
171  if(n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
172  {
173  Scalar c, s;
174  SINCOS(theta,&s,&c);
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();
178  }
179  else
180  {
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();
184 
185  }
186  }
187 
194  template<typename QuaternionLike, typename Matrix3Like>
195  void Jlog3(const Eigen::QuaternionBase<QuaternionLike> & quat,
196  const Eigen::MatrixBase<Matrix3Like> & Jlog)
197  {
198  typedef typename QuaternionLike::Scalar Scalar;
199  typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> Vector3;
200 
201  Scalar t;
202  Vector3 w(log3(quat,t));
203  pinocchio::Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
204  }
205  } // namespace quaternion
206 }
207 
208 #endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Vec3f n
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
int eps
Definition: dcrba.py:384
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
Vec3f c
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.
SE3::Scalar Scalar
Definition: conversions.cpp:13
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.
Definition: sincos.hpp:26
Main pinocchio namespace.
Definition: timings.cpp:28
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...
res
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)
Transform3f t
w
Definition: ur5x4.py:45


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29