aba.hpp

//
// Copyright (c) 2019-2020 INRIA CNRS
//
 
#ifndef __pinocchio_autodiff_cppad_algorithm_aba_hpp__
#define __pinocchio_autodiff_cppad_algorithm_aba_hpp__
 
namespace pinocchio
{
  namespace internal
  {
 
    // Fwd def
    template<typename _Scalar>
    struct SE3actOn;
 
    template<typename _Scalar>
    struct SE3actOn<CppAD::AD<_Scalar>>
    {
      typedef CppAD::AD<_Scalar> Scalar;
 
      template<int Options, typename Matrix6Type>
      static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type)
        run(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Type> & I)
      {
        typedef SE3Tpl<Scalar, Options> SE3;
 
        typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
 
        typename SE3::ActionMatrixType dual_action_matrix(M.toDualActionMatrix());
        typename SE3::ActionMatrixType action_matrix(M.toActionMatrixInverse());
        ReturnType intermediate_result = dual_action_matrix * I;
        ReturnType res = intermediate_result * action_matrix;
        return res;
      }
    };
  } // namespace internal
} // namespace pinocchio
 
#endif // __pinocchio_autodiff_cppad_algorithm_aba_hpp__