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__
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:614
pinocchio::LieGroupBase::nv
Index nv() const
Get dimension of Lie Group tangent space.
pinocchio::LieGroupBase::LieGroupBase
LieGroupBase()
Definition: liegroup-base.hpp:609
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:575
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:24
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,...