GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/context/casadi.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 85 131 64.9%
Branches: 34 78 43.6%
Line Branch Exec Source
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
26 namespace eigenpy
27 {
28
29 namespace bp = boost::python;
30
31 namespace casadi
32 {
33
34 struct CasadiType
35 {
36 646 static PyTypeObject * getSXType()
37 {
38 646 return reinterpret_cast<PyTypeObject *>(getInstance().casadi_SX_type.ptr());
39 }
40
41 private:
42 646 static const CasadiType & getInstance()
43 {
44
4/8
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 642 times.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 646 static CasadiType elt;
45 646 return elt;
46 }
47
48 4 CasadiType()
49
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 {
50
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 casadi_module = bp::import("casadi");
51
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 casadi_SX_type = casadi_module.attr("SX");
52 4 Py_INCREF(casadi_module.ptr());
53 4 }
54
55 4 ~CasadiType()
56 {
57 4 casadi_SX_type.~object();
58 // casadi_module.~object();
59 4 }
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
87 /// \brief Determine if pyObj can be converted into a MatType object
88 static void * convertible(PyObject * pyObj);
89
90 /// \brief Allocate memory and copy pyObj in the new storage
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 2686 void * EigenFromPy<
98 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>::
99 convertible(PyObject * pyObj)
100 {
101
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1343 times.
 2686 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 2686 eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
111
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1343 times.
 2686 if (casadi_matrix_swig_obj == NULL)
112 ✗ RETURN_VALUE(0);
113
114 2686 CasadiMatrix * casadi_matrix_ptr =
115 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
116 2686 const CasadiMatrix & casadi_matrix = *casadi_matrix_ptr;
117
118 2686 const casadi_int R = casadi_matrix.rows(), C = casadi_matrix.columns(),
119 2686 size = casadi_matrix.numel();
120
121
5/8
✓ Branch 0 taken 1343 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1343 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1343 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1341 times.
✓ Branch 7 taken 2 times.
 2686 const int ndim = (R == 0 || C == 0) ? 0 : (R == 1 || C == 1) ? 1 : 2;
122
123 if (MatType::IsVectorAtCompileTime)
124 {
125 2684 const Eigen::DenseIndex size_at_compile_time =
126 MatType::IsRowMajor ? MatType::ColsAtCompileTime : MatType::RowsAtCompileTime;
127
128
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1341 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2684 switch (ndim)
129 {
130 ✗ case 0:
131 ✗ RETURN_VALUE(0);
132 2682 case 1: {
133 if (size_at_compile_time != Eigen::Dynamic)
134 {
135 // check that the sizes at compile time matche
136
1/2
✓ Branch 0 taken 113 times.
✗ Branch 1 not taken.
 226 if (size == size_at_compile_time)
137 {
138
2/4
✓ Branch 0 taken 113 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 113 times.
 226 if (MatType::ColsAtCompileTime != C || MatType::RowsAtCompileTime != R)
139 {
140 ✗ RETURN_VALUE(0);
141 }
142 else
143 {
144 226 RETURN_VALUE(pyObj);
145 }
146 }
147 else
148 ✗ RETURN_VALUE(0);
149 }
150 else // This is a dynamic MatType
151 2456 RETURN_VALUE(pyObj);
152 }
153 2 case 2: {
154
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 assert(R > 1 && C > 1);
155 2 RETURN_VALUE(0);
156 }
157 ✗ default:
158 ✗ RETURN_VALUE(0);
159 }
160 }
161 else // this is a matrix
162 {
163
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 if (ndim == 1) // We can always convert a vector into a matrix
164 ✗ RETURN_VALUE(pyObj);
165
166
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (ndim == 2)
167 {
168
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 if ((MatType::RowsAtCompileTime != R) && (MatType::RowsAtCompileTime != Eigen::Dynamic))
169 ✗ RETURN_VALUE(0);
170
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 if ((MatType::ColsAtCompileTime != C) && (MatType::ColsAtCompileTime != Eigen::Dynamic))
171 ✗ RETURN_VALUE(0);
172 }
173 }
174
175 2 RETURN_VALUE(pyObj);
176 #undef RETURN_VALUE
177 }
178
179 template<typename CasadiScalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
180 2684 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 2684 eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
185
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1342 times.
 2684 assert(casadi_matrix_swig_obj != NULL);
186
187 2684 CasadiMatrix * casadi_matrix_ptr =
188 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
189 2684 const CasadiMatrix & casadi_matrix = *casadi_matrix_ptr;
190
191 2684 const casadi_int R = casadi_matrix.rows(), C = casadi_matrix.columns();
192
193 2684 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 2684 void * storage_ptr = storage->storage.bytes;
199 MatType * eigen_matrix_ptr =
200 2684 ::eigenpy::details::init_matrix_or_array<MatType>::run(R, C, storage_ptr);
201
202 // Copy element to matrix
203 2684 pinocchio::casadi::copy(casadi_matrix, *eigen_matrix_ptr);
204
205 2684 memory->convertible = storage->storage.bytes;
206 2684 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 276 void EigenFromPy<
211 Eigen::Matrix<::casadi::Matrix<CasadiScalar>, Rows, Cols, Options, MaxRows, MaxCols>>::
212 registration()
213 {
214 276 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 276 }
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 1228 convert(typename boost::add_reference<typename boost::add_const<MatType>::type>::type mat)
231 {
232
2/4
✓ Branch 1 taken 586 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 586 times.
✗ Branch 5 not taken.
 1228 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 1228 PyObject_CallObject(reinterpret_cast<PyObject *>(casadi::CasadiType::getSXType()), NULL);
238
239 eigenpy::PySwigObject * casadi_matrix_swig_obj =
240 1228 eigenpy::get_PySwigObject(casadi_matrix_py_ptr);
241
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 586 times.
 1228 assert(casadi_matrix_swig_obj != NULL);
242
243 1228 CasadiMatrix * casadi_matrix_obj_ptr =
244 reinterpret_cast<CasadiMatrix *>(casadi_matrix_swig_obj->ptr);
245 1228 pinocchio::casadi::copy(mat, *casadi_matrix_obj_ptr);
246
247 1228 Py_DECREF(reinterpret_cast<PyObject *>(casadi_matrix_swig_obj));
248 1228 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 24 static void run()
264 {
265 24 }
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 16 static void run()
275 {
276 16 }
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
339 namespace 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 4 static void registration()
370 {
371 4 boost::python::to_python_converter<CasadiMatrix, CasadiMatrixToPython, true>();
372 4 }
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 4 static void registration()
387 {
388 4 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 4 }
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 4 inline boost::python::object getScalarType()
450 {
451 namespace bp = boost::python;
452
453 4 PyObject * pyObj = reinterpret_cast<PyObject *>(::eigenpy::casadi::CasadiType::getSXType());
454
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 bp::object scalar_type(bp::handle<>(bp::borrowed(pyObj)));
455
456 4 return scalar_type;
457 }
458
459 4 inline void exposeSpecificTypeFeatures()
460 {
461 typedef pinocchio::python::context::Scalar Scalar;
462 4 CasadiMatrixToPython<Scalar>::registration();
463 4 CasadiMatrixFromPython<Scalar>::registration();
464 4 boost::python::implicitly_convertible<double, Scalar>();
465 4 boost::python::implicitly_convertible<float, Scalar>();
466 4 boost::python::implicitly_convertible<int, Scalar>();
467 4 boost::python::implicitly_convertible<long, Scalar>();
468 4 boost::python::implicitly_convertible<bool, Scalar>();
469 4 };
470
471 } // namespace python
472 } // namespace pinocchio
473
474 namespace 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__
485