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__