5 #ifndef __pinocchio_autodiff_casadi_hpp__
6 #define __pinocchio_autodiff_casadi_hpp__
8 #define PINOCCHIO_WITH_CASADI_SUPPORT
10 #include "pinocchio/math/fwd.hpp"
12 #include <casadi/casadi.hpp>
15 namespace boost {
namespace math {
namespace constants {
namespace detail {
17 struct constant_pi<::casadi::SX> : constant_pi<double> {};
19 template<
typename Scalar>
20 struct constant_pi< ::casadi::Matrix<Scalar> > : constant_pi<Scalar> {};
26 inline bool operator||(
const bool x,
const casadi::Matrix<SXElem> & )
34 template<
typename Scalar>
41 static ::casadi::Matrix<Scalar>
precision()
43 return ::casadi::Matrix<Scalar>(Base::template precision<degree>());
54 template<
typename Scalar>
55 struct cast_impl<casadi::SX,Scalar>
57 #if EIGEN_VERSION_AT_LEAST(3,2,90)
60 static inline Scalar run(
const casadi::SX & x)
62 return static_cast<Scalar
>(x);
66 #if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
67 template<
typename Scalar,
bool IsInteger>
68 struct significant_decimals_default_impl< ::casadi::Matrix<Scalar>,IsInteger>
70 static inline int run()
72 return std::numeric_limits<Scalar>::digits10;
83 template<
typename Scalar>
84 struct NumTraits< casadi::Matrix<Scalar> >
86 using Real = casadi::Matrix<Scalar>;
87 using NonInteger = casadi::Matrix<Scalar>;
88 using Literal = casadi::Matrix<Scalar>;
89 using Nested = casadi::Matrix<Scalar>;
99 RequireInitialization = 1 ,
106 static casadi::Matrix<Scalar> epsilon()
108 return casadi::Matrix<Scalar>(std::numeric_limits<double>::epsilon());
111 static casadi::Matrix<Scalar> dummy_precision()
113 return casadi::Matrix<Scalar>(NumTraits<double>::dummy_precision());
116 static casadi::Matrix<Scalar> highest()
118 return casadi::Matrix<Scalar>(std::numeric_limits<double>::max());
121 static casadi::Matrix<Scalar> lowest()
123 return casadi::Matrix<Scalar>(std::numeric_limits<double>::min());
126 static int digits10()
128 return std::numeric_limits<double>::digits10;
138 template<
typename MT,
typename Scalar>
139 inline void copy(::casadi::Matrix<Scalar>
const & src,
140 Eigen::MatrixBase<MT> & dst)
142 Eigen::DenseIndex
const m = src.size1();
143 Eigen::DenseIndex
const n = src.size2();
147 for (Eigen::DenseIndex i = 0; i < m; ++i)
148 for (Eigen::DenseIndex j = 0; j < n; ++j)
149 dst(i, j) = src(i, j);
154 template<
typename MT,
typename Scalar>
155 inline void copy(Eigen::MatrixBase<MT>
const & src,
156 ::casadi::Matrix<Scalar> & dst)
158 Eigen::DenseIndex
const m = src.rows();
159 Eigen::DenseIndex
const n = src.cols();
163 for (Eigen::DenseIndex i = 0; i < m; ++i)
164 for (Eigen::DenseIndex j = 0; j < n; ++j)
165 dst(i, j) = src(i, j);
169 template<
typename MatrixDerived>
170 inline void sym(
const Eigen::MatrixBase<MatrixDerived> & eig_mat,
171 std::string
const &
name)
173 typedef typename MatrixDerived::Scalar SX;
175 MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived,eig_mat);
176 for (Eigen::DenseIndex i = 0; i < eig_mat.rows(); ++i)
177 for (Eigen::DenseIndex j = 0; j < eig_mat.cols(); ++j)
178 eig_mat_(i, j) = SX::sym(
name +
"_" + std::to_string(i) +
"_" + std::to_string(j));
191 template<
typename Scalar>
192 struct return_type_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar>>
194 typedef ::casadi::Matrix<Scalar> type;
197 template<
typename Scalar,
typename T>
198 struct return_type_max< ::casadi::Matrix<Scalar>,T>
200 typedef ::casadi::Matrix<Scalar> type;
203 template<
typename Scalar,
typename T>
204 struct return_type_max<T,::casadi::Matrix<Scalar> >
206 typedef ::casadi::Matrix<Scalar> type;
209 template<
typename Scalar>
210 struct call_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar> >
212 static inline ::casadi::Matrix<Scalar> run(const ::casadi::Matrix<Scalar> & a,
213 const ::casadi::Matrix<Scalar> & b)
214 {
return fmax(a,b); }
217 template<
typename S1,
typename S2>
218 struct call_max< ::casadi::Matrix<S1>,S2>
220 typedef ::casadi::Matrix<S1> CasadiType;
221 static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a,
223 {
return fmax(a,
static_cast<CasadiType
>(b)); }
226 template<
typename S1,
typename S2>
227 struct call_max<S1,::casadi::Matrix<S2>>
229 typedef ::casadi::Matrix<S2> CasadiType;
230 static inline ::casadi::Matrix<S2> run(
const S1 & a,
231 const ::casadi::Matrix<S2> & b)
232 {
return fmax(
static_cast<CasadiType
>(a),b); }
240 #include "pinocchio/autodiff/casadi/spatial/se3-tpl.hpp"
241 #include "pinocchio/autodiff/casadi/utils/static-if.hpp"
242 #include "pinocchio/autodiff/casadi/math/matrix.hpp"
243 #include "pinocchio/autodiff/casadi/math/quaternion.hpp"
245 #endif // #ifndef __pinocchio_autodiff_casadi_hpp__