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 #ifdef PINOCCHIO_WITH_CXX11_SUPPORT 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;
364 template <
class Config_t>
365 void normalize(
const Eigen::MatrixBase<Config_t> & qout)
const;
375 template <
class Config_t>
376 bool isNormalized(
const Eigen::MatrixBase<Config_t> & qin,
377 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision())
const;
387 template <
class Config_t>
388 void random(
const Eigen::MatrixBase<Config_t> & qout)
const;
399 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
401 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
402 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
414 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
415 void difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
416 const Eigen::MatrixBase<ConfigR_t> & q1,
417 const Eigen::MatrixBase<Tangent_t> & v)
const;
439 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
440 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
441 const Eigen::MatrixBase<ConfigR_t> & q1,
442 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
455 template<
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
456 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
457 const Eigen::MatrixBase<ConfigR_t> & q1,
458 const Eigen::MatrixBase<JacobianOut_t> & J,
461 template<ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
462 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
463 const Eigen::MatrixBase<ConfigR_t> & q1,
464 const Eigen::MatrixBase<JacobianIn_t> & Jin,
466 const Eigen::MatrixBase<JacobianOut_t> & Jout,
467 const AssignmentOperatorType op = SETTO)
const;
469 template<ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
470 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
471 const Eigen::MatrixBase<ConfigR_t> & q1,
473 const Eigen::MatrixBase<JacobianIn_t> & Jin,
474 const Eigen::MatrixBase<JacobianOut_t> & Jout,
475 const AssignmentOperatorType op = SETTO)
const;
485 template <
class ConfigL_t,
class ConfigR_t>
487 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
497 template <
class ConfigL_t,
class ConfigR_t>
498 Scalar
distance(
const Eigen::MatrixBase<ConfigL_t> & q0,
499 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
509 template <
class ConfigL_t,
class ConfigR_t>
511 const Eigen::MatrixBase<ConfigR_t> & q1,
512 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const;
516 return derived().isEqual_impl(other.derived());
521 return derived().isDifferent_impl(other.derived());
528 template <
class Config_t,
class Tangent_t>
529 ConfigVector_t
integrate(
const Eigen::MatrixBase<Config_t> & q,
530 const Eigen::MatrixBase<Tangent_t> & v)
const ;
532 template <
class ConfigL_t,
class ConfigR_t>
533 ConfigVector_t
interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
534 const Eigen::MatrixBase<ConfigR_t> & q1,
535 const Scalar& u)
const;
537 ConfigVector_t random()
const;
539 template <
class ConfigL_t,
class ConfigR_t>
541 (
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
542 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
const;
544 template <
class ConfigL_t,
class ConfigR_t>
545 TangentVector_t
difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
546 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
553 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
554 void dIntegrate_product_impl(
const Config_t & q,
556 const JacobianIn_t & Jin,
557 JacobianOut_t & Jout,
558 bool dIntegrateOnTheLeft,
560 const AssignmentOperatorType op)
const;
562 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
563 void dDifference_product_impl(
const ConfigL_t & q0,
564 const ConfigR_t & q1,
565 const JacobianIn_t & Jin,
566 JacobianOut_t & Jout,
567 bool dDifferenceOnTheLeft,
568 const AssignmentOperatorType op)
const;
570 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
571 void interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
572 const Eigen::MatrixBase<ConfigR_t> & q1,
574 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
576 template <
class Config_t>
577 void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
const;
579 template <
class Config_t>
580 bool isNormalized_impl(
const Eigen::MatrixBase<Config_t> & qin,
581 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const;
583 template <
class ConfigL_t,
class ConfigR_t>
584 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
585 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
587 template <
class ConfigL_t,
class ConfigR_t>
588 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
589 const Eigen::MatrixBase<ConfigR_t> & q1,
590 const Scalar & prec)
const;
597 return !derived().isEqual_impl(other.derived());
608 ConfigVector_t
neutral ()
const;
611 std::string
name ()
const;
615 return static_cast <Derived&> (*this);
618 const Derived& derived ()
const 620 return static_cast <
const Derived&> (*this);
638 #include "pinocchio/multibody/liegroup/liegroup-base.hxx" 640 #endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
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...
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 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...
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.
std::string name() const
Get name of instance.
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.
Index nv() const
Get dimension of Lie Group tangent space.
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...
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
bool isEqual_impl(const LieGroupBase &) const
Default equality check. By default, two LieGroupBase of same type are considered equal.
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
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...
LieGroupBase(const LieGroupBase &)
Main pinocchio namespace.
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 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.
Common traits structure to fully define base classes for CRTP.
bool isNormalized(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Check whether the input joint configuration is normalized. For instance, the quaternion must be unita...
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 normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
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...
ConfigVector_t neutral() const
Get neutral element as a vector.