pinocchio  2.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
aba.hpp
1 //
2 // Copyright (c) 2019-2020 INRIA CNRS
3 //
4 
5 #ifndef __pinocchio_autodiff_cppad_algorithm_aba_hpp__
6 #define __pinocchio_autodiff_cppad_algorithm_aba_hpp__
7 
8 namespace pinocchio
9 {
10  namespace internal
11  {
12 
13  //Fwd def
14  template<typename _Scalar> struct SE3actOn;
15 
17  template<typename _Scalar>
18  struct SE3actOn< CppAD::AD<_Scalar> >
19  {
20  typedef CppAD::AD<_Scalar> Scalar;
21 
22  template<int Options, typename Matrix6Type>
23  static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type)
24  run(const SE3Tpl<Scalar,Options> & M,
25  const Eigen::MatrixBase<Matrix6Type> & I)
26  {
27  typedef SE3Tpl<Scalar,Options> SE3;
28 
29  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
30 
31  typename SE3::ActionMatrixType dual_action_matrix(M.toDualActionMatrix());
32  typename SE3::ActionMatrixType action_matrix(M.toActionMatrixInverse());
33  ReturnType intermediate_result = dual_action_matrix*I;
34  ReturnType res = intermediate_result*action_matrix;
35  return res;
36  }
37  };
38  }
39 }
40 
41 #endif // __pinocchio_autodiff_cppad_algorithm_aba_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24