pinocchio  2.5.0
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 #if __cplusplus >= 201103L
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  {
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 
361  template <class Config_t>
362  void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
363 
372  template <class Config_t>
373  void random(const Eigen::MatrixBase<Config_t> & qout) const;
374 
384  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
385  void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
386  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
387  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
388 
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;
403 
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;
428 
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,
444  const ArgumentPosition arg) const;
445 
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,
450  int self,
451  const Eigen::MatrixBase<JacobianOut_t> & Jout,
452  const AssignmentOperatorType op = SETTO) const;
453 
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,
457  int self,
458  const Eigen::MatrixBase<JacobianIn_t> & Jin,
459  const Eigen::MatrixBase<JacobianOut_t> & Jout,
460  const AssignmentOperatorType op = SETTO) const;
461 
470  template <class ConfigL_t, class ConfigR_t>
471  Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
472  const Eigen::MatrixBase<ConfigR_t> & q1) const;
473 
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;
485 
494  template <class ConfigL_t, class ConfigR_t>
495  bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
496  const Eigen::MatrixBase<ConfigR_t> & q1,
497  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
498 
499  bool operator== (const LieGroupBase& other) const
500  {
501  return derived().isEqual_impl(other.derived());
502  }
503 
504  bool operator!= (const LieGroupBase& other) const
505  {
506  return derived().isDifferent_impl(other.derived());
507  }
509 
512 
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 ;
516 
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;
521 
522  ConfigVector_t random() const;
523 
524  template <class ConfigL_t, class ConfigR_t>
525  ConfigVector_t randomConfiguration
526  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
527  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
528 
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;
533 
534 
537 
538  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
539  void dIntegrate_product_impl(const Config_t & q,
540  const Tangent_t & v,
541  const JacobianIn_t & Jin,
542  JacobianOut_t & Jout,
543  bool dIntegrateOnTheLeft,
544  const ArgumentPosition arg,
545  const AssignmentOperatorType op) const;
546 
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;
554 
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,
558  const Scalar& u,
559  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
560 
561  template <class Config_t>
562  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
563 
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;
567 
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;
572 
575  bool isEqual_impl (const LieGroupBase& /*other*/) const { return true; }
576  bool isDifferent_impl (const LieGroupBase& other) const
577  {
578  return !derived().isEqual_impl(other.derived());
579  }
580 
585  Index nq () const;
587  Index nv () const;
589  ConfigVector_t neutral () const;
590 
592  std::string name () const;
593 
594  Derived& derived ()
595  {
596  return static_cast <Derived&> (*this);
597  }
598 
599  const Derived& derived () const
600  {
601  return static_cast <const Derived&> (*this);
602  }
604 
605  protected:
610 
614  LieGroupBase( const LieGroupBase & /*clone*/) {}
615  }; // struct LieGroupBase
616 
617 } // namespace pinocchio
618 
619 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
620 
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&#39;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.
Definition: fwd.hpp:59
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.
Definition: treeview.dox:24
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.
Definition: fwd.hpp:44
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&#39;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.