5 #ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__ 6 #define __pinocchio_multibody_liegroup_vector_space_operation_hpp__ 8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp" 11 #include <boost/integer/static_min_max.hpp> 17 template<
int Dim,
typename _Scalar,
int _Options>
28 template<
int Dim,
typename _Scalar,
int _Options>
30 :
public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
40 assert(size_.value() >= 0);
48 assert(size_.value() >= 0);
53 size_.setValue(other.
size_.value());
54 assert(size_.value() >= 0);
69 return ConfigVector_t::Zero(size_.value());
74 std::ostringstream oss; oss <<
"R^" <<
nq();
78 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
80 const Eigen::MatrixBase<ConfigR_t> &
q1,
81 const Eigen::MatrixBase<Tangent_t> &
d)
86 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
88 const Eigen::MatrixBase<ConfigR_t> &,
89 const Eigen::MatrixBase<JacobianOut_t> &
J)
const 97 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
100 const JacobianIn_t & Jin,
101 JacobianOut_t & Jout,
107 if (arg ==
ARG0) Jout = - Jin;
111 if (arg ==
ARG0) Jout -= Jin;
115 if (arg ==
ARG0) Jout += Jin;
121 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
123 const Eigen::MatrixBase<Velocity_t> &
v,
124 const Eigen::MatrixBase<ConfigOut_t> & qout)
129 template <
class Config_t,
class Jacobian_t>
131 const Eigen::MatrixBase<Jacobian_t> &
J)
136 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
138 const Eigen::MatrixBase<Tangent_t> & ,
139 const Eigen::MatrixBase<JacobianOut_t> &
J,
149 Jout.diagonal().array() +=
Scalar(1);
152 Jout.diagonal().array() -=
Scalar(1);
155 assert(
false &&
"Wrong Op requesed value");
160 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
162 const Eigen::MatrixBase<Tangent_t> & ,
163 const Eigen::MatrixBase<JacobianOut_t> &
J,
173 Jout.diagonal().array() +=
Scalar(1);
176 Jout.diagonal().array() -=
Scalar(1);
179 assert(
false &&
"Wrong Op requesed value");
184 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
187 const JacobianIn_t & Jin,
188 JacobianOut_t & Jout,
204 assert(
false &&
"Wrong Op requesed value");
209 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
211 const Eigen::MatrixBase<Tangent_t> & ,
212 const Eigen::MatrixBase<JacobianIn_t> & Jin,
213 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const 218 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
220 const Eigen::MatrixBase<Tangent_t> & ,
221 const Eigen::MatrixBase<JacobianIn_t> & Jin,
222 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const 227 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
229 const Eigen::MatrixBase<Tangent_t> & ,
230 const Eigen::MatrixBase<Jacobian_t> & )
const {}
232 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
234 const Eigen::MatrixBase<Tangent_t> & ,
235 const Eigen::MatrixBase<Jacobian_t> & )
const {}
243 template <
class Config_t>
247 template <
class Config_t>
253 template <
class Config_t>
259 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
260 void randomConfiguration_impl
261 (
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
262 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
263 const Eigen::MatrixBase<ConfigOut_t> & qout)
const 266 for (
int i = 0;
i <
nq (); ++
i)
268 if(lower_pos_limit[
i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() ||
269 upper_pos_limit[
i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
271 std::ostringstream error;
272 error <<
"non bounded limit. Cannot uniformly sample joint at rank " <<
i;
274 throw std::range_error(error.str());
276 res[
i] = lower_pos_limit[
i] + (( upper_pos_limit[
i] - lower_pos_limit[
i]) *
rand())/RAND_MAX;
282 return size_.value() == other.
size_.value();
287 Eigen::internal::variable_if_dynamic<Index, Dim>
size_;
292 #endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< Jacobian_t > &) const
void dIntegrate_product_impl(const Config_t &, const Tangent_t &, const JacobianIn_t &Jin, JacobianOut_t &Jout, bool, const ArgumentPosition, const AssignmentOperatorType op) const
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)
VectorSpaceOperationTpl(int size=boost::static_signed_max< 0, Dim >::value)
void random_impl(const Eigen::MatrixBase< Config_t > &qout) const
traits< LieGroupDerived >::Scalar Scalar
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
static void normalize_impl(const Eigen::MatrixBase< Config_t > &)
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Jacobian_t > &J)
static void integrate_impl(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Velocity_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
static void dIntegrate_dq_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
VectorSpaceOperationTpl(const VectorSpaceOperationTpl &other)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
static void dIntegrate_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO)
static bool isNormalized_impl(const Eigen::MatrixBase< Config_t > &, const Scalar &)
Main pinocchio namespace.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Common traits structure to fully define base classes for CRTP.
bool isEqual_impl(const VectorSpaceOperationTpl &other) const
Eigen::internal::variable_if_dynamic< Index, Dim > size_
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase< Config_t > &, const Eigen::MatrixBase< Tangent_t > &, const Eigen::MatrixBase< Jacobian_t > &) const
VectorSpaceOperationTpl & operator=(const VectorSpaceOperationTpl &other)
ConfigVector_t neutral() const
void dDifference_product_impl(const ConfigL_t &, const ConfigR_t &, const JacobianIn_t &Jin, JacobianOut_t &Jout, bool, const AssignmentOperatorType op) const
void dDifference_impl(const Eigen::MatrixBase< ConfigL_t > &, const Eigen::MatrixBase< ConfigR_t > &, const Eigen::MatrixBase< JacobianOut_t > &J) const
static void difference_impl(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &d)