29 #ifndef HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH
30 #define HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH
33 #include <hpp/util/exception-factory.hh>
34 #include <pinocchio/multibody/liegroup/vector-space.hpp>
43 template <
class D1,
class D2>
44 static void run(
const Eigen::MatrixBase<D1>& ,
45 const Eigen::MatrixBase<D2>& ) {}
48 struct assign_if<true> {
49 template <
class D1,
class D2>
50 static void run(
const Eigen::MatrixBase<D1>& Jin,
51 const Eigen::MatrixBase<D2>& Jout) {
52 const_cast<Eigen::MatrixBase<D2>&
>(Jout) = Jin;
58 template <
int Size,
bool rot>
60 :
public ::pinocchio::VectorSpaceOperationTpl<Size, value_type> {
61 typedef ::pinocchio::VectorSpaceOperationTpl<Size, value_type>
Base;
62 enum {
BoundSize = Size,
NR = (rot ? Size : 0),
NT = (rot ? 0 : Size) };
69 template <
class ConfigL_t,
class ConfigR_t>
71 const Eigen::MatrixBase<ConfigR_t>& q1) {
72 return Base::squaredDistance(q0, q1);
75 template <
class ConfigL_t,
class ConfigR_t>
77 const Eigen::MatrixBase<ConfigR_t>& q1,
78 const typename ConfigL_t::Scalar& w) {
85 template <
class ConfigIn_t,
class ConfigOut_t>
86 static void setBound(
const Eigen::MatrixBase<ConfigIn_t>& bounds,
87 const Eigen::MatrixBase<ConfigOut_t>& out) {
89 HPP_THROW(std::invalid_argument,
"Expected vector of size "
93 const_cast<Eigen::MatrixBase<ConfigOut_t>&
>(out) = bounds;
96 template <
class JacobianIn_t,
class JacobianOut_t>
98 const Eigen::MatrixBase<JacobianIn_t>& Jin,
99 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<
117 Size, rot>::Base> {};
120 #endif // HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH