pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
liegroup-base.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
6 #define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
7 
8 #include "pinocchio/multibody/liegroup/fwd.hpp"
9 
10 #include <limits>
11 
12 namespace pinocchio
13 {
14 #ifdef PINOCCHIO_WITH_CXX11_SUPPORT
15  constexpr int SELF = 0;
16 #else
17  enum { SELF = 0 };
18 #endif
19 
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; \
24  enum { \
25  Options = traits<Derived>::Options, \
26  NQ = Base::NQ, \
27  NV = Base::NV \
28  }; \
29  typedef TYPENAME Base::ConfigVector_t ConfigVector_t; \
30  typedef TYPENAME Base::TangentVector_t TangentVector_t; \
31  typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t
32 
33 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived) \
34 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG)
35 
36 #define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived) \
37 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
38 
39  template<typename Derived>
40  struct LieGroupBase
41  {
42  typedef Derived LieGroupDerived;
43  typedef int Index;
44  typedef typename traits<LieGroupDerived>::Scalar Scalar;
45  enum
46  {
47  Options = traits<LieGroupDerived>::Options,
48  NQ = traits<LieGroupDerived>::NQ,
49  NV = traits<LieGroupDerived>::NV
50  };
51 
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;
55 
58 
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;
71 
84  template<class Config_t, class Jacobian_t>
85  void integrateCoeffWiseJacobian(const Eigen::MatrixBase<Config_t > & q,
86  const Eigen::MatrixBase<Jacobian_t> & J) const;
87 
101  template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
102  void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
103  const Eigen::MatrixBase<Tangent_t> & v,
104  const Eigen::MatrixBase<JacobianOut_t> & J,
105  AssignmentOperatorType op = SETTO) const
106  {
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);
109  }
110 
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,
128  const ArgumentPosition arg,
129  const AssignmentOperatorType op = SETTO) const;
130 
143  template <class Config_t, class Tangent_t, class JacobianOut_t>
144  void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
145  const Eigen::MatrixBase<Tangent_t> & v,
146  const Eigen::MatrixBase<JacobianOut_t> & J,
147  const AssignmentOperatorType op = SETTO) const;
148 
149  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
150  void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
151  const Eigen::MatrixBase<Tangent_t> & v,
152  const Eigen::MatrixBase<JacobianIn_t> & Jin,
153  int self,
154  const Eigen::MatrixBase<JacobianOut_t> & Jout,
155  const AssignmentOperatorType op = SETTO) const;
156 
157  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
158  void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
159  const Eigen::MatrixBase<Tangent_t> & v,
160  int self,
161  const Eigen::MatrixBase<JacobianIn_t> & Jin,
162  const Eigen::MatrixBase<JacobianOut_t> & Jout,
163  const AssignmentOperatorType op = SETTO) const;
164 
177  template <class Config_t, class Tangent_t, class JacobianOut_t>
178  void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
179  const Eigen::MatrixBase<Tangent_t> & v,
180  const Eigen::MatrixBase<JacobianOut_t> & J,
181  const AssignmentOperatorType op = SETTO) const;
182 
183  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
184  void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
185  const Eigen::MatrixBase<Tangent_t> & v,
186  int self,
187  const Eigen::MatrixBase<JacobianIn_t> & Jin,
188  const Eigen::MatrixBase<JacobianOut_t> & Jout,
189  const AssignmentOperatorType op = SETTO) const;
190 
191  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
192  void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
193  const Eigen::MatrixBase<Tangent_t> & v,
194  const Eigen::MatrixBase<JacobianIn_t> & Jin,
195  int self,
196  const Eigen::MatrixBase<JacobianOut_t> & Jout,
197  const AssignmentOperatorType op = SETTO) const;
198 
219  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
220  void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
221  const Eigen::MatrixBase<Tangent_t> & v,
222  const Eigen::MatrixBase<JacobianIn_t> & Jin,
223  const Eigen::MatrixBase<JacobianOut_t> & Jout,
224  const ArgumentPosition arg) const;
225 
244  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
245  void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
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>
268  void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
269  const Eigen::MatrixBase<Tangent_t> & v,
270  const Eigen::MatrixBase<JacobianIn_t> & Jin,
271  const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
272 
273 
291  template<class Config_t, class Tangent_t, class Jacobian_t>
292  void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
293  const Eigen::MatrixBase<Tangent_t> & v,
294  const Eigen::MatrixBase<Jacobian_t> & J,
295  const ArgumentPosition arg) const;
296 
314  template <class Config_t, class Tangent_t, class Jacobian_t>
315  void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
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>
336  void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
337  const Eigen::MatrixBase<Tangent_t> & v,
338  const Eigen::MatrixBase<Jacobian_t> & J) const;
339 
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,
352  const Scalar& u,
353  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
354 
364  template <class Config_t>
365  void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
366 
375  template <class Config_t>
376  bool isNormalized(const Eigen::MatrixBase<Config_t> & qin,
377  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
378 
387  template <class Config_t>
388  void random(const Eigen::MatrixBase<Config_t> & qout) const;
389 
399  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
400  void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
401  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
402  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
403 
417  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
418  void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
419  const Eigen::MatrixBase<ConfigR_t> & q1,
420  const Eigen::MatrixBase<Tangent_t> & v) const;
421 
442  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
443  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
444  const Eigen::MatrixBase<ConfigR_t> & q1,
445  const Eigen::MatrixBase<JacobianOut_t> & J) const;
446 
458  template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
459  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
460  const Eigen::MatrixBase<ConfigR_t> & q1,
461  const Eigen::MatrixBase<JacobianOut_t> & J,
462  const ArgumentPosition arg) const;
463 
464  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
465  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
466  const Eigen::MatrixBase<ConfigR_t> & q1,
467  const Eigen::MatrixBase<JacobianIn_t> & Jin,
468  int self,
469  const Eigen::MatrixBase<JacobianOut_t> & Jout,
470  const AssignmentOperatorType op = SETTO) const;
471 
472  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
473  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
474  const Eigen::MatrixBase<ConfigR_t> & q1,
475  int self,
476  const Eigen::MatrixBase<JacobianIn_t> & Jin,
477  const Eigen::MatrixBase<JacobianOut_t> & Jout,
478  const AssignmentOperatorType op = SETTO) const;
479 
488  template <class ConfigL_t, class ConfigR_t>
489  Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
490  const Eigen::MatrixBase<ConfigR_t> & q1) const;
491 
500  template <class ConfigL_t, class ConfigR_t>
501  Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
502  const Eigen::MatrixBase<ConfigR_t> & q1) const;
503 
512  template <class ConfigL_t, class ConfigR_t>
513  bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
514  const Eigen::MatrixBase<ConfigR_t> & q1,
515  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
516 
517  bool operator== (const LieGroupBase& other) const
518  {
519  return derived().isEqual_impl(other.derived());
520  }
521 
522  bool operator!= (const LieGroupBase& other) const
523  {
524  return derived().isDifferent_impl(other.derived());
525  }
527 
530 
531  template <class Config_t, class Tangent_t>
532  ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
533  const Eigen::MatrixBase<Tangent_t> & v) const ;
534 
535  template <class ConfigL_t, class ConfigR_t>
536  ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
537  const Eigen::MatrixBase<ConfigR_t> & q1,
538  const Scalar& u) const;
539 
540  ConfigVector_t random() const;
541 
542  template <class ConfigL_t, class ConfigR_t>
543  ConfigVector_t randomConfiguration
544  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
545  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
546 
547  template <class ConfigL_t, class ConfigR_t>
548  TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
549  const Eigen::MatrixBase<ConfigR_t> & q1) const;
551 
552 
555 
556  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
557  void dIntegrate_product_impl(const Config_t & q,
558  const Tangent_t & v,
559  const JacobianIn_t & Jin,
560  JacobianOut_t & Jout,
561  bool dIntegrateOnTheLeft,
562  const ArgumentPosition arg,
563  const AssignmentOperatorType op) const;
564 
565  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
566  void dDifference_product_impl(const ConfigL_t & q0,
567  const ConfigR_t & q1,
568  const JacobianIn_t & Jin,
569  JacobianOut_t & Jout,
570  bool dDifferenceOnTheLeft,
571  const AssignmentOperatorType op) const;
572 
573  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
574  void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
575  const Eigen::MatrixBase<ConfigR_t> & q1,
576  const Scalar& u,
577  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
578 
579  template <class Config_t>
580  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
581 
582  template <class Config_t>
583  bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
584  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
585 
586  template <class ConfigL_t, class ConfigR_t>
587  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
588  const Eigen::MatrixBase<ConfigR_t> & q1) const;
589 
590  template <class ConfigL_t, class ConfigR_t>
591  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
592  const Eigen::MatrixBase<ConfigR_t> & q1,
593  const Scalar & prec) const;
594 
597  bool isEqual_impl (const LieGroupBase& /*other*/) const { return true; }
598  bool isDifferent_impl (const LieGroupBase& other) const
599  {
600  return !derived().isEqual_impl(other.derived());
601  }
602 
607  Index nq () const;
609  Index nv () const;
611  ConfigVector_t neutral () const;
612 
614  std::string name () const;
615 
616  Derived& derived ()
617  {
618  return static_cast <Derived&> (*this);
619  }
620 
621  const Derived& derived () const
622  {
623  return static_cast <const Derived&> (*this);
624  }
626 
627  protected:
631  LieGroupBase() {}
632 
636  LieGroupBase( const LieGroupBase & /*clone*/) {}
637  LieGroupBase& operator=(const LieGroupBase & /*x*/) { return *this; }
638 
639  // C++11
640  // LieGroupBase(const LieGroupBase &) = delete;
641  // LieGroupBase& operator=(const LieGroupBase & /*x*/) = delete;
642  }; // struct LieGroupBase
643 
644 } // namespace pinocchio
645 
646 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
647 
648 #endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
pinocchio::LieGroupBase::difference
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.
pinocchio::LieGroupBase::neutral
ConfigVector_t neutral() const
Get neutral element as a vector.
pinocchio::LieGroupBase::distance
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
pinocchio::LieGroupBase::dIntegrate_dq
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...
pinocchio::LieGroupBase::dIntegrate
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...
Definition: liegroup-base.hpp:102
pinocchio::LieGroupBase::interpolate
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.
pinocchio::LieGroupBase::dIntegrateTransport_dq
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,...
pinocchio::LieGroupBase::name
std::string name() const
Get name of instance.
pinocchio::LieGroupBase::integrateCoeffWiseJacobian
void integrateCoeffWiseJacobian(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J) const
Computes the Jacobian of the integrate operator around zero.
pinocchio::LieGroupBase::normalize
void normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary.
pinocchio::LieGroupBase::randomConfiguration
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.
pinocchio::LieGroupBase::LieGroupBase
LieGroupBase(const LieGroupBase &)
Definition: liegroup-base.hpp:636
pinocchio::LieGroupBase::nv
Index nv() const
Get dimension of Lie Group tangent space.
pinocchio::LieGroupBase::LieGroupBase
LieGroupBase()
Definition: liegroup-base.hpp:631
pinocchio::LieGroupBase::isNormalized
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...
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
pinocchio::LieGroupBase
Definition: liegroup-base.hpp:40
pinocchio::LieGroupBase::squaredDistance
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
pinocchio::LieGroupBase::dDifference
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.
pinocchio::LieGroupBase::isEqual_impl
bool isEqual_impl(const LieGroupBase &) const
Default equality check. By default, two LieGroupBase of same type are considered equal.
Definition: liegroup-base.hpp:597
pinocchio::LieGroupBase::nq
Index nq() const
pinocchio::LieGroupBase::isSameConfiguration
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.
pinocchio::LieGroupBase::dIntegrateTransport
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,...
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::LieGroupBase::random
void random(const Eigen::MatrixBase< Config_t > &qout) const
Generate a random joint configuration, normalizing quaternions when necessary.
pinocchio::LieGroupBase::integrate
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.
pinocchio::LieGroupBase::dIntegrate_dv
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.
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::LieGroupBase::dIntegrateTransport_dv
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,...