Program Listing for File casadi.hpp
↰ Return to documentation for file (include/pinocchio/autodiff/casadi.hpp
)
//
// Copyright (c) 2019-2024 INRIA CNRS
//
#ifndef __pinocchio_autodiff_casadi_hpp__
#define __pinocchio_autodiff_casadi_hpp__
#define PINOCCHIO_WITH_CASADI_SUPPORT
#include "pinocchio/math/fwd.hpp"
#include <casadi/casadi.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
namespace boost
{
namespace math
{
namespace constants
{
namespace detail
{
template<>
struct constant_pi<::casadi::SX> : constant_pi<double>
{
};
template<typename Scalar>
struct constant_pi<::casadi::Matrix<Scalar>> : constant_pi<Scalar>
{
};
} // namespace detail
} // namespace constants
} // namespace math
} // namespace boost
// This is a workaround to make the code compiling with Eigen.
namespace casadi
{
inline bool operator||(const bool x, const ::casadi::Matrix<SXElem> & /*y*/)
{
return x;
}
} // namespace casadi
namespace pinocchio
{
template<typename Scalar>
struct TaylorSeriesExpansion<::casadi::Matrix<Scalar>> : TaylorSeriesExpansion<Scalar>
{
typedef TaylorSeriesExpansion<Scalar> Base;
template<int degree>
static ::casadi::Matrix<Scalar> precision()
{
return ::casadi::Matrix<Scalar>(Base::template precision<degree>());
}
};
} // namespace pinocchio
namespace Eigen
{
namespace internal
{
// Specialization of Eigen::internal::cast_impl for Casadi input types
template<typename Scalar>
struct cast_impl<::casadi::Matrix<Scalar>, Scalar>
{
#if EIGEN_VERSION_AT_LEAST(3, 2, 90)
EIGEN_DEVICE_FUNC
#endif
static inline Scalar run(const ::casadi::Matrix<Scalar> & x)
{
return static_cast<Scalar>(x);
}
};
#if EIGEN_VERSION_AT_LEAST(3, 2, 90) && !EIGEN_VERSION_AT_LEAST(3, 2, 93)
template<typename Scalar, bool IsInteger>
struct significant_decimals_default_impl<::casadi::Matrix<Scalar>, IsInteger>
{
static inline int run()
{
return std::numeric_limits<Scalar>::digits10;
}
};
#endif
} // namespace internal
} // namespace Eigen
namespace Eigen
{
template<typename Scalar>
struct NumTraits<::casadi::Matrix<Scalar>>
{
using Real = ::casadi::Matrix<Scalar>;
using NonInteger = ::casadi::Matrix<Scalar>;
using Literal = ::casadi::Matrix<Scalar>;
using Nested = ::casadi::Matrix<Scalar>;
enum
{
// does not support complex Base types
IsComplex = 0,
// does not support integer Base types
IsInteger = 0,
// only support signed Base types
IsSigned = 1,
// must initialize an AD<Base> object
RequireInitialization = 1,
// computational cost of the corresponding operations
ReadCost = 1,
AddCost = 2,
MulCost = 2
};
static ::casadi::Matrix<Scalar> epsilon()
{
return ::casadi::Matrix<Scalar>(std::numeric_limits<double>::epsilon());
}
static ::casadi::Matrix<Scalar> dummy_precision()
{
return ::casadi::Matrix<Scalar>(NumTraits<double>::dummy_precision());
}
static ::casadi::Matrix<Scalar> highest()
{
return ::casadi::Matrix<Scalar>(std::numeric_limits<double>::max());
}
static ::casadi::Matrix<Scalar> lowest()
{
return ::casadi::Matrix<Scalar>(std::numeric_limits<double>::min());
}
static int digits10()
{
return std::numeric_limits<double>::digits10;
}
};
} // namespace Eigen
namespace pinocchio
{
namespace casadi
{
// Copy casadi matrix to Eigen matrix
template<typename MT, typename Scalar>
inline void copy(::casadi::Matrix<Scalar> const & src, Eigen::MatrixBase<MT> & dst)
{
Eigen::DenseIndex const m = src.size1();
Eigen::DenseIndex const n = src.size2();
dst.resize(m, n);
for (Eigen::DenseIndex i = 0; i < m; ++i)
for (Eigen::DenseIndex j = 0; j < n; ++j)
dst(i, j) = src(i, j);
}
// Copy casadi matrix to Eigen::Tensor
template<typename TensorDerived, typename Scalar>
inline void copy(::casadi::Matrix<Scalar> const & src, Eigen::TensorBase<TensorDerived> & dst_)
{
TensorDerived & dst = static_cast<TensorDerived &>(dst_);
Eigen::Index const m = src.size1();
Eigen::Index const n = src.size2();
// Eigen::Index const d = 1;
// dst.resize(d, m, n); // TODO(jcarpent) enable the resize depending on the tensor type.
// Otherwise we should throw an error.
for (Eigen::Index i = 0; i < m; ++i)
for (Eigen::Index j = 0; j < n; ++j)
dst(0, i, j) = src(i, j);
}
// Copy Eigen matrix to casadi matrix
template<typename TensorDerived, typename Scalar>
inline void copy(Eigen::TensorBase<TensorDerived> const & _src, ::casadi::Matrix<Scalar> & dst)
{
const TensorDerived & src = static_cast<const TensorDerived &>(_src);
Eigen::Index const m = src.dimension(1);
Eigen::Index const n = src.dimension(2);
PINOCCHIO_CHECK_ARGUMENT_SIZE(src.dimension(0), 1);
dst.resize(m, n);
for (Eigen::Index i = 0; i < m; ++i)
for (Eigen::Index j = 0; j < n; ++j)
dst(i, j) = src(0, i, j);
}
// Copy Eigen matrix to casadi matrix
template<typename MT, typename Scalar>
inline void copy(Eigen::MatrixBase<MT> const & src, ::casadi::Matrix<Scalar> & dst)
{
Eigen::DenseIndex const m = src.rows();
Eigen::DenseIndex const n = src.cols();
dst.resize(m, n);
for (Eigen::DenseIndex i = 0; i < m; ++i)
for (Eigen::DenseIndex j = 0; j < n; ++j)
dst(i, j) = src(i, j);
}
// Copy Eigen sparse matrix to casadi matrix
template<typename MT, typename Scalar>
inline void copy(Eigen::SparseMatrixBase<MT> const & src, ::casadi::Matrix<Scalar> & dst)
{
Eigen::DenseIndex const m = src.rows();
Eigen::DenseIndex const n = src.cols();
dst.resize(m, n);
// TODO this method is not optimal
// (we could copy only non zero values)
for (Eigen::Index i = 0; i < m; ++i)
for (Eigen::Index j = 0; j < n; ++j)
dst(i, j) = src.derived().coeff(i, j);
}
// Make an Eigen matrix consisting of pure casadi symbolics
template<typename MatrixDerived>
inline void sym(const Eigen::MatrixBase<MatrixDerived> & eig_mat, std::string const & name)
{
typedef typename MatrixDerived::Scalar SX;
MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived, eig_mat);
for (Eigen::DenseIndex i = 0; i < eig_mat.rows(); ++i)
for (Eigen::DenseIndex j = 0; j < eig_mat.cols(); ++j)
eig_mat_(i, j) = SX::sym(name + "_" + std::to_string(i) + "_" + std::to_string(j));
}
} // namespace casadi
} // namespace pinocchio
// Overloading of min/max operator
namespace pinocchio
{
namespace math
{
namespace internal
{
template<typename Scalar>
struct return_type_min<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
{
typedef ::casadi::Matrix<Scalar> type;
};
template<typename Scalar, typename T>
struct return_type_min<::casadi::Matrix<Scalar>, T>
{
typedef ::casadi::Matrix<Scalar> type;
};
template<typename Scalar, typename T>
struct return_type_min<T, ::casadi::Matrix<Scalar>>
{
typedef ::casadi::Matrix<Scalar> type;
};
template<typename Scalar>
struct call_min<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
{
static inline ::casadi::Matrix<Scalar>
run(const ::casadi::Matrix<Scalar> & a, const ::casadi::Matrix<Scalar> & b)
{
return fmin(a, b);
}
};
template<typename S1, typename S2>
struct call_min<::casadi::Matrix<S1>, S2>
{
typedef ::casadi::Matrix<S1> CasadiType;
static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a, const S2 & b)
{
return fmin(a, static_cast<CasadiType>(b));
}
};
template<typename S1, typename S2>
struct call_min<S1, ::casadi::Matrix<S2>>
{
typedef ::casadi::Matrix<S2> CasadiType;
static inline ::casadi::Matrix<S2> run(const S1 & a, const ::casadi::Matrix<S2> & b)
{
return fmin(static_cast<CasadiType>(a), b);
}
};
template<typename Scalar>
struct return_type_max<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
{
typedef ::casadi::Matrix<Scalar> type;
};
template<typename Scalar, typename T>
struct return_type_max<::casadi::Matrix<Scalar>, T>
{
typedef ::casadi::Matrix<Scalar> type;
};
template<typename Scalar, typename T>
struct return_type_max<T, ::casadi::Matrix<Scalar>>
{
typedef ::casadi::Matrix<Scalar> type;
};
template<typename Scalar>
struct call_max<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
{
static inline ::casadi::Matrix<Scalar>
run(const ::casadi::Matrix<Scalar> & a, const ::casadi::Matrix<Scalar> & b)
{
return fmax(a, b);
}
};
template<typename S1, typename S2>
struct call_max<::casadi::Matrix<S1>, S2>
{
typedef ::casadi::Matrix<S1> CasadiType;
static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a, const S2 & b)
{
return fmax(a, static_cast<CasadiType>(b));
}
};
template<typename S1, typename S2>
struct call_max<S1, ::casadi::Matrix<S2>>
{
typedef ::casadi::Matrix<S2> CasadiType;
static inline ::casadi::Matrix<S2> run(const S1 & a, const ::casadi::Matrix<S2> & b)
{
return fmax(static_cast<CasadiType>(a), b);
}
};
} // namespace internal
} // namespace math
} // namespace pinocchio
namespace Eigen
{
// Max operator
template<typename Scalar>
struct ScalarBinaryOpTraits<
::casadi::Matrix<Scalar>,
::casadi::Matrix<Scalar>,
internal::scalar_max_op<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>>
{
typedef ::casadi::Matrix<Scalar> ReturnType;
};
template<typename S1, typename S2>
struct ScalarBinaryOpTraits<
::casadi::Matrix<S1>,
S2,
internal::scalar_max_op<::casadi::Matrix<S1>, S2>>
{
typedef ::casadi::Matrix<S1> ReturnType;
};
template<typename S1, typename S2>
struct ScalarBinaryOpTraits<
S1,
::casadi::Matrix<S2>,
internal::scalar_max_op<S1, ::casadi::Matrix<S2>>>
{
typedef ::casadi::Matrix<S1> ReturnType;
};
namespace internal
{
template<typename Scalar>
struct scalar_max_op<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
: binary_op_base<::casadi::Matrix<Scalar>, ::casadi::Matrix<Scalar>>
{
typedef ::casadi::Matrix<Scalar> LhsScalar;
typedef ::casadi::Matrix<Scalar> RhsScalar;
typedef
typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
operator()(const LhsScalar & a, const RhsScalar & b) const
{
return ::pinocchio::math::internal::call_max<LhsScalar, RhsScalar>::run(a, b);
}
// template<typename Packet>
// EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const
// Packet& b) const { return internal::pmax(a,b); } template<typename Packet>
// EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const
// { return internal::predux_max(a); }
};
template<typename S1, typename S2>
struct scalar_max_op<S1, ::casadi::Matrix<S2>> : binary_op_base<S1, ::casadi::Matrix<S2>>
{
typedef S1 LhsScalar;
typedef ::casadi::Matrix<S2> RhsScalar;
typedef
typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
operator()(const LhsScalar & a, const RhsScalar & b) const
{
return ::pinocchio::math::internal::call_max<LhsScalar, RhsScalar>::run(a, b);
}
};
template<typename S1, typename S2>
struct scalar_max_op<::casadi::Matrix<S1>, S2> : binary_op_base<::casadi::Matrix<S1>, S2>
{
typedef ::casadi::Matrix<S1> LhsScalar;
typedef S2 RhsScalar;
typedef
typename ScalarBinaryOpTraits<LhsScalar, RhsScalar, scalar_max_op>::ReturnType result_type;
EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type
operator()(const LhsScalar & a, const RhsScalar & b) const
{
return ::pinocchio::math::internal::call_max<LhsScalar, RhsScalar>::run(a, b);
}
};
} // namespace internal
} // namespace Eigen
#include "pinocchio/autodiff/casadi/spatial/se3-tpl.hpp"
#include "pinocchio/autodiff/casadi/utils/static-if.hpp"
#include "pinocchio/autodiff/casadi/math/matrix.hpp"
#include "pinocchio/autodiff/casadi/math/quaternion.hpp"
#include "pinocchio/autodiff/casadi/math/triangular-matrix.hpp"
#endif // #ifndef __pinocchio_autodiff_casadi_hpp__