pinocchio  2.3.1-dirty
autodiff/casadi.hpp
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #ifndef __pinocchio_autodiff_casadi_hpp__
6 #define __pinocchio_autodiff_casadi_hpp__
7 
8 #include "pinocchio/math/fwd.hpp"
9 #include "pinocchio/math/sincos.hpp"
10 #include "pinocchio/math/quaternion.hpp"
11 
12 #include <casadi/casadi.hpp>
13 #include <Eigen/Core>
14 
15 namespace boost { namespace math { namespace constants { namespace detail {
16  template<typename Scalar>
17  struct constant_pi< ::casadi::Matrix<Scalar> > : constant_pi<double> {};
18 }}}}
19 
20 // This is a workaround to make the code compiling with Eigen.
21 namespace casadi
22 {
23  inline bool operator||(const bool & x, const casadi::Matrix<SXElem> & /*y*/)
24  {
25  return x;
26  }
27 }
28 
29 namespace pinocchio
30 {
31  template<typename Scalar>
32  struct TaylorSeriesExpansion< ::casadi::Matrix<Scalar> >
33  : TaylorSeriesExpansion<Scalar>
34  {
36  using Base::precision;
37  };
38 }
39 
40 namespace Eigen
41 {
42  namespace internal
43  {
44  // Specialization of Eigen::internal::cast_impl for Casadi input types
45  template<typename Scalar>
46  struct cast_impl<casadi::SX,Scalar>
47  {
48 #if EIGEN_VERSION_AT_LEAST(3,2,90)
49  EIGEN_DEVICE_FUNC
50 #endif
51  static inline Scalar run(const casadi::SX & x)
52  {
53  return static_cast<Scalar>(x);
54  }
55  };
56 
57 #if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
58  template<typename Scalar, bool IsInteger>
59  struct significant_decimals_default_impl< ::casadi::Matrix<Scalar>,IsInteger>
60  {
61  static inline int run()
62  {
63  return std::numeric_limits<Scalar>::digits10;
64  }
65  };
66 #endif
67  }
68 }
69 
70 namespace Eigen
71 {
74  template<typename Scalar>
75  struct NumTraits< casadi::Matrix<Scalar> >
76  {
77  using Real = casadi::Matrix<Scalar>;
78  using NonInteger = casadi::Matrix<Scalar>;
79  using Literal = casadi::Matrix<Scalar>;
80  using Nested = casadi::Matrix<Scalar>;
81 
82  enum {
83  // does not support complex Base types
84  IsComplex = 0 ,
85  // does not support integer Base types
86  IsInteger = 0 ,
87  // only support signed Base types
88  IsSigned = 1 ,
89  // must initialize an AD<Base> object
90  RequireInitialization = 1 ,
91  // computational cost of the corresponding operations
92  ReadCost = 1 ,
93  AddCost = 2 ,
94  MulCost = 2
95  };
96 
97  static double epsilon()
98  {
99  return std::numeric_limits<double>::epsilon();
100  }
101 
102  static double dummy_precision()
103  {
104  return NumTraits<double>::dummy_precision();
105  }
106 
107  static double highest()
108  {
109  return std::numeric_limits<double>::max();
110  }
111 
112  static double lowest()
113  {
114  return std::numeric_limits<double>::min();
115  }
116 
117  static int digits10()
118  {
119  return std::numeric_limits<double>::digits10;
120  }
121  };
122 } // namespace Eigen
123 
124 namespace pinocchio
125 {
126  namespace casadi
127  {
128  // Copy casadi matrix to Eigen matrix
129  template<typename MT, typename Scalar>
130  inline void copy(::casadi::Matrix<Scalar> const & src,
131  Eigen::MatrixBase<MT> & dst)
132  {
133  Eigen::Index const m = src.size1();
134  Eigen::Index const n = src.size2();
135 
136  dst.resize(m, n);
137 
138  for (Eigen::Index i = 0; i < m; ++i)
139  for (Eigen::Index j = 0; j < n; ++j)
140  dst(i, j) = src(i, j);
141  }
142 
143 
144  // Copy Eigen matrix to casadi matrix
145  template<typename MT, typename Scalar>
146  inline void copy(Eigen::MatrixBase<MT> const & src,
147  ::casadi::Matrix<Scalar> & dst)
148  {
149  Eigen::Index const m = src.rows();
150  Eigen::Index const n = src.cols();
151 
152  dst.resize(m, n);
153 
154  for (Eigen::Index i = 0; i < m; ++i)
155  for (Eigen::Index j = 0; j < n; ++j)
156  dst(i, j) = src(i, j);
157  }
158 
159  // Make an Eigen matrix consisting of pure casadi symbolics
160  template<typename MatrixDerived>
161  inline void sym(const Eigen::MatrixBase<MatrixDerived> & eig_mat,
162  std::string const & name)
163  {
164  typedef typename MatrixDerived::Scalar SX;
165 
166  MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived,eig_mat);
167  for (Eigen::Index i = 0; i < eig_mat.rows(); ++i)
168  for (Eigen::Index j = 0; j < eig_mat.cols(); ++j)
169  eig_mat_(i, j) = SX::sym(name + "_" + std::to_string(i) + "_" + std::to_string(j));
170  }
171 
172  } // namespace casadi
173 } // namespace pinocchio
174 
175 #include "pinocchio/math/matrix.hpp"
176 
177 namespace pinocchio
178 {
179  namespace internal
180  {
181  template<typename Scalar>
182  struct CallCorrectMatrixInverseAccordingToScalar< ::casadi::Matrix<Scalar> >
183  {
184  typedef ::casadi::Matrix<Scalar> SX;
185  template<typename MatrixIn, typename MatrixOut>
186  static void run(const Eigen::MatrixBase<MatrixIn> & mat,
187  const Eigen::MatrixBase<MatrixOut> & dest)
188  {
189  SX cs_mat(mat.rows(),mat.cols());
190  casadi::copy(mat.derived(),cs_mat);
191 
192  SX cs_mat_inv = SX::inv(cs_mat);
193 
194  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
195  casadi::copy(cs_mat_inv,dest_);
196  }
197 
198  };
199 
200  }
201 }
202 
204 namespace pinocchio
205 {
206  namespace math
207  {
208  namespace internal
209  {
210  template<typename Scalar>
211  struct return_type_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar>>
212  {
213  typedef ::casadi::Matrix<Scalar> type;
214  };
215 
216  template<typename Scalar, typename T>
217  struct return_type_max< ::casadi::Matrix<Scalar>,T>
218  {
219  typedef ::casadi::Matrix<Scalar> type;
220  };
221 
222  template<typename Scalar, typename T>
223  struct return_type_max<T,::casadi::Matrix<Scalar> >
224  {
225  typedef ::casadi::Matrix<Scalar> type;
226  };
227 
228  template<typename Scalar>
229  struct call_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar> >
230  {
231  static inline ::casadi::Matrix<Scalar> run(const ::casadi::Matrix<Scalar> & a,
232  const ::casadi::Matrix<Scalar> & b)
233  { return fmax(a,b); }
234  };
235 
236  template<typename S1, typename S2>
237  struct call_max< ::casadi::Matrix<S1>,S2>
238  {
239  typedef ::casadi::Matrix<S1> CasadiType;
240  static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a,
241  const S2 & b)
242  { return fmax(a,static_cast<CasadiType>(b)); }
243  };
244 
245  template<typename S1, typename S2>
246  struct call_max<S1,::casadi::Matrix<S2>>
247  {
248  typedef ::casadi::Matrix<S2> CasadiType;
249  static inline ::casadi::Matrix<S2> run(const S1 & a,
250  const ::casadi::Matrix<S2> & b)
251  { return fmax(static_cast<CasadiType>(a),b); }
252  };
253  } // namespace internal
254 
255  } // namespace math
256 
257 } // namespace pinocchio
258 
259 #include "pinocchio/math/quaternion.hpp"
260 namespace pinocchio
261 {
262  namespace quaternion
263  {
264  namespace internal
265  {
266 
267  template<typename _Scalar>
268  struct quaternionbase_assign_impl< ::casadi::Matrix<_Scalar> >
269  {
270  typedef ::casadi::Matrix<_Scalar> Scalar;
271  template<typename Matrix3, typename QuaternionDerived>
272  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
273  const Matrix3 & mat)
274  {
275  typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
276 
277  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
278  typedef Eigen::Quaternion<Scalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
279  QuaternionPlain quat_t_positive;
280 
281  Scalar t = mat.trace();
282  quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);
283 
284  QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
285 
286  quaternionbase_assign_impl_if_t_negative<0>::run(t,quat_t_negative_0,mat);
287  quaternionbase_assign_impl_if_t_negative<1>::run(t,quat_t_negative_1,mat);
288  quaternionbase_assign_impl_if_t_negative<2>::run(t,quat_t_negative_2,mat);
289 
290  // Build the expression graph
291  const Scalar t_greater_than_zero = t > Scalar(0);
292  const Scalar cond1 = mat.coeff(1,1) > mat.coeff(0,0);
293  const Scalar cond2 = (cond1 && mat.coeff(2,2) > mat.coeff(1,1)) || (mat.coeff(2,2) > mat.coeff(0,0));
294 
295  for(Eigen::DenseIndex k = 0; k < 4; ++k)
296  {
297  Scalar t_is_negative_cond1 = Scalar::if_else(cond1,
298  quat_t_negative_1.coeffs().coeff(k),
299  quat_t_negative_0.coeffs().coeff(k));
300  Scalar t_is_negative_cond2 = Scalar::if_else(cond2,
301  quat_t_negative_2.coeffs().coeff(k),
302  t_is_negative_cond1);
303 
304  q.coeffs().coeffRef(k) = Scalar::if_else(t_greater_than_zero,
305  quat_t_positive.coeffs().coeff(k),
306  t_is_negative_cond2);
307  }
308 
309 // if (t > Scalar(0))
310 // quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
311 // else
312 // {
313 // Eigen::DenseIndex i = 0;
314 // if (mat.coeff(1,1) > mat.coeff(0,0))
315 // i = 1;
316 // if (mat.coeff(2,2) > mat.coeff(i,i))
317 // i = 2;
318 //
319 // if(i==0)
320 // quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
321 // else if(i==1)
322 // quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
323 // else
324 // quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
325 // }
326  }
327  };
328 
329  } // namespace internal
330 
331  } // namespace quaternion
332 
333 } // namespace pinocchio
334 
335 #include "pinocchio/utils/static-if.hpp"
336 
337 namespace pinocchio
338 {
339  namespace internal
340  {
341 // template<typename if_type, typename Scalar, typename else_type>
342 // struct traits<if_then_else_impl<if_type,::casadi::Matrix<Scalar>,else_type> >
343 // {
344 // typedef ::casadi::Matrix<Scalar> ReturnType;
345 // };
346 //
347 // template<typename if_type, typename Scalar, typename then_type>
348 // struct traits<if_then_else_impl<if_type,then_type,::casadi::Matrix<Scalar>> >
349 // {
350 // typedef ::casadi::Matrix<Scalar> ReturnType;
351 // };
352 
353  template<typename Scalar, typename then_type, typename else_type>
354  struct if_then_else_impl< ::casadi::Matrix<Scalar>,then_type,else_type>
355  {
356  typedef typename traits<if_then_else_impl>::ReturnType ReturnType;
357 
358  static inline ReturnType run(const ::casadi::Matrix<Scalar> & condition,
359  const then_type & then_value,
360  const else_type & else_value)
361  {
362  return ReturnType::if_else(condition,then_value,else_value);
363  }
364  };
365  }
366 }
367 
368 #endif // #ifndef __pinocchio_autodiff_casadi_hpp__
Source from #include <cppad/example/cppad_eigen.hpp>
Main pinocchio namespace.
Definition: treeview.dox:24