17 #ifndef HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH
18 #define HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH
20 #include <pinocchio/multibody/liegroup/vector-space.hpp>
22 #include <hpp/util/exception-factory.hh>
31 template <
bool Test>
struct assign_if {
32 template <
class D1,
class D2>
static void run(
33 const Eigen::MatrixBase<D1> & ,
34 const Eigen::MatrixBase<D2> & )
37 template <>
struct assign_if<true> {
38 template <
class D1,
class D2>
static void run(
39 const Eigen::MatrixBase<D1> & Jin,
40 const Eigen::MatrixBase<D2> & Jout)
42 const_cast<Eigen::MatrixBase<D2>&
> (Jout) = Jin;
48 template<
int Size,
bool rot>
51 typedef ::pinocchio::VectorSpaceOperationTpl<Size, value_type>
Base;
54 NR = (rot ? Size : 0),
65 template <
class ConfigL_t,
class ConfigR_t>
67 const Eigen::MatrixBase<ConfigL_t> & q0,
68 const Eigen::MatrixBase<ConfigR_t> & q1)
70 return Base::squaredDistance(q0, q1);
73 template <
class ConfigL_t,
class ConfigR_t>
75 const Eigen::MatrixBase<ConfigL_t> & q0,
76 const Eigen::MatrixBase<ConfigR_t> & q1,
77 const typename ConfigL_t::Scalar& w)
83 template <
class ConfigIn_t,
class ConfigOut_t>
85 const Eigen::MatrixBase<ConfigIn_t > & bounds,
86 const Eigen::MatrixBase<ConfigOut_t> & out)
89 HPP_THROW(std::invalid_argument,
"Expected vector of size " <<
90 (
int)
BoundSize <<
", got size " << bounds.size());
92 const_cast<Eigen::MatrixBase<ConfigOut_t>&
> (out) = bounds;
95 template <
class JacobianIn_t,
class JacobianOut_t>
97 const Eigen::MatrixBase<JacobianIn_t > & Jin,
98 const Eigen::MatrixBase<JacobianOut_t> & Jout)
100 details::assign_if<rot>::run(Jin, Jout);
103 template <
class ConfigIn_t>
114 template<
int Size,
bool rot>
115 struct traits<
hpp::pinocchio::liegroup::VectorSpaceOperation<Size, rot> > :
116 public traits<typename hpp::pinocchio::liegroup::VectorSpaceOperation<Size, rot>::Base>
120 #endif // HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH