autodiff/casadi/math/quaternion.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__
6 #define __pinocchio_autodiff_casadi_math_quaternion_hpp__
7 
9 
10 namespace pinocchio
11 {
12  namespace quaternion
13  {
14  namespace internal
15  {
16 
17  template<typename _Scalar>
18  struct quaternionbase_assign_impl<::casadi::Matrix<_Scalar>, false>
19  {
20  typedef ::casadi::Matrix<_Scalar> Scalar;
21  template<typename Matrix3, typename QuaternionDerived>
22  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
23  {
24  typedef
25  typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
26 
27  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
28  typedef Eigen::Quaternion<Scalar, QuatCoefficientsPlainType::Options> QuaternionPlain;
29  QuaternionPlain quat_t_positive;
30 
31  Scalar t = mat.trace();
33 
34  QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
35 
39 
40  // Build the expression graph
41  const Scalar t_greater_than_zero = t > Scalar(0);
42  const Scalar cond1 = mat.coeff(1, 1) > mat.coeff(0, 0);
43  const Scalar cond2 =
44  (cond1 && mat.coeff(2, 2) > mat.coeff(1, 1)) || (mat.coeff(2, 2) > mat.coeff(0, 0));
45 
46  for (Eigen::DenseIndex k = 0; k < 4; ++k)
47  {
48  Scalar t_is_negative_cond1 = Scalar::if_else(
49  cond1, quat_t_negative_1.coeffs().coeff(k), quat_t_negative_0.coeffs().coeff(k));
50  Scalar t_is_negative_cond2 =
51  Scalar::if_else(cond2, quat_t_negative_2.coeffs().coeff(k), t_is_negative_cond1);
52 
53  q.coeffs().coeffRef(k) = Scalar::if_else(
54  t_greater_than_zero, quat_t_positive.coeffs().coeff(k), t_is_negative_cond2);
55  }
56  }
57  };
58 
59  } // namespace internal
60 
61  } // namespace quaternion
62 
63 } // namespace pinocchio
64 
65 namespace Eigen
66 {
67  namespace internal
68  {
69  template<class Scalar, int Options>
70  struct quaternionbase_assign_impl<Eigen::Matrix<::casadi::Matrix<Scalar>, 3, 3, Options>, 3, 3>
71  {
72  template<typename QuaternionDerived>
73  EIGEN_DEVICE_FUNC static inline void run(
74  QuaternionBase<QuaternionDerived> & q,
75  const Eigen::Matrix<::casadi::Matrix<Scalar>, 3, 3, Options> & a_mat)
76  {
78  ::casadi::Matrix<Scalar>, false>::run(q, a_mat);
79  }
80  };
81  } // namespace internal
82 } // namespace Eigen
83 
84 #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__
quaternion.hpp
Eigen
pinocchio::quaternion::internal::quaternionbase_assign_impl_if_t_negative::run
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
Definition: math/quaternion.hpp:149
pinocchio::quaternion::internal::quaternionbase_assign_impl
Definition: math/quaternion.hpp:142
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: cmake/hpp/idl/omniidl_be_python_with_docstring.py:140
pinocchio::quaternion::internal::quaternionbase_assign_impl<::casadi::Matrix< _Scalar >, false >::run
static void run(Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
Definition: autodiff/casadi/math/quaternion.hpp:22
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
Eigen::internal::quaternionbase_assign_impl< Eigen::Matrix<::casadi::Matrix< Scalar >, 3, 3, Options >, 3, 3 >::run
static EIGEN_DEVICE_FUNC void run(QuaternionBase< QuaternionDerived > &q, const Eigen::Matrix<::casadi::Matrix< Scalar >, 3, 3, Options > &a_mat)
Definition: autodiff/casadi/math/quaternion.hpp:73
pinocchio::quaternion::internal::quaternionbase_assign_impl<::casadi::Matrix< _Scalar >, false >::Scalar
::casadi::Matrix< _Scalar > Scalar
Definition: autodiff/casadi/math/quaternion.hpp:20
pinocchio::quaternion::internal::quaternionbase_assign_impl_if_t_positive::run
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
Definition: math/quaternion.hpp:169
pinocchio::python::Options
@ Options
Definition: expose-contact-inverse-dynamics.cpp:22
mat
mat
q
q
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
t
Transform3f t
pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE
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.
EIGEN_DEVICE_FUNC
#define EIGEN_DEVICE_FUNC
Definition: tensor.hpp:11
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12