5 #ifndef __pinocchio_autodiff_casadi_hpp__ 6 #define __pinocchio_autodiff_casadi_hpp__ 8 #include "pinocchio/math/fwd.hpp" 9 #include "pinocchio/math/sincos.hpp" 10 #include "pinocchio/math/quaternion.hpp" 12 #include <casadi/casadi.hpp> 15 namespace boost {
namespace math {
namespace constants {
namespace detail {
16 template<
typename Scalar>
17 struct constant_pi< ::casadi::Matrix<Scalar> > : constant_pi<double> {};
23 inline bool operator||(
const bool & x,
const casadi::Matrix<SXElem> & )
31 template<
typename Scalar>
36 using Base::precision;
45 template<
typename Scalar>
46 struct cast_impl<casadi::SX,Scalar>
48 #if EIGEN_VERSION_AT_LEAST(3,2,90) 51 static inline Scalar run(
const casadi::SX & x)
53 return static_cast<Scalar
>(x);
57 #if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93) 58 template<
typename Scalar,
bool IsInteger>
59 struct significant_decimals_default_impl<::casadi::Matrix<Scalar>,IsInteger>
61 static inline int run()
63 return std::numeric_limits<Scalar>::digits10;
74 template<
typename Scalar>
75 struct NumTraits<
casadi::Matrix<Scalar> >
77 using Real = casadi::Matrix<Scalar>;
78 using NonInteger = casadi::Matrix<Scalar>;
79 using Literal = casadi::Matrix<Scalar>;
80 using Nested = casadi::Matrix<Scalar>;
90 RequireInitialization = 1 ,
97 static double epsilon()
99 return std::numeric_limits<double>::epsilon();
102 static double dummy_precision()
104 return NumTraits<double>::dummy_precision();
107 static double highest()
109 return std::numeric_limits<double>::max();
112 static double lowest()
114 return std::numeric_limits<double>::min();
117 static int digits10()
119 return std::numeric_limits<double>::digits10;
129 template<
typename MT,
typename Scalar>
130 inline void copy(::casadi::Matrix<Scalar>
const & src,
131 Eigen::MatrixBase<MT> & dst)
133 Eigen::Index
const m = src.size1();
134 Eigen::Index
const n = src.size2();
138 for (Eigen::Index i = 0; i < m; ++i)
139 for (Eigen::Index j = 0; j < n; ++j)
140 dst(i, j) = src(i, j);
145 template<
typename MT,
typename Scalar>
146 inline void copy(Eigen::MatrixBase<MT>
const & src,
147 ::casadi::Matrix<Scalar> & dst)
149 Eigen::Index
const m = src.rows();
150 Eigen::Index
const n = src.cols();
154 for (Eigen::Index i = 0; i < m; ++i)
155 for (Eigen::Index j = 0; j < n; ++j)
156 dst(i, j) = src(i, j);
160 template<
typename MatrixDerived>
161 inline void sym(
const Eigen::MatrixBase<MatrixDerived> & eig_mat,
162 std::string
const & name)
164 typedef typename MatrixDerived::Scalar SX;
166 MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived,eig_mat);
167 for (Eigen::Index i = 0; i < eig_mat.rows(); ++i)
168 for (Eigen::Index j = 0; j < eig_mat.cols(); ++j)
169 eig_mat_(i, j) = SX::sym(name +
"_" + std::to_string(i) +
"_" + std::to_string(j));
175 #include "pinocchio/math/matrix.hpp" 181 template<
typename Scalar>
182 struct CallCorrectMatrixInverseAccordingToScalar< ::casadi::Matrix<Scalar> >
184 typedef ::casadi::Matrix<Scalar> SX;
185 template<
typename MatrixIn,
typename MatrixOut>
186 static void run(
const Eigen::MatrixBase<MatrixIn> & mat,
187 const Eigen::MatrixBase<MatrixOut> & dest)
189 SX cs_mat(mat.rows(),mat.cols());
190 casadi::copy(mat.derived(),cs_mat);
192 SX cs_mat_inv = SX::inv(cs_mat);
194 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
195 casadi::copy(cs_mat_inv,dest_);
210 template<
typename Scalar>
211 struct return_type_max<::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar>>
213 typedef ::casadi::Matrix<Scalar> type;
216 template<
typename Scalar,
typename T>
217 struct return_type_max<::casadi::Matrix<Scalar>,T>
219 typedef ::casadi::Matrix<Scalar> type;
222 template<
typename Scalar,
typename T>
223 struct return_type_max<T,::casadi::Matrix<Scalar> >
225 typedef ::casadi::Matrix<Scalar> type;
228 template<
typename Scalar>
229 struct call_max<::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar> >
231 static inline ::casadi::Matrix<Scalar> run(const ::casadi::Matrix<Scalar> & a,
232 const ::casadi::Matrix<Scalar> & b)
233 {
return fmax(a,b); }
236 template<
typename S1,
typename S2>
237 struct call_max<::casadi::Matrix<S1>,S2>
239 typedef ::casadi::Matrix<S1> CasadiType;
240 static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a,
242 {
return fmax(a,static_cast<CasadiType>(b)); }
245 template<
typename S1,
typename S2>
246 struct call_max<S1,::casadi::Matrix<S2>>
248 typedef ::casadi::Matrix<S2> CasadiType;
249 static inline ::casadi::Matrix<S2> run(
const S1 & a,
250 const ::casadi::Matrix<S2> & b)
251 {
return fmax(static_cast<CasadiType>(a),b); }
259 #include "pinocchio/math/quaternion.hpp" 267 template<
typename _Scalar>
268 struct quaternionbase_assign_impl<::casadi::Matrix<_Scalar> >
270 typedef ::casadi::Matrix<_Scalar> Scalar;
271 template<
typename Matrix3,
typename QuaternionDerived>
272 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
275 typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
277 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
278 typedef Eigen::Quaternion<Scalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
279 QuaternionPlain quat_t_positive;
281 Scalar t = mat.trace();
282 quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);
284 QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
286 quaternionbase_assign_impl_if_t_negative<0>::run(t,quat_t_negative_0,mat);
287 quaternionbase_assign_impl_if_t_negative<1>::run(t,quat_t_negative_1,mat);
288 quaternionbase_assign_impl_if_t_negative<2>::run(t,quat_t_negative_2,mat);
291 const Scalar t_greater_than_zero = t > Scalar(0);
292 const Scalar cond1 = mat.coeff(1,1) > mat.coeff(0,0);
293 const Scalar cond2 = (cond1 && mat.coeff(2,2) > mat.coeff(1,1)) || (mat.coeff(2,2) > mat.coeff(0,0));
295 for(Eigen::DenseIndex k = 0; k < 4; ++k)
297 Scalar t_is_negative_cond1 = Scalar::if_else(cond1,
298 quat_t_negative_1.coeffs().coeff(k),
299 quat_t_negative_0.coeffs().coeff(k));
300 Scalar t_is_negative_cond2 = Scalar::if_else(cond2,
301 quat_t_negative_2.coeffs().coeff(k),
302 t_is_negative_cond1);
304 q.coeffs().coeffRef(k) = Scalar::if_else(t_greater_than_zero,
305 quat_t_positive.coeffs().coeff(k),
306 t_is_negative_cond2);
335 #include "pinocchio/utils/static-if.hpp" 353 template<
typename Scalar,
typename then_type,
typename else_type>
354 struct if_then_else_impl<::casadi::Matrix<Scalar>,then_type,else_type>
356 typedef typename traits<if_then_else_impl>::ReturnType ReturnType;
358 static inline ReturnType run(const ::casadi::Matrix<Scalar> & condition,
359 const then_type & then_value,
360 const else_type & else_value)
362 return ReturnType::if_else(condition,then_value,else_value);
368 #endif // #ifndef __pinocchio_autodiff_casadi_hpp__
Source from #include <cppad/example/cppad_eigen.hpp>
Main pinocchio namespace.