autodiff/casadi.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA, CNRS
3 //
4 
5 #ifndef __pinocchio_autodiff_casadi_hpp__
6 #define __pinocchio_autodiff_casadi_hpp__
7 
8 #define PINOCCHIO_WITH_CASADI_SUPPORT
9 
10 #include "pinocchio/math/fwd.hpp"
11 
12 #include <casadi/casadi.hpp>
13 #include <Eigen/Core>
14 
15 namespace boost { namespace math { namespace constants { namespace detail {
16  template<>
17  struct constant_pi<::casadi::SX> : constant_pi<double> {};
18 
19  template<typename Scalar>
20  struct constant_pi< ::casadi::Matrix<Scalar> > : constant_pi<Scalar> {};
21 }}}}
22 
23 // This is a workaround to make the code compiling with Eigen.
24 namespace casadi
25 {
26  inline bool operator||(const bool x, const casadi::Matrix<SXElem> & /*y*/)
27  {
28  return x;
29  }
30 }
31 
32 namespace pinocchio
33 {
34  template<typename Scalar>
35  struct TaylorSeriesExpansion< ::casadi::Matrix<Scalar> >
36  : TaylorSeriesExpansion<Scalar>
37  {
39 
40  template<int degree>
41  static ::casadi::Matrix<Scalar> precision()
42  {
43  return ::casadi::Matrix<Scalar>(Base::template precision<degree>());
44  }
45 
46  };
47 }
48 
49 namespace Eigen
50 {
51  namespace internal
52  {
53  // Specialization of Eigen::internal::cast_impl for Casadi input types
54  template<typename Scalar>
55  struct cast_impl<casadi::SX,Scalar>
56  {
57 #if EIGEN_VERSION_AT_LEAST(3,2,90)
59 #endif
60  static inline Scalar run(const casadi::SX & x)
61  {
62  return static_cast<Scalar>(x);
63  }
64  };
65 
66 #if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
67  template<typename Scalar, bool IsInteger>
68  struct significant_decimals_default_impl< ::casadi::Matrix<Scalar>,IsInteger>
69  {
70  static inline int run()
71  {
72  return std::numeric_limits<Scalar>::digits10;
73  }
74  };
75 #endif
76  }
77 }
78 
79 namespace Eigen
80 {
83  template<typename Scalar>
84  struct NumTraits< casadi::Matrix<Scalar> >
85  {
86  using Real = casadi::Matrix<Scalar>;
87  using NonInteger = casadi::Matrix<Scalar>;
88  using Literal = casadi::Matrix<Scalar>;
89  using Nested = casadi::Matrix<Scalar>;
90 
91  enum {
92  // does not support complex Base types
93  IsComplex = 0 ,
94  // does not support integer Base types
95  IsInteger = 0 ,
96  // only support signed Base types
97  IsSigned = 1 ,
98  // must initialize an AD<Base> object
99  RequireInitialization = 1 ,
100  // computational cost of the corresponding operations
101  ReadCost = 1 ,
102  AddCost = 2 ,
103  MulCost = 2
104  };
105 
106  static casadi::Matrix<Scalar> epsilon()
107  {
108  return casadi::Matrix<Scalar>(std::numeric_limits<double>::epsilon());
109  }
110 
111  static casadi::Matrix<Scalar> dummy_precision()
112  {
113  return casadi::Matrix<Scalar>(NumTraits<double>::dummy_precision());
114  }
115 
116  static casadi::Matrix<Scalar> highest()
117  {
118  return casadi::Matrix<Scalar>(std::numeric_limits<double>::max());
119  }
120 
121  static casadi::Matrix<Scalar> lowest()
122  {
123  return casadi::Matrix<Scalar>(std::numeric_limits<double>::min());
124  }
125 
126  static int digits10()
127  {
128  return std::numeric_limits<double>::digits10;
129  }
130  };
131 } // namespace Eigen
132 
133 namespace pinocchio
134 {
135  namespace casadi
136  {
137  // Copy casadi matrix to Eigen matrix
138  template<typename MT, typename Scalar>
139  inline void copy(::casadi::Matrix<Scalar> const & src,
140  Eigen::MatrixBase<MT> & dst)
141  {
142  Eigen::DenseIndex const m = src.size1();
143  Eigen::DenseIndex const n = src.size2();
144 
145  dst.resize(m, n);
146 
147  for (Eigen::DenseIndex i = 0; i < m; ++i)
148  for (Eigen::DenseIndex j = 0; j < n; ++j)
149  dst(i, j) = src(i, j);
150  }
151 
152 
153  // Copy Eigen matrix to casadi matrix
154  template<typename MT, typename Scalar>
155  inline void copy(Eigen::MatrixBase<MT> const & src,
156  ::casadi::Matrix<Scalar> & dst)
157  {
158  Eigen::DenseIndex const m = src.rows();
159  Eigen::DenseIndex const n = src.cols();
160 
161  dst.resize(m, n);
162 
163  for (Eigen::DenseIndex i = 0; i < m; ++i)
164  for (Eigen::DenseIndex j = 0; j < n; ++j)
165  dst(i, j) = src(i, j);
166  }
167 
168  // Make an Eigen matrix consisting of pure casadi symbolics
169  template<typename MatrixDerived>
170  inline void sym(const Eigen::MatrixBase<MatrixDerived> & eig_mat,
171  std::string const & name)
172  {
173  typedef typename MatrixDerived::Scalar SX;
174 
175  MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived,eig_mat);
176  for (Eigen::DenseIndex i = 0; i < eig_mat.rows(); ++i)
177  for (Eigen::DenseIndex j = 0; j < eig_mat.cols(); ++j)
178  eig_mat_(i, j) = SX::sym(name + "_" + std::to_string(i) + "_" + std::to_string(j));
179  }
180 
181  } // namespace casadi
182 } // namespace pinocchio
183 
184 // Overloading of max operator
185 namespace pinocchio
186 {
187  namespace math
188  {
189  namespace internal
190  {
191  template<typename Scalar>
192  struct return_type_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar>>
193  {
194  typedef ::casadi::Matrix<Scalar> type;
195  };
196 
197  template<typename Scalar, typename T>
198  struct return_type_max< ::casadi::Matrix<Scalar>,T>
199  {
200  typedef ::casadi::Matrix<Scalar> type;
201  };
202 
203  template<typename Scalar, typename T>
204  struct return_type_max<T,::casadi::Matrix<Scalar> >
205  {
206  typedef ::casadi::Matrix<Scalar> type;
207  };
208 
209  template<typename Scalar>
210  struct call_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar> >
211  {
212  static inline ::casadi::Matrix<Scalar> run(const ::casadi::Matrix<Scalar> & a,
213  const ::casadi::Matrix<Scalar> & b)
214  { return fmax(a,b); }
215  };
216 
217  template<typename S1, typename S2>
218  struct call_max< ::casadi::Matrix<S1>,S2>
219  {
220  typedef ::casadi::Matrix<S1> CasadiType;
221  static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a,
222  const S2 & b)
223  { return fmax(a,static_cast<CasadiType>(b)); }
224  };
225 
226  template<typename S1, typename S2>
227  struct call_max<S1,::casadi::Matrix<S2>>
228  {
229  typedef ::casadi::Matrix<S2> CasadiType;
230  static inline ::casadi::Matrix<S2> run(const S1 & a,
231  const ::casadi::Matrix<S2> & b)
232  { return fmax(static_cast<CasadiType>(a),b); }
233  };
234  } // namespace internal
235 
236  } // namespace math
237 
238 } // namespace pinocchio
239 
240 #include "pinocchio/autodiff/casadi/spatial/se3-tpl.hpp"
241 #include "pinocchio/autodiff/casadi/utils/static-if.hpp"
242 #include "pinocchio/autodiff/casadi/math/matrix.hpp"
243 #include "pinocchio/autodiff/casadi/math/quaternion.hpp"
244 
245 #endif // #ifndef __pinocchio_autodiff_casadi_hpp__
JointCollectionTpl const DataTpl< Scalar, Options, JointCollectionTpl > const Eigen::MatrixBase< Mat > & min
Definition: cholesky.hpp:71
Vec3f n
Vec3f b
static inline ::casadi::Matrix< S1 > run(const ::casadi::Matrix< S1 > &a, const S2 &b)
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
void copy(Eigen::MatrixBase< MT > const &src, ::casadi::Matrix< Scalar > &dst)
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
SE3::Scalar Scalar
Definition: conversions.cpp:13
static casadi::Matrix< Scalar > epsilon()
#define EIGEN_DEVICE_FUNC
Definition: tensor.hpp:11
static casadi::Matrix< Scalar > lowest()
static inline ::casadi::Matrix< S2 > run(const S1 &a, const ::casadi::Matrix< S2 > &b)
static inline ::casadi::Matrix< Scalar > run(const ::casadi::Matrix< Scalar > &a, const ::casadi::Matrix< Scalar > &b)
static Scalar run(const casadi::SX &x)
Vec3f a
static casadi::Matrix< Scalar > highest()
float m
static casadi::Matrix< Scalar > dummy_precision()
Main pinocchio namespace.
Definition: timings.cpp:28
bool operator||(const bool x, const casadi::Matrix< SXElem > &)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29