pinocchio  2.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
quaternion.hpp
1 //
2 // Copyright (c) 2019-2020 INRIA CNRS
3 //
4 
5 #ifndef __pinocchio_autodiff_cppad_math_quaternion_hpp__
6 #define __pinocchio_autodiff_cppad_math_quaternion_hpp__
7 
8 #include "pinocchio/math/quaternion.hpp"
9 
10 namespace pinocchio
11 {
12  namespace quaternion
13  {
14  namespace internal
15  {
16 
17  template<typename _Scalar>
18  struct quaternionbase_assign_impl<CppAD::AD<_Scalar>, false >
19  {
20  typedef _Scalar Scalar;
21  typedef CppAD::AD<Scalar> ADScalar;
22  template<typename Matrix3, typename QuaternionDerived>
23  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
24  const Matrix3 & mat)
25  {
26  typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
27 
28  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
29  typedef Eigen::Quaternion<ADScalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
30  QuaternionPlain quat_t_positive;
31 
32  ADScalar t = mat.trace();
33  quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);
34 
35  QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
36 
37  quaternionbase_assign_impl_if_t_negative<0>::run(t,quat_t_negative_0,mat);
38  quaternionbase_assign_impl_if_t_negative<1>::run(t,quat_t_negative_1,mat);
39  quaternionbase_assign_impl_if_t_negative<2>::run(t,quat_t_negative_2,mat);
40 
41  // Build the expression graph
42  for(Eigen::DenseIndex k = 0; k < 4; ++k)
43  {
44  ADScalar t_is_negative_cond1 = CppAD::CondExpGt<Scalar>(mat.coeff(1,1), mat.coeff(0,0),
45  quat_t_negative_1.coeffs().coeff(k),
46  quat_t_negative_0.coeffs().coeff(k));
47 
48  ADScalar t_is_negative_cond2 = CppAD::CondExpGt<Scalar>(mat.coeff(2,2), mat.coeff(0,0),
49  quat_t_negative_2.coeffs().coeff(k),
50  CppAD::CondExpGt<Scalar>(mat.coeff(1,1), mat.coeff(0,0),
51  CppAD::CondExpGt<Scalar>(mat.coeff(2,2), mat.coeff(1,1),
52  quat_t_negative_2.coeffs().coeff(k),
53  t_is_negative_cond1),
54  t_is_negative_cond1));
55  q.coeffs().coeffRef(k) = CppAD::CondExpGt<Scalar>(t,ADScalar(0),
56  quat_t_positive.coeffs().coeff(k),
57  t_is_negative_cond2);
58  }
59  }
60  };
61 
62  } // namespace internal
63 
64  } // namespace quaternion
65 
66 } // namespace pinocchio
67 
68 #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__
Definition: cppad.hpp:123
Main pinocchio namespace.
Definition: treeview.dox:24