5 #ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__ 6 #define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__ 8 #include "pinocchio/multibody/liegroup/fwd.hpp" 14 #if __cplusplus >= 201103L 15 constexpr
int SELF = 0;
20 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME) \ 21 typedef LieGroupBase<Derived> Base; \ 22 typedef TYPENAME Base::Index Index; \ 23 typedef TYPENAME traits<Derived>::Scalar Scalar; \ 25 Options = traits<Derived>::Options, \ 29 typedef TYPENAME Base::ConfigVector_t ConfigVector_t; \ 30 typedef TYPENAME Base::TangentVector_t TangentVector_t; \ 31 typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t 33 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived) \ 34 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG) 36 #define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived) \ 37 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename) 39 template<
typename Derived>
42 typedef Derived LieGroupDerived;
52 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
53 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
54 typedef Eigen::Matrix<Scalar,NV,NV,Options> JacobianMatrix_t;
67 template <
class ConfigIn_t,
class Tangent_t,
class ConfigOut_t>
68 void integrate(
const Eigen::MatrixBase<ConfigIn_t> & q,
69 const Eigen::MatrixBase<Tangent_t> & v,
70 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
84 template<
class Config_t,
class Jacobian_t>
86 const Eigen::MatrixBase<Jacobian_t> & J)
const;
101 template <ArgumentPosition arg,
class Config_t,
class Tangent_t,
class JacobianOut_t>
103 const Eigen::MatrixBase<Tangent_t> & v,
104 const Eigen::MatrixBase<JacobianOut_t> & J,
105 AssignmentOperatorType op = SETTO)
const 107 PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
108 return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg,op);
124 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
125 void dIntegrate(
const Eigen::MatrixBase<Config_t > & q,
126 const Eigen::MatrixBase<Tangent_t> & v,
127 const Eigen::MatrixBase<JacobianOut_t> & J,
129 const AssignmentOperatorType op = SETTO)
const;
143 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
145 const Eigen::MatrixBase<Tangent_t> & v,
146 const Eigen::MatrixBase<JacobianOut_t> & J,
147 const AssignmentOperatorType op = SETTO)
const;
149 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
151 const Eigen::MatrixBase<Tangent_t> & v,
152 const Eigen::MatrixBase<JacobianIn_t> & Jin,
154 const Eigen::MatrixBase<JacobianOut_t> & Jout,
155 const AssignmentOperatorType op = SETTO)
const;
157 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
159 const Eigen::MatrixBase<Tangent_t> & v,
161 const Eigen::MatrixBase<JacobianIn_t> & Jin,
162 const Eigen::MatrixBase<JacobianOut_t> & Jout,
163 const AssignmentOperatorType op = SETTO)
const;
177 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
179 const Eigen::MatrixBase<Tangent_t> & v,
180 const Eigen::MatrixBase<JacobianOut_t> & J,
181 const AssignmentOperatorType op = SETTO)
const;
183 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
185 const Eigen::MatrixBase<Tangent_t> & v,
187 const Eigen::MatrixBase<JacobianIn_t> & Jin,
188 const Eigen::MatrixBase<JacobianOut_t> & Jout,
189 const AssignmentOperatorType op = SETTO)
const;
191 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
193 const Eigen::MatrixBase<Tangent_t> & v,
194 const Eigen::MatrixBase<JacobianIn_t> & Jin,
196 const Eigen::MatrixBase<JacobianOut_t> & Jout,
197 const AssignmentOperatorType op = SETTO)
const;
219 template<
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
221 const Eigen::MatrixBase<Tangent_t> & v,
222 const Eigen::MatrixBase<JacobianIn_t> & Jin,
223 const Eigen::MatrixBase<JacobianOut_t> & Jout,
244 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
246 const Eigen::MatrixBase<Tangent_t> & v,
247 const Eigen::MatrixBase<JacobianIn_t> & Jin,
248 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const;
267 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
269 const Eigen::MatrixBase<Tangent_t> & v,
270 const Eigen::MatrixBase<JacobianIn_t> & Jin,
271 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const;
291 template<
class Config_t,
class Tangent_t,
class Jacobian_t>
293 const Eigen::MatrixBase<Tangent_t> & v,
294 const Eigen::MatrixBase<Jacobian_t> & J,
314 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
316 const Eigen::MatrixBase<Tangent_t> & v,
317 const Eigen::MatrixBase<Jacobian_t> & J)
const;
335 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
337 const Eigen::MatrixBase<Tangent_t> & v,
338 const Eigen::MatrixBase<Jacobian_t> & J)
const;
349 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
350 void interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
351 const Eigen::MatrixBase<ConfigR_t> & q1,
353 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
361 template <
class Config_t>
362 void normalize(
const Eigen::MatrixBase<Config_t> & qout)
const;
372 template <
class Config_t>
373 void random(
const Eigen::MatrixBase<Config_t> & qout)
const;
384 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
386 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
387 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
399 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
400 void difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
401 const Eigen::MatrixBase<ConfigR_t> & q1,
402 const Eigen::MatrixBase<Tangent_t> & v)
const;
424 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
425 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
426 const Eigen::MatrixBase<ConfigR_t> & q1,
427 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
440 template<
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
441 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
442 const Eigen::MatrixBase<ConfigR_t> & q1,
443 const Eigen::MatrixBase<JacobianOut_t> & J,
446 template<ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
447 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
448 const Eigen::MatrixBase<ConfigR_t> & q1,
449 const Eigen::MatrixBase<JacobianIn_t> & Jin,
451 const Eigen::MatrixBase<JacobianOut_t> & Jout,
452 const AssignmentOperatorType op = SETTO)
const;
454 template<ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
455 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
456 const Eigen::MatrixBase<ConfigR_t> & q1,
458 const Eigen::MatrixBase<JacobianIn_t> & Jin,
459 const Eigen::MatrixBase<JacobianOut_t> & Jout,
460 const AssignmentOperatorType op = SETTO)
const;
470 template <
class ConfigL_t,
class ConfigR_t>
472 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
482 template <
class ConfigL_t,
class ConfigR_t>
483 Scalar
distance(
const Eigen::MatrixBase<ConfigL_t> & q0,
484 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
494 template <
class ConfigL_t,
class ConfigR_t>
496 const Eigen::MatrixBase<ConfigR_t> & q1,
497 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const;
501 return derived().isEqual_impl(other.derived());
506 return derived().isDifferent_impl(other.derived());
513 template <
class Config_t,
class Tangent_t>
514 ConfigVector_t
integrate(
const Eigen::MatrixBase<Config_t> & q,
515 const Eigen::MatrixBase<Tangent_t> & v)
const ;
517 template <
class ConfigL_t,
class ConfigR_t>
518 ConfigVector_t
interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
519 const Eigen::MatrixBase<ConfigR_t> & q1,
520 const Scalar& u)
const;
522 ConfigVector_t random()
const;
524 template <
class ConfigL_t,
class ConfigR_t>
526 (
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
527 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
const;
529 template <
class ConfigL_t,
class ConfigR_t>
530 TangentVector_t
difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
531 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
538 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
539 void dIntegrate_product_impl(
const Config_t & q,
541 const JacobianIn_t & Jin,
542 JacobianOut_t & Jout,
543 bool dIntegrateOnTheLeft,
545 const AssignmentOperatorType op)
const;
547 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
548 void dDifference_product_impl(
const ConfigL_t & q0,
549 const ConfigR_t & q1,
550 const JacobianIn_t & Jin,
551 JacobianOut_t & Jout,
552 bool dDifferenceOnTheLeft,
553 const AssignmentOperatorType op)
const;
555 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
556 void interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
557 const Eigen::MatrixBase<ConfigR_t> & q1,
559 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
561 template <
class Config_t>
562 void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
const;
564 template <
class ConfigL_t,
class ConfigR_t>
565 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
566 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
568 template <
class ConfigL_t,
class ConfigR_t>
569 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
570 const Eigen::MatrixBase<ConfigR_t> & q1,
571 const Scalar & prec)
const;
578 return !derived().isEqual_impl(other.derived());
589 ConfigVector_t
neutral ()
const;
592 std::string
name ()
const;
596 return static_cast <Derived&> (*this);
599 const Derived& derived ()
const 601 return static_cast <
const Derived&> (*this);
619 #include "pinocchio/multibody/liegroup/liegroup-base.hxx" 621 #endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
bool isSameConfiguration(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Check if two configurations are equivalent within the given precision.
void normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
void integrateCoeffWiseJacobian(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J) const
Computes the Jacobian of the integrate operator around zero.
void dIntegrateTransport_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
ConfigVector_t neutral() const
Get neutral element as a vector.
void integrate(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Integrate a joint's configuration with a tangent vector during one unit time duration.
std::string name() const
Get name of instance.
void difference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &v) const
Computes the tangent vector that must be integrated during one unit time to go from q0 to q1...
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
void dIntegrateTransport(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout, const ArgumentPosition arg) const
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
void dIntegrate_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the tangent vector into tangent space at identity...
void dIntegrate_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector into tangent space at identity...
bool isEqual_impl(const LieGroupBase &) const
Default equality check. By default, two LieGroupBase of same type are considered equal.
void dIntegrate(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tan...
LieGroupBase(const LieGroupBase &)
Main pinocchio namespace.
Index nv() const
Get dimension of Lie Group tangent space.
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Common traits structure to fully define base classes for CRTP.
void randomConfiguration(const Eigen::MatrixBase< ConfigL_t > &lower_pos_limit, const Eigen::MatrixBase< ConfigR_t > &upper_pos_limit, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Generate a configuration vector uniformly sampled among provided limits.
void interpolate(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &u, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Interpolation between two joint's configurations.
void dIntegrateTransport_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
void dDifference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of the difference operation with respect to q0 or q1.