pinocchio  2.4.4
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
35  angleBetweenQuaternions(const Eigen::QuaternionBase<D1> & q1,
36  const Eigen::QuaternionBase<D2> & q2)
37  {
38  typedef typename D1::Scalar Scalar;
39  const Scalar innerprod = q1.dot(q2);
40  Scalar theta = math::acos(innerprod);
41  static const Scalar PI_value = PI<Scalar>();
42 
43  theta = internal::if_then_else(internal::LT, innerprod, Scalar(0),
44  PI_value - theta,
45  theta);
46  return theta;
47  }
48 
58  template<typename D1, typename D2>
59  bool defineSameRotation(const Eigen::QuaternionBase<D1> & q1,
60  const Eigen::QuaternionBase<D2> & q2,
61  const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
62  {
63  return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
64  }
65 
87  template<typename D>
88  void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
89  {
90  typedef typename D::Scalar Scalar;
91  const Scalar N2 = q.squaredNorm();
92 #ifndef NDEBUG
93  const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
95  assert(static_leq::op(math::fabs(N2-1.), epsilon));
96 #endif
97  const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
98  PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
99 #ifndef NDEBUG
100  const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
101  assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
102  math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
103 #endif
104  }
105 
107  template<typename Derived>
108  void uniformRandom(const Eigen::QuaternionBase<Derived> & q)
109  {
110  typedef typename Derived::Scalar Scalar;
111 
112  // Rotational part
113  const Scalar u1 = (Scalar)rand() / RAND_MAX;
114  const Scalar u2 = (Scalar)rand() / RAND_MAX;
115  const Scalar u3 = (Scalar)rand() / RAND_MAX;
116 
117  const Scalar mult1 = sqrt(Scalar(1)-u1);
118  const Scalar mult2 = sqrt(u1);
119 
120  static const Scalar PI_value = PI<Scalar>();
121  Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2);
122  Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3);
123 
124  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).w() = mult1 * s2;
125  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).x() = mult1 * c2;
126  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).y() = mult2 * s3;
127  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).z() = mult2 * c3;
128  }
129 
130  namespace internal
131  {
132 
133  template<typename Scalar, bool value = boost::is_floating_point<Scalar>::value>
134  struct quaternionbase_assign_impl;
135 
136  template<Eigen::DenseIndex i>
137  struct quaternionbase_assign_impl_if_t_negative
138  {
139  template<typename Scalar, typename Matrix3, typename QuaternionDerived>
140  static inline void run(Scalar t,
141  Eigen::QuaternionBase<QuaternionDerived> & q,
142  const Matrix3 & mat)
143  {
144  using pinocchio::math::sqrt;
145 
146  Eigen::DenseIndex j = (i+1)%3;
147  Eigen::DenseIndex k = (j+1)%3;
148 
149  t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
150  q.coeffs().coeffRef(i) = Scalar(0.5) * t;
151  t = Scalar(0.5)/t;
152  q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
153  q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
154  q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
155  }
156  };
157 
158  struct quaternionbase_assign_impl_if_t_positive
159  {
160  template<typename Scalar, typename Matrix3, typename QuaternionDerived>
161  static inline void run(Scalar t,
162  Eigen::QuaternionBase<QuaternionDerived> & q,
163  const Matrix3 & mat)
164  {
165  using pinocchio::math::sqrt;
166 
167  t = sqrt(t + Scalar(1.0));
168  q.w() = Scalar(0.5)*t;
169  t = Scalar(0.5)/t;
170  q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
171  q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
172  q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
173  }
174  };
175 
176  template<typename Scalar>
177  struct quaternionbase_assign_impl<Scalar, true>
178  {
179  template<typename Matrix3, typename QuaternionDerived>
180  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
181  const Matrix3 & mat)
182  {
183  using pinocchio::math::sqrt;
184 
185  Scalar t = mat.trace();
186  if (t > Scalar(0.))
187  quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
188  else
189  {
190  Eigen::DenseIndex i = 0;
191  if (mat.coeff(1,1) > mat.coeff(0,0))
192  i = 1;
193  if (mat.coeff(2,2) > mat.coeff(i,i))
194  i = 2;
195 
196  if(i==0)
197  quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
198  else if(i==1)
199  quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
200  else
201  quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
202  }
203  }
204  };
205 
206  } // namespace internal
207 
208  template<typename D, typename Matrix3>
209  void assignQuaternion(Eigen::QuaternionBase<D> & quat,
210  const Eigen::MatrixBase<Matrix3> & R)
211  {
212  internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
213  R.derived());
214  }
215 
224  template<typename Quaternion>
225  inline bool isNormalized(const Eigen::QuaternionBase<Quaternion> & quat,
226  const typename Quaternion::Coefficients::RealScalar & prec =
227  Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
228  {
229  return pinocchio::isNormalized(quat.coeffs(),prec);
230  }
231 
232  } // namespace quaternion
233 
234 }
235 #endif //#ifndef __pinocchio_math_quaternion_hpp__
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:59
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
Definition: quaternion.hpp:35
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: quaternion.hpp:88
bool isNormalized(const Eigen::MatrixBase< VectorLike > &vec, const typename VectorLike::RealScalar &prec=Eigen::NumTraits< typename VectorLike::Scalar >::dummy_precision())
Check whether the input vector is Normalized within the given precision.
Definition: matrix.hpp:148
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:108
Main pinocchio namespace.
Definition: treeview.dox:24
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:225