pinocchio  3.3.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
quaternion.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_math_quaternion_hpp__
6 #define __pinocchio_math_quaternion_hpp__
7 
8 #ifndef PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
9  #define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE 1e-8
10 #endif
11 
12 #include "pinocchio/math/fwd.hpp"
13 #include "pinocchio/math/comparison-operators.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 #include "pinocchio/math/sincos.hpp"
16 #include "pinocchio/utils/static-if.hpp"
17 
18 #include <boost/type_traits.hpp>
19 #include <Eigen/Geometry>
20 
21 namespace pinocchio
22 {
23  namespace quaternion
24  {
33  template<typename D1, typename D2>
34  typename D1::Scalar angleBetweenQuaternions(
35  const Eigen::QuaternionBase<D1> & q1, const Eigen::QuaternionBase<D2> & q2)
36  {
37  typedef typename D1::Scalar Scalar;
38  const Scalar innerprod = q1.dot(q2);
39  Scalar theta = math::acos(innerprod);
40  static const Scalar PI_value = PI<Scalar>();
41 
42  theta = internal::if_then_else(
43  internal::LT, innerprod, Scalar(0), static_cast<Scalar>(PI_value - theta), theta);
44  return theta;
45  }
46 
56  template<typename D1, typename D2>
58  const Eigen::QuaternionBase<D1> & q1,
59  const Eigen::QuaternionBase<D2> & q2,
60  const typename D1::RealScalar & prec =
61  Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
62  {
63  return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec));
64  }
65 
88  template<typename D>
89  void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
90  {
91  typedef typename D::Scalar Scalar;
92  const Scalar N2 = q.squaredNorm();
93 #ifndef NDEBUG
94  const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
96  static_leq;
97  assert(static_leq::op(math::fabs(static_cast<Scalar>(N2 - Scalar(1))), epsilon));
98 #endif
99  const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
100  PINOCCHIO_EIGEN_CONST_CAST(D, q).coeffs() *= alpha;
101 #ifndef NDEBUG
102  const Scalar M =
103  Scalar(3) * math::pow(Scalar(1) - epsilon, ((Scalar)-Scalar(5)) / Scalar(2)) / Scalar(4);
104  assert(static_leq::op(
105  math::fabs(static_cast<Scalar>(q.norm() - Scalar(1))),
106  math::max(
107  M * sqrt(N2) * (N2 - Scalar(1)) * (N2 - Scalar(1)) / Scalar(2),
108  Eigen::NumTraits<Scalar>::dummy_precision())));
109 #endif
110  }
111 
113  template<typename Derived>
114  void uniformRandom(Eigen::QuaternionBase<Derived> & q)
115  {
116  typedef typename Derived::Scalar Scalar;
117 
118  // Rotational part
119  const Scalar u1 = (Scalar)rand() / RAND_MAX;
120  const Scalar u2 = (Scalar)rand() / RAND_MAX;
121  const Scalar u3 = (Scalar)rand() / RAND_MAX;
122 
123  const Scalar mult1 = sqrt(Scalar(1) - u1);
124  const Scalar mult2 = sqrt(u1);
125 
126  static const Scalar PI_value = PI<Scalar>();
127  Scalar s2, c2;
128  SINCOS(Scalar(2) * PI_value * u2, &s2, &c2);
129  Scalar s3, c3;
130  SINCOS(Scalar(2) * PI_value * u3, &s3, &c3);
131 
132  q.w() = mult1 * s2;
133  q.x() = mult1 * c2;
134  q.y() = mult2 * s3;
135  q.z() = mult2 * c3;
136  }
137 
138  namespace internal
139  {
140 
141  template<typename Scalar, bool value = is_floating_point<Scalar>::value>
142  struct quaternionbase_assign_impl;
143 
144  template<Eigen::DenseIndex i>
145  struct quaternionbase_assign_impl_if_t_negative
146  {
147  template<typename Scalar, typename Matrix3, typename QuaternionDerived>
148  static inline void
149  run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
150  {
151  using pinocchio::math::sqrt;
152 
153  Eigen::DenseIndex j = (i + 1) % 3;
154  Eigen::DenseIndex k = (j + 1) % 3;
155 
156  t = sqrt(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0));
157  q.coeffs().coeffRef(i) = Scalar(0.5) * t;
158  t = Scalar(0.5) / t;
159  q.w() = (mat.coeff(k, j) - mat.coeff(j, k)) * t;
160  q.coeffs().coeffRef(j) = (mat.coeff(j, i) + mat.coeff(i, j)) * t;
161  q.coeffs().coeffRef(k) = (mat.coeff(k, i) + mat.coeff(i, k)) * t;
162  }
163  };
164 
165  struct quaternionbase_assign_impl_if_t_positive
166  {
167  template<typename Scalar, typename Matrix3, typename QuaternionDerived>
168  static inline void
169  run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
170  {
171  using pinocchio::math::sqrt;
172 
173  t = sqrt(t + Scalar(1.0));
174  q.w() = Scalar(0.5) * t;
175  t = Scalar(0.5) / t;
176  q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t;
177  q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * t;
178  q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * t;
179  }
180  };
181 
182  template<typename Scalar>
183  struct quaternionbase_assign_impl<Scalar, true>
184  {
185  template<typename Matrix3, typename QuaternionDerived>
186  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
187  {
188  using pinocchio::math::sqrt;
189 
190  Scalar t = mat.trace();
191  if (t > Scalar(0.))
192  quaternionbase_assign_impl_if_t_positive::run(t, q, mat);
193  else
194  {
195  Eigen::DenseIndex i = 0;
196  if (mat.coeff(1, 1) > mat.coeff(0, 0))
197  i = 1;
198  if (mat.coeff(2, 2) > mat.coeff(i, i))
199  i = 2;
200 
201  if (i == 0)
202  quaternionbase_assign_impl_if_t_negative<0>::run(t, q, mat);
203  else if (i == 1)
204  quaternionbase_assign_impl_if_t_negative<1>::run(t, q, mat);
205  else
206  quaternionbase_assign_impl_if_t_negative<2>::run(t, q, mat);
207  }
208  }
209  };
210 
211  } // namespace internal
212 
213  template<typename D, typename Matrix3>
214  void assignQuaternion(Eigen::QuaternionBase<D> & quat, const Eigen::MatrixBase<Matrix3> & R)
215  {
216  internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(
217  quat.derived(), R.derived());
218  }
219 
228  template<typename Quaternion>
229  inline bool isNormalized(
230  const Eigen::QuaternionBase<Quaternion> & quat,
231  const typename Quaternion::Coefficients::RealScalar & prec =
232  Eigen::NumTraits<typename Quaternion::Coefficients::RealScalar>::dummy_precision())
233  {
234  return pinocchio::isNormalized(quat.coeffs(), prec);
235  }
236 
242  template<typename Quaternion>
243  inline void normalize(const Eigen::QuaternionBase<Quaternion> & quat)
244  {
245  return pinocchio::normalize(quat.const_cast_derived().coeffs());
246  }
247 
254  template<
255  typename Scalar,
256  typename QuaternionIn1,
257  typename QuaternionIn2,
258  typename QuaternionOut>
259  inline void slerp(
260  const Scalar & u,
261  const Eigen::QuaternionBase<QuaternionIn1> & quat0,
262  const Eigen::QuaternionBase<QuaternionIn2> & quat1,
263  const Eigen::QuaternionBase<QuaternionOut> & res)
264  {
265  const Scalar one = Scalar(1) - Eigen::NumTraits<Scalar>::epsilon();
266  const Scalar d = quat0.dot(quat1);
267  const Scalar absD = fabs(d);
268 
269  const Scalar theta = acos(absD);
270  const Scalar sinTheta = sin(theta);
271 
272  using namespace pinocchio::internal;
273 
274  const Scalar scale0 = if_then_else(
275  pinocchio::internal::GE, absD, one,
276  static_cast<Scalar>(Scalar(1) - u), // then
277  static_cast<Scalar>(sin((Scalar(1) - u) * theta) / sinTheta) // else
278  );
279 
280  const Scalar scale1_factor =
281  if_then_else(pinocchio::internal::LT, d, Scalar(0), Scalar(-1), Scalar(1));
282  const Scalar scale1 = if_then_else(
283  pinocchio::internal::GE, absD, one,
284  u, // then
285  static_cast<Scalar>(sin((u * theta)) / sinTheta) // else
286  )
287  * scale1_factor;
288 
289  PINOCCHIO_EIGEN_CONST_CAST(QuaternionOut, res.derived()).coeffs() =
290  scale0 * quat0.coeffs() + scale1 * quat1.coeffs();
291  }
292 
293  } // namespace quaternion
294 
295 } // namespace pinocchio
296 #endif // #ifndef __pinocchio_math_quaternion_hpp__
void slerp(const Scalar &u, const Eigen::QuaternionBase< QuaternionIn1 > &quat0, const Eigen::QuaternionBase< QuaternionIn2 > &quat1, const Eigen::QuaternionBase< QuaternionOut > &res)
Definition: quaternion.hpp:259
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: quaternion.hpp:89
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
Definition: quaternion.hpp:34
bool defineSameRotation(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
Check if two quaternions define the same rotations.
Definition: quaternion.hpp:57
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec=Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
Check whether the input quaternion is Normalized within the given precision.
Definition: quaternion.hpp:229
void normalize(const Eigen::QuaternionBase< Quaternion > &quat)
Normalize the input quaternion.
Definition: quaternion.hpp:243
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:114
Main pinocchio namespace.
Definition: treeview.dox:11
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27