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>
20 typedef _Scalar Scalar;
28 template<
int Dim,
typename _Scalar,
int _Options>
30 :
public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
40 assert(size_.value() >= 0);
46 : Base(), size_(other.size_.value())
48 assert(size_.value() >= 0);
62 return ConfigVector_t::Zero(size_.value());
65 std::string
name ()
const 67 std::ostringstream oss; oss <<
"R^" <<
nq();
71 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
72 static void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
73 const Eigen::MatrixBase<ConfigR_t> & q1,
74 const Eigen::MatrixBase<Tangent_t> & d)
76 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) = q1 - q0;
79 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
80 void dDifference_impl (
const Eigen::MatrixBase<ConfigL_t> &,
81 const Eigen::MatrixBase<ConfigR_t> &,
82 const Eigen::MatrixBase<JacobianOut_t> & J)
const 85 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity(size_.value(),size_.value());
87 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
90 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
91 void dDifference_product_impl(
const ConfigL_t &,
93 const JacobianIn_t & Jin,
96 const AssignmentOperatorType op)
const 100 if (arg == ARG0) Jout = - Jin;
104 if (arg == ARG0) Jout -= Jin;
108 if (arg == ARG0) Jout += Jin;
114 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
115 static void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
116 const Eigen::MatrixBase<Velocity_t> & v,
117 const Eigen::MatrixBase<ConfigOut_t> & qout)
119 PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q + v;
122 template <
class Config_t,
class Jacobian_t>
123 static void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> &,
124 const Eigen::MatrixBase<Jacobian_t> & J)
126 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).setIdentity();
129 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
130 static void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
131 const Eigen::MatrixBase<Tangent_t> & ,
132 const Eigen::MatrixBase<JacobianOut_t> & J,
133 const AssignmentOperatorType op=SETTO)
135 Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
142 Jout.diagonal().array() += Scalar(1);
145 Jout.diagonal().array() -= Scalar(1);
148 assert(
false &&
"Wrong Op requesed value");
153 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
154 static void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
155 const Eigen::MatrixBase<Tangent_t> & ,
156 const Eigen::MatrixBase<JacobianOut_t> & J,
157 const AssignmentOperatorType op=SETTO)
159 Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
166 Jout.diagonal().array() += Scalar(1);
169 Jout.diagonal().array() -= Scalar(1);
172 assert(
false &&
"Wrong Op requesed value");
177 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
178 void dIntegrate_product_impl(
const Config_t &,
180 const JacobianIn_t & Jin,
181 JacobianOut_t & Jout,
184 const AssignmentOperatorType op)
const 197 assert(
false &&
"Wrong Op requesed value");
202 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
203 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
204 const Eigen::MatrixBase<Tangent_t> & ,
205 const Eigen::MatrixBase<JacobianIn_t> & Jin,
206 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const 208 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
211 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
212 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
213 const Eigen::MatrixBase<Tangent_t> & ,
214 const Eigen::MatrixBase<JacobianIn_t> & Jin,
215 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const 217 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
220 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
221 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & ,
222 const Eigen::MatrixBase<Tangent_t> & ,
223 const Eigen::MatrixBase<Jacobian_t> & )
const {}
225 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
226 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & ,
227 const Eigen::MatrixBase<Tangent_t> & ,
228 const Eigen::MatrixBase<Jacobian_t> & )
const {}
236 template <
class Config_t>
237 static void normalize_impl (
const Eigen::MatrixBase<Config_t>& )
240 template <
class Config_t>
241 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 243 PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom();
246 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
247 void randomConfiguration_impl
248 (
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
249 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
250 const Eigen::MatrixBase<ConfigOut_t> & qout)
const 252 ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived();
253 for (
int i = 0; i <
nq (); ++i)
255 if(lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() ||
256 upper_pos_limit[i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
258 std::ostringstream error;
259 error <<
"non bounded limit. Cannot uniformly sample joint at rank " << i;
261 throw std::range_error(error.str());
263 res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
269 return size_.value() == other.size_.value();
274 Eigen::internal::variable_if_dynamic<Index, Dim> size_;
279 #endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__ int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
VectorSpaceOperationTpl(int size=boost::static_signed_max< 0, Dim >::value)
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
VectorSpaceOperationTpl(const VectorSpaceOperationTpl &other)
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Main pinocchio namespace.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Common traits structure to fully define base classes for CRTP.