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) {
80 return w * squaredDistance(q0, q1);
82 return squaredDistance(q0, q1);
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) {
88 if (bounds.size() != BoundSize) {
89 HPP_THROW(std::invalid_argument,
"Expected vector of size " 90 << (
int)BoundSize <<
", got 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 ::pinocchio::VectorSpaceOperationTpl< Size, value_type > Base
Definition: vector-space.hh:61
VectorSpaceOperation(int size=std::max(0, Size))
Definition: vector-space.hh:67
Utility functions.
Definition: body.hh:39
static void setBound(const Eigen::MatrixBase< ConfigIn_t > &bounds, const Eigen::MatrixBase< ConfigOut_t > &out)
Definition: vector-space.hh:86
Definition: vector-space.hh:59
double squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const typename ConfigL_t::Scalar &w)
Definition: vector-space.hh:76
double value_type
Definition: fwd.hh:50
Definition: collision-object.hh:40
static bool isNormalized(const Eigen::MatrixBase< ConfigIn_t > &, const value_type &)
Definition: vector-space.hh:104
static void getRotationSubJacobian(const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout)
Definition: vector-space.hh:97
double squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1)
Definition: vector-space.hh:70