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) 2019-2024 INRIA CNRS
3//
4
5#pragma once
6
7// IWYU pragma: always_keep
8
9#define PINOCCHIO_WITH_CASADI_SUPPORT
10
11// IWYU pragma: begin_keep
12#include <limits>
13#include <string>
14
15#include <casadi/casadi.hpp>
16
17#include <Eigen/Core>
18#include <Eigen/SparseCore>
19#include <unsupported/Eigen/CXX11/Tensor>
20
21#include <boost/math/constants/constants.hpp>
22
23#include "pinocchio/fwd.hpp"
24#include "pinocchio/macros.hpp"
25#include "pinocchio/eigen-common.hpp"
26#include "pinocchio/math.hpp"
27#include "pinocchio/spatial.hpp"
28// IWYU pragma: end_keep
29
30namespace boost
31{
32 namespace math
33 {
34 namespace constants
35 {
36 namespace detail
37 {
38 template<>
39 struct constant_pi<::casadi::SX> : constant_pi<double>
40 {
41 };
42
43 template<typename Scalar>
44 struct constant_pi<::casadi::Matrix<Scalar>> : constant_pi<Scalar>
45 {
46 };
47 } // namespace detail
48 } // namespace constants
49 } // namespace math
50} // namespace boost
51
52// This is a workaround to make the code compiling with Eigen.
53namespace casadi
54{
55 inline bool operator||(const bool x, const ::casadi::Matrix<SXElem> & /*y*/)
56 {
57 return x;
58 }
59} // namespace casadi
60
61namespace Eigen
62{
63 namespace internal
64 {
65 // Specialization of Eigen::internal::cast_impl for Casadi input types
66 template<typename Scalar>
67 struct cast_impl<::casadi::Matrix<Scalar>, Scalar>
68 {
69 EIGEN_DEVICE_FUNC
70 static inline Scalar run(const ::casadi::Matrix<Scalar> & x)
71 {
72 return static_cast<Scalar>(x);
73 }
74 };
75 } // namespace internal
76
77#if EIGEN_VERSION_AT_LEAST(3, 4, 90)
78 // The function Eigen::internal::gemv_dense_selector::run defined in
79 // Eigen/src/Core/GeneralProduct.h call at some point Eigen::numext::is_exactly_zero function.
80 // To avoid this issue we specialize this function for casadi.
81 namespace numext
82 {
83 template<>
84 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool
85 is_exactly_zero(const ::casadi::Matrix<::casadi::SXElem> & x)
86 {
87 return x.is_zero();
88 }
89 } // namespace numext
90#endif // if EIGEN_VERSION_AT_LEAST(3, 4, 90)
91
92} // namespace Eigen
93
94namespace Eigen
95{
98 template<typename Scalar>
99 struct NumTraits<::casadi::Matrix<Scalar>>
100 {
101 using Real = ::casadi::Matrix<Scalar>;
102 using NonInteger = ::casadi::Matrix<Scalar>;
103 using Literal = ::casadi::Matrix<Scalar>;
104 using Nested = ::casadi::Matrix<Scalar>;
105
106 enum
107 {
108 // does not support complex Base types
109 IsComplex = 0,
110 // does not support integer Base types
111 IsInteger = 0,
112 // only support signed Base types
113 IsSigned = 1,
114 // must initialize an AD<Base> object
115 RequireInitialization = 1,
116 // computational cost of the corresponding operations
117 ReadCost = 1,
118 AddCost = 2,
119 MulCost = 2
120 };
121
122 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline ::casadi::Matrix<Scalar> epsilon()
123 {
124 return ::casadi::Matrix<Scalar>(std::numeric_limits<double>::epsilon());
125 }
126
127 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline ::casadi::Matrix<Scalar> dummy_precision()
128 {
129 return ::casadi::Matrix<Scalar>(NumTraits<double>::dummy_precision());
130 }
131
132 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline ::casadi::Matrix<Scalar> highest()
133 {
134 return ::casadi::Matrix<Scalar>(std::numeric_limits<double>::max());
135 }
136
137 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline ::casadi::Matrix<Scalar> lowest()
138 {
139 return ::casadi::Matrix<Scalar>(std::numeric_limits<double>::min());
140 }
141
142 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10()
143 {
144 return std::numeric_limits<double>::digits10;
145 }
146
147 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits()
148 {
149 return NumTraits<double>::digits();
150 }
151
152#if EIGEN_VERSION_AT_LEAST(3, 4, 90)
153 EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10()
154 {
155 return NumTraits<double>::max_digits10();
156 }
157#endif
158 };
159} // namespace Eigen
160
161namespace pinocchio
162{
163 namespace casadi
164 {
165 // Copy casadi matrix to Eigen matrix
166 template<typename MT, typename Scalar>
167 inline void copy(::casadi::Matrix<Scalar> const & src, Eigen::MatrixBase<MT> & dst)
168 {
169 Eigen::Index const m = src.size1();
170 Eigen::Index const n = src.size2();
171
172 dst.resize(m, n);
173
174 for (Eigen::Index i = 0; i < m; ++i)
175 for (Eigen::Index j = 0; j < n; ++j)
176 dst(i, j) = src(i, j);
177 }
178
179 // Copy casadi matrix to Eigen::Tensor
180 template<typename TensorDerived, typename Scalar>
181 inline void copy(::casadi::Matrix<Scalar> const & src, Eigen::TensorBase<TensorDerived> & dst_)
182 {
183 TensorDerived & dst = static_cast<TensorDerived &>(dst_);
184 Eigen::Index const m = src.size1();
185 Eigen::Index const n = src.size2();
186 // Eigen::Index const d = 1;
187 // dst.resize(d, m, n); // TODO(jcarpent) enable the resize depending on the tensor type.
188 // Otherwise we should throw an error.
189
190 for (Eigen::Index i = 0; i < m; ++i)
191 for (Eigen::Index j = 0; j < n; ++j)
192 dst(0, i, j) = src(i, j);
193 }
194
195 // Copy Eigen matrix to casadi matrix
196 template<typename TensorDerived, typename Scalar>
197 inline void copy(Eigen::TensorBase<TensorDerived> const & _src, ::casadi::Matrix<Scalar> & dst)
198 {
199 const TensorDerived & src = static_cast<const TensorDerived &>(_src);
200 Eigen::Index const m = src.dimension(1);
201 Eigen::Index const n = src.dimension(2);
202
203 PINOCCHIO_CHECK_ARGUMENT_SIZE(src.dimension(0), 1);
204 dst.resize(m, n);
205
206 for (Eigen::Index i = 0; i < m; ++i)
207 for (Eigen::Index j = 0; j < n; ++j)
208 dst(i, j) = src(0, i, j);
209 }
210
211 // Copy Eigen matrix to casadi matrix
212 template<typename MT, typename Scalar>
213 inline void copy(Eigen::MatrixBase<MT> const & src, ::casadi::Matrix<Scalar> & dst)
214 {
215 Eigen::Index const m = src.rows();
216 Eigen::Index const n = src.cols();
217
218 dst.resize(m, n);
219
220 for (Eigen::Index i = 0; i < m; ++i)
221 for (Eigen::Index j = 0; j < n; ++j)
222 dst(i, j) = src(i, j);
223 }
224
225 // Copy Eigen sparse matrix to casadi matrix
226 template<typename MT, typename Scalar>
227 inline void copy(Eigen::SparseMatrixBase<MT> const & src, ::casadi::Matrix<Scalar> & dst)
228 {
229 Eigen::Index const m = src.rows();
230 Eigen::Index const n = src.cols();
231
232 dst.resize(m, n);
233
234 // TODO this method is not optimal
235 // (we could copy only non zero values)
236 for (Eigen::Index i = 0; i < m; ++i)
237 for (Eigen::Index j = 0; j < n; ++j)
238 dst(i, j) = src.derived().coeff(i, j);
239 }
240
241 // Make an Eigen matrix consisting of pure casadi symbolics
242 template<typename MatrixDerived>
243 inline void sym(const Eigen::MatrixBase<MatrixDerived> & eig_mat, std::string const & name)
244 {
245 typedef typename MatrixDerived::Scalar SX;
246
247 MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived, eig_mat);
248 for (Eigen::Index i = 0; i < eig_mat.rows(); ++i)
249 for (Eigen::Index j = 0; j < eig_mat.cols(); ++j)
250 eig_mat_(i, j) = SX::sym(name + "_" + std::to_string(i) + "_" + std::to_string(j));
251 }
252
253 } // namespace casadi
254} // namespace pinocchio
255
256// IWYU pragma: begin_exports
257#include "pinocchio/src/autodiff/casadi/math/alias.hxx"
258// IWYU pragma: end_exports
259
260namespace Eigen
261{
262 // Max operator
263 template<typename Scalar>
264 struct ScalarBinaryOpTraits<
265 ::casadi::Matrix<Scalar>,
266 ::casadi::Matrix<Scalar>,
267 internal::scalar_max_op<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>>
268 {
269 typedef ::casadi::Matrix<Scalar> ReturnType;
270 };
271
272 template<typename S1, typename S2>
273 struct ScalarBinaryOpTraits<
274 ::casadi::Matrix<S1>,
275 S2,
276 internal::scalar_max_op<::casadi::Matrix<S1>, S2>>
277 {
278 typedef ::casadi::Matrix<S1> ReturnType;
279 };
280
281 template<typename S1, typename S2>
282 struct ScalarBinaryOpTraits<
283 S1,
284 ::casadi::Matrix<S2>,
285 internal::scalar_max_op<S1, ::casadi::Matrix<S2>>>
286 {
287 typedef ::casadi::Matrix<S1> ReturnType;
288 };
289
290 namespace internal
291 {
292
293 template<typename Scalar>
294 struct scalar_max_op<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
295 : binary_op_base<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
296 {
297 typedef ::casadi::Matrix<Scalar> LhsScalar;
298 typedef ::casadi::Matrix<Scalar> RhsScalar;
299 typedef
300 typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
301
302 EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
303 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
304 operator()(const LhsScalar & a, const RhsScalar & b) const
305 {
306 return ::pinocchio::math::internal::call_max<LhsScalar, RhsScalar>::run(a, b);
307 }
308 };
309
310 template<typename S1, typename S2>
311 struct scalar_max_op<S1, ::casadi::Matrix<S2>> : binary_op_base<S1, ::casadi::Matrix<S2>>
312 {
313 typedef S1 LhsScalar;
314 typedef ::casadi::Matrix<S2> RhsScalar;
315 typedef
316 typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
317
318 EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
319 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
320 operator()(const LhsScalar & a, const RhsScalar & b) const
321 {
322 return ::pinocchio::math::internal::call_max<LhsScalar, RhsScalar>::run(a, b);
323 }
324 };
325
326 template<typename S1, typename S2>
327 struct scalar_max_op<::casadi::Matrix<S1>, S2> : binary_op_base<::casadi::Matrix<S1>, S2>
328 {
329 typedef ::casadi::Matrix<S1> LhsScalar;
330 typedef S2 RhsScalar;
331 typedef
332 typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
333
334 EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
335 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
336 operator()(const LhsScalar & a, const RhsScalar & b) const
337 {
338 return ::pinocchio::math::internal::call_max<LhsScalar, RhsScalar>::run(a, b);
339 }
340 };
341 } // namespace internal
342} // namespace Eigen
343
344// IWYU pragma: begin_exports
345#include "pinocchio/src/autodiff/casadi/math/taylor-series-expansion.hxx"
346#include "pinocchio/src/autodiff/casadi/math/alias.hxx"
347#include "pinocchio/src/autodiff/casadi/math/matrix.hxx"
348#include "pinocchio/src/autodiff/casadi/math/quaternion.hxx"
349#include "pinocchio/src/autodiff/casadi/math/triangular-matrix.hxx"
350#include "pinocchio/src/autodiff/casadi/spatial/se3-tpl.hxx"
351#include "pinocchio/src/autodiff/casadi/utils/static-if.hxx"
352// IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11