17 #ifndef HPP_PINOCCHIO_LIEGROUP_SPECIAL_EUCLIDEAN_OPERATION_HH
18 #define HPP_PINOCCHIO_LIEGROUP_SPECIAL_EUCLIDEAN_OPERATION_HH
20 #include <pinocchio/multibody/liegroup/special-euclidean.hpp>
28 typedef ::pinocchio::SpecialEuclideanOperationTpl<N, value_type>
Base;
35 template <
class ConfigL_t,
class ConfigR_t>
37 const Eigen::MatrixBase<ConfigL_t> & q0,
38 const Eigen::MatrixBase<ConfigR_t> & q1)
40 return Base::squaredDistance(q0, q1);
59 template <
class ConfigIn_t,
class ConfigOut_t>
61 const Eigen::MatrixBase<ConfigIn_t > & bound,
62 const Eigen::MatrixBase<ConfigOut_t> & out)
64 if (bound.size() == Base::NQ || bound.size() ==
BoundSize) {
65 const_cast<Eigen::MatrixBase<ConfigOut_t>&
>(out).head(bound.size()) = bound;
67 HPP_THROW(std::invalid_argument,
"Expected vector of size "
69 <<
", got size " << bound.size());
73 template <
class JacobianIn_t,
class JacobianOut_t>
75 const Eigen::MatrixBase<JacobianIn_t > & Jin,
76 const Eigen::MatrixBase<JacobianOut_t> & Jout)
78 const_cast<Eigen::MatrixBase<JacobianOut_t>&
> (Jout) = Jin.template bottomLeftCorner<3,3>();
81 template <
class ConfigIn_t>
84 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
85 return (std::abs(q.template tail<4>().norm() - 1) < eps );
92 #endif // HPP_PINOCCHIO_LIEGROUP_SPECIAL_EUCLIDEAN_OPERATION_HH