pinocchio  2.4.5
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
matrix.hpp
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #ifndef __pinocchio_autodiff_casadi_math_matrix_hpp__
6 #define __pinocchio_autodiff_casadi_math_matrix_hpp__
7 
8 #include "pinocchio/math/matrix.hpp"
9 
10 namespace pinocchio
11 {
12  namespace internal
13  {
14  template<typename Scalar>
15  struct CallCorrectMatrixInverseAccordingToScalar< ::casadi::Matrix<Scalar> >
16  {
17  typedef ::casadi::Matrix<Scalar> SX;
18  template<typename MatrixIn, typename MatrixOut>
19  static void run(const Eigen::MatrixBase<MatrixIn> & mat,
20  const Eigen::MatrixBase<MatrixOut> & dest)
21  {
22  SX cs_mat(mat.rows(),mat.cols());
23  casadi::copy(mat.derived(),cs_mat);
24 
25  SX cs_mat_inv = SX::inv(cs_mat);
26 
27  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
28  casadi::copy(cs_mat_inv,dest_);
29  }
30 
31  };
32  }
33 } // namespace pinocchio
34 
35 #endif // ifndef __pinocchio_autodiff_casadi_math_matrix_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24