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> {};
double value_type
Definition: fwd.hh:50
Utility functions.
Definition: body.hh:39
Definition: collision-object.hh:40
Definition: vector-space.hh:60
static bool isNormalized(const Eigen::MatrixBase< ConfigIn_t > &, const value_type &)
Definition: vector-space.hh:104
static void setBound(const Eigen::MatrixBase< ConfigIn_t > &bounds, const Eigen::MatrixBase< ConfigOut_t > &out)
Definition: vector-space.hh:86
double squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1)
Definition: vector-space.hh:70
VectorSpaceOperation(int size=std::max(0, Size))
Definition: vector-space.hh:67
::pinocchio::VectorSpaceOperationTpl< Size, value_type > Base
Definition: vector-space.hh:61
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, const typename ConfigL_t::Scalar &w)
Definition: vector-space.hh:76
@ NR
Definition: vector-space.hh:62
@ BoundSize
Definition: vector-space.hh:62
@ NT
Definition: vector-space.hh:62