pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
casadi.hpp
1//
2// Copyright (c) 2020 INRIA
3//
4
5#ifndef __pinocchio_python_context_casadi_hpp__
6#define __pinocchio_python_context_casadi_hpp__
7
8#include "pinocchio/autodiff/casadi.hpp"
9
10#define PINOCCHIO_PYTHON_SCALAR_TYPE ::casadi::SX
11#include "pinocchio/bindings/python/context/generic.hpp"
12#undef PINOCCHIO_PYTHON_SCALAR_TYPE
13
14#define PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
15#define PINOCCHIO_PYTHON_NO_SERIALIZATION
16#define PINOCCHIO_PYTHON_SKIP_REACHABLE_WORKSPACE
17#define PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
18
19#define PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
20
21#include <eigenpy/eigenpy.hpp>
22#include <eigenpy/user-type.hpp>
23#include <eigenpy/ufunc.hpp>
24#include <eigenpy/swig.hpp>
25
26namespace eigenpy
27{
28
29 namespace bp = boost::python;
30
31 namespace casadi
32 {
33
34 struct CasadiType
35 {
36 static PyTypeObject * getSXType()
37 {
38 return reinterpret_cast<PyTypeObject *>(getInstance().casadi_SX_type.ptr());
39 }
40
41 private:
42 static const CasadiType & getInstance()
43 {
44 static CasadiType elt;
45 return elt;
46 }
47
48 CasadiType()
49 {
50 casadi_module = bp::import("casadi");
51 casadi_SX_type = casadi_module.attr("SX");
52 Py_INCREF(casadi_module.ptr());
53 }
54
55 ~CasadiType()
56 {
57 casadi_SX_type.~object();
58 // casadi_module.~object();
59 }
60
61 bp::object casadi_module;
62 bp::object casadi_SX_type;
63 };
64
65 } // namespace casadi
66
67 template<typename CasadiScalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
68 struct expected_pytype_for_arg<
69 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>,
70 Eigen::MatrixBase<
71 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>>
72 {
73 static PyTypeObject const * get_pytype()
74 {
75 return ::eigenpy::casadi::CasadiType::getSXType();
76 }
77 };
78
79 template<typename CasadiScalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
80 struct EigenFromPy<
81 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>
82 {
83 typedef ::casadi::Matrix<CasadiScalar> CasadiMatrix;
84 typedef Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>
85 MatType;
86
88 static void * convertible(PyObject * pyObj);
89
91 static void construct(PyObject * pyObj, bp::converter::rvalue_from_python_stage1_data * memory);
92
93 static void registration();
94 };
95
96 template<typename CasadiScalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
97 void * EigenFromPy<
98 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>::
99 convertible(PyObject * pyObj)
100 {
101 if (std::strcmp(pyObj->ob_type->tp_name, CasadiMatrix::type_name().c_str()) != 0)
102 return 0;
103
104#define RETURN_VALUE(value) \
105 { \
106 Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj)); \
107 return value; \
108 }
109
110 eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
111 if (casadi_matrix_swig_obj == NULL)
112 RETURN_VALUE(0);
113
114 CasadiMatrix * casadi_matrix_ptr =
115 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
116 const CasadiMatrix & casadi_matrix = *casadi_matrix_ptr;
117
118 const casadi_int R = casadi_matrix.rows(), C = casadi_matrix.columns(),
119 size = casadi_matrix.numel();
120
121 const int ndim = (R == 0 || C == 0) ? 0 : (R == 1 || C == 1) ? 1 : 2;
122
123 if (MatType::IsVectorAtCompileTime)
124 {
125 const Eigen::DenseIndex size_at_compile_time =
126 MatType::IsRowMajor ? MatType::ColsAtCompileTime : MatType::RowsAtCompileTime;
127
128 switch (ndim)
129 {
130 case 0:
131 RETURN_VALUE(0);
132 case 1: {
133 if (size_at_compile_time != Eigen::Dynamic)
134 {
135 // check that the sizes at compile time matche
136 if (size == size_at_compile_time)
137 {
138 if (MatType::ColsAtCompileTime != C || MatType::RowsAtCompileTime != R)
139 {
140 RETURN_VALUE(0);
141 }
142 else
143 {
144 RETURN_VALUE(pyObj);
145 }
146 }
147 else
148 RETURN_VALUE(0);
149 }
150 else // This is a dynamic MatType
151 RETURN_VALUE(pyObj);
152 }
153 case 2: {
154 assert(R > 1 && C > 1);
155 RETURN_VALUE(0);
156 }
157 default:
158 RETURN_VALUE(0);
159 }
160 }
161 else // this is a matrix
162 {
163 if (ndim == 1) // We can always convert a vector into a matrix
164 RETURN_VALUE(pyObj);
165
166 if (ndim == 2)
167 {
168 if ((MatType::RowsAtCompileTime != R) && (MatType::RowsAtCompileTime != Eigen::Dynamic))
169 RETURN_VALUE(0);
170 if ((MatType::ColsAtCompileTime != C) && (MatType::ColsAtCompileTime != Eigen::Dynamic))
171 RETURN_VALUE(0);
172 }
173 }
174
175 RETURN_VALUE(pyObj);
176#undef RETURN_VALUE
177 }
178
179 template<typename CasadiScalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
180 void EigenFromPy<
181 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>::
182 construct(PyObject * pyObj, bp::converter::rvalue_from_python_stage1_data * memory)
183 {
184 eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
185 assert(casadi_matrix_swig_obj != NULL);
186
187 CasadiMatrix * casadi_matrix_ptr =
188 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
189 const CasadiMatrix & casadi_matrix = *casadi_matrix_ptr;
190
191 const casadi_int R = casadi_matrix.rows(), C = casadi_matrix.columns();
192
193 bp::converter::rvalue_from_python_storage<MatType> * storage =
194 reinterpret_cast<bp::converter::rvalue_from_python_storage<MatType> *>(
195 reinterpret_cast<void *>(memory));
196
197 // Allocate memory
198 void * storage_ptr = storage->storage.bytes;
199 MatType * eigen_matrix_ptr =
200 ::eigenpy::details::init_matrix_or_array<MatType>::run(R, C, storage_ptr);
201
202 // Copy element to matrix
203 pinocchio::casadi::copy(casadi_matrix, *eigen_matrix_ptr);
204
205 memory->convertible = storage->storage.bytes;
206 Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj));
207 }
208
209 template<typename CasadiScalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
210 void EigenFromPy<
211 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>::
212 registration()
213 {
214 bp::converter::registry::push_back(
215 reinterpret_cast<void * (*)(_object *)>(&EigenFromPy::convertible), &EigenFromPy::construct,
216 bp::type_id<MatType>()
217#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
218 ,
219 &eigenpy::expected_pytype_for_arg<MatType>::get_pytype
220#endif
221 );
222 }
223
224 template<typename MatType>
225 struct EigenToPy<MatType, ::casadi::Matrix<::casadi::SXElem>>
226 {
227 typedef ::casadi::Matrix<::casadi::SXElem> CasadiMatrix;
228
229 static PyObject *
230 convert(typename boost::add_reference<typename boost::add_const<MatType>::type>::type mat)
231 {
232 assert(
233 (mat.rows() < INT_MAX) && (mat.cols() < INT_MAX)
234 && "Matrix range larger than int ... should never happen.");
235
236 PyObject * casadi_matrix_py_ptr =
237 PyObject_CallObject(reinterpret_cast<PyObject *>(casadi::CasadiType::getSXType()), NULL);
238
239 eigenpy::PySwigObject * casadi_matrix_swig_obj =
240 eigenpy::get_PySwigObject(casadi_matrix_py_ptr);
241 assert(casadi_matrix_swig_obj != NULL);
242
243 CasadiMatrix * casadi_matrix_obj_ptr =
244 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
245 pinocchio::casadi::copy(mat, *casadi_matrix_obj_ptr);
246
247 Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj));
248 return casadi_matrix_py_ptr;
249 }
250
251 static PyTypeObject const * get_pytype()
252 {
253 return ::eigenpy::casadi::CasadiType::getSXType();
254 }
255 };
256
257 template<typename TensorType, typename _Scalar>
258 struct expose_eigen_type_impl<
259 TensorType,
260 Eigen::TensorBase<TensorType>,
261 ::casadi::Matrix<_Scalar>>
262 {
263 static void run()
264 {
265 }
266 };
267
268 template<typename SparseType, typename _Scalar>
269 struct expose_eigen_type_impl<
270 SparseType,
271 Eigen::SparseMatrixBase<SparseType>,
272 ::casadi::Matrix<_Scalar>>
273 {
274 static void run()
275 {
276 }
277 };
278
279 template<typename MatType, int Options, typename Stride>
280 struct EigenToPy<Eigen::Ref<MatType, Options, Stride>, ::casadi::Matrix<::casadi::SXElem>>
281 {
282 typedef ::casadi::Matrix<::casadi::SXElem> CasadiMatrix;
283
284 static PyObject * convert(const Eigen::Ref<MatType, Options, Stride> & mat)
285 {
286 assert(
287 (mat.rows() < INT_MAX) && (mat.cols() < INT_MAX)
288 && "Matrix range larger than int ... should never happen.");
289 PyObject * casadi_matrix_py_ptr =
290 PyObject_CallObject(reinterpret_cast<PyObject *>(casadi::CasadiType::getSXType()), NULL);
291
292 eigenpy::PySwigObject * casadi_matrix_swig_obj =
293 eigenpy::get_PySwigObject(casadi_matrix_py_ptr);
294 assert(casadi_matrix_swig_obj != NULL);
295
296 CasadiMatrix * casadi_matrix_obj_ptr =
297 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
298 pinocchio::casadi::copy(mat.derived(), *casadi_matrix_obj_ptr);
299
300 Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj));
301 return casadi_matrix_py_ptr;
302 }
303
304 static PyTypeObject const * get_pytype()
305 {
306 return ::eigenpy::casadi::CasadiType::getSXType();
307 }
308 };
309
310 namespace internal
311 {
312
313 template<>
314 inline npy_bool SpecialMethods<pinocchio::python::context::Scalar, NPY_USERDEF>::nonzero(
315 void * ip, void * array)
316 {
317
318 typedef pinocchio::python::context::Scalar Scalar;
319 PyArrayObject * py_array = static_cast<PyArrayObject *>(array);
320 if (py_array == NULL || PyArray_ISBEHAVED_RO(py_array))
321 {
322 const Scalar & value = *static_cast<Scalar *>(ip);
323 return (npy_bool)(value.is_zero());
324 }
325 else
326 {
327 Scalar tmp_value;
328 PyArray_Descr * descr = PyArray_DESCR(py_array);
329 PyArray_ArrFuncs * f = PyDataType_GetArrFuncs(descr);
330 f->copyswap(&tmp_value, ip, PyArray_ISBYTESWAPPED(py_array), array);
331 return (npy_bool)(tmp_value.is_zero());
332 }
333 }
334
335 } // namespace internal
336
337} // namespace eigenpy
338
339namespace pinocchio
340{
341 namespace python
342 {
343
344 template<typename CasadiMatrix>
345 struct CasadiMatrixToPython
346 {
347
348 static PyObject * convert(CasadiMatrix const & x)
349 {
350 PyObject * casadi_matrix_py_ptr = PyObject_CallObject(
351 reinterpret_cast<PyObject *>(eigenpy::casadi::CasadiType::getSXType()), NULL);
352 eigenpy::PySwigObject * casadi_matrix_swig_obj =
353 eigenpy::get_PySwigObject(casadi_matrix_py_ptr);
354 assert(casadi_matrix_swig_obj != NULL);
355
356 CasadiMatrix * casadi_matrix_obj_ptr =
357 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
358 *casadi_matrix_obj_ptr = x;
359
360 Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj));
361 return casadi_matrix_py_ptr;
362 }
363
364 static PyTypeObject const * get_pytype()
365 {
366 return ::eigenpy::casadi::CasadiType::getSXType();
367 }
368
369 static void registration()
370 {
371 boost::python::to_python_converter<CasadiMatrix, CasadiMatrixToPython, true>();
372 }
373 };
374
375 template<typename CasadiMatrix>
376 struct CasadiMatrixFromPython
377 {
378 struct Extractor
379 {
380 static CasadiMatrix & execute(PyObject * /*pyObj*/)
381 {
382 throw std::runtime_error("Should never be called");
383 }
384 };
385
386 static void registration()
387 {
388 boost::python::converter::registry::insert(
389 &extract, boost::python::detail::extractor_type_id(&Extractor::execute)
390#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
391 ,
392 &get_pytype
393#endif
394 );
395 }
396
397 private:
398 static void * extract(PyObject * pyObj)
399 {
400 if (!PyObject_TypeCheck(pyObj, ::eigenpy::casadi::CasadiType::getSXType()))
401 return 0;
402
403 eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
404 return casadi_matrix_swig_obj->ptr;
405 }
406#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
407 static PyTypeObject const * get_pytype()
408 {
409 return ::eigenpy::casadi::CasadiType::getSXType();
410 }
411#endif
412 };
413
414 // template<typename CasadiMatrix>
415 // struct CasadiMatrixFromPython
416 // {
417 // static void* convertible(PyObject * pyObj)
418 // {
419 // if(PyFloat_Check(pyObj))
420 // return pyObj;
421 // if(std::strcmp(pyObj->ob_type->tp_name,CasadiMatrix::type_name().c_str()) != 0)
422 // return 0;
423 //
424 // return pyObj;
425 // }
426 // static void construct(PyObject * pyObj,
427 // boost::python::converter::rvalue_from_python_stage1_data * memory)
428 // {
429 // eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
430 // assert(casadi_matrix_swig_obj != NULL);
431 //
432 // CasadiMatrix * casadi_matrix_ptr =
433 // reinterpret_cast<CasadiMatrix*>(casadi_matrix_swig_obj->ptr); const CasadiMatrix &
434 // casadi_matrix = *casadi_matrix_ptr;
435 //
436 // bp::converter::rvalue_from_python_storage<CasadiMatrix>* storage =
437 // reinterpret_cast<bp::converter::rvalue_from_python_storage<CasadiMatrix>*>
438 // (reinterpret_cast<void*>(memory));
439 //
440 // // Allocate memory
441 // void * storage_ptr = storage->storage.bytes;
442 // CasadiMatrix * casadi_matrix_cpp = new (storage_ptr) CasadiMatrix(casadi_matrix);
443 //
444 // memory->convertible = storage->storage.bytes;
445 // Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj));
446 // }
447 // };
448
449 inline boost::python::object getScalarType()
450 {
451 namespace bp = boost::python;
452
453 PyObject * pyObj = reinterpret_cast<PyObject *>(::eigenpy::casadi::CasadiType::getSXType());
454 bp::object scalar_type(bp::handle<>(bp::borrowed(pyObj)));
455
456 return scalar_type;
457 }
458
459 inline void exposeSpecificTypeFeatures()
460 {
461 typedef pinocchio::python::context::Scalar Scalar;
462 CasadiMatrixToPython<Scalar>::registration();
463 CasadiMatrixFromPython<Scalar>::registration();
464 boost::python::implicitly_convertible<double, Scalar>();
465 boost::python::implicitly_convertible<float, Scalar>();
466 boost::python::implicitly_convertible<int, Scalar>();
467 boost::python::implicitly_convertible<long, Scalar>();
468 boost::python::implicitly_convertible<bool, Scalar>();
469 };
470
471 } // namespace python
472} // namespace pinocchio
473
474namespace eigenpy
475{
476
477 template<typename Scalar>
478 struct has_operator_equal<::casadi::Matrix<Scalar>> : boost::false_type
479 {
480 };
481
482} // namespace eigenpy
483
484#endif // #ifndef __pinocchio_python_context_casadi_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11