17 #ifndef HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH
18 #define HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH
20 #include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
28 typedef ::pinocchio::SpecialOrthogonalOperationTpl<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);
43 template <
class ConfigL_t,
class ConfigR_t>
45 const Eigen::MatrixBase<ConfigL_t> & q0,
46 const Eigen::MatrixBase<ConfigR_t> & q1,
47 const typename ConfigL_t::Scalar& w)
52 template <
class ConfigIn_t,
class ConfigOut_t>
54 const Eigen::MatrixBase<ConfigIn_t > & bound,
55 const Eigen::MatrixBase<ConfigOut_t> & out)
57 if (bound.size() == 0)
return;
58 if (bound.size() != Base::NQ) {
59 HPP_THROW(std::invalid_argument,
"Expected vector of size 0 or "
60 << (
int)Base::NQ <<
", got size " << bound.size());
62 const_cast<Eigen::MatrixBase<ConfigOut_t>&
>(out).head(bound.size()) = bound;
65 template <
class JacobianIn_t,
class JacobianOut_t>
67 const Eigen::MatrixBase<JacobianIn_t > & Jin,
68 const Eigen::MatrixBase<JacobianOut_t> & Jout)
70 const_cast<Eigen::MatrixBase<JacobianOut_t>&
> (Jout) = Jin;
73 template <
class ConfigIn_t>
76 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
77 return (std::abs(q.norm() - 1) < eps );
84 #endif // HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH