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