pinocchio  2.4.5
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 
15 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME) \
16  typedef LieGroupBase<Derived> Base; \
17  typedef TYPENAME Base::Index Index; \
18  typedef TYPENAME traits<Derived>::Scalar Scalar; \
19  enum { \
20  Options = traits<Derived>::Options, \
21  NQ = Base::NQ, \
22  NV = Base::NV \
23  }; \
24  typedef TYPENAME Base::ConfigVector_t ConfigVector_t; \
25  typedef TYPENAME Base::TangentVector_t TangentVector_t; \
26  typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t
27 
28 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived) \
29 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG)
30 
31 #define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived) \
32 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
33 
34  template<typename Derived>
35  struct LieGroupBase
36  {
37  typedef Derived LieGroupDerived;
38  typedef int Index;
39  typedef typename traits<LieGroupDerived>::Scalar Scalar;
40  enum
41  {
45  };
46 
47  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
48  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
49  typedef Eigen::Matrix<Scalar,NV,NV,Options> JacobianMatrix_t;
50 
53 
62  template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
63  void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
64  const Eigen::MatrixBase<Tangent_t> & v,
65  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
66 
79  template<class Config_t, class Jacobian_t>
80  void integrateCoeffWiseJacobian(const Eigen::MatrixBase<Config_t > & q,
81  const Eigen::MatrixBase<Jacobian_t> & J) const;
82 
96  template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
97  void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
98  const Eigen::MatrixBase<Tangent_t> & v,
99  const Eigen::MatrixBase<JacobianOut_t> & J,
100  AssignmentOperatorType op = SETTO) const
101  {
102  PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
103  return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg,op);
104  }
105 
119  template<class Config_t, class Tangent_t, class JacobianOut_t>
120  void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
121  const Eigen::MatrixBase<Tangent_t> & v,
122  const Eigen::MatrixBase<JacobianOut_t> & J,
123  const ArgumentPosition arg,
124  const AssignmentOperatorType op = SETTO) const;
125 
138  template <class Config_t, class Tangent_t, class JacobianOut_t>
139  void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
140  const Eigen::MatrixBase<Tangent_t> & v,
141  const Eigen::MatrixBase<JacobianOut_t> & J,
142  const AssignmentOperatorType op = SETTO) const;
143 
156  template <class Config_t, class Tangent_t, class JacobianOut_t>
157  void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
158  const Eigen::MatrixBase<Tangent_t> & v,
159  const Eigen::MatrixBase<JacobianOut_t> & J,
160  const AssignmentOperatorType op = SETTO) const;
161 
182  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
183  void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
184  const Eigen::MatrixBase<Tangent_t> & v,
185  const Eigen::MatrixBase<JacobianIn_t> & Jin,
186  const Eigen::MatrixBase<JacobianOut_t> & Jout,
187  const ArgumentPosition arg) const;
188 
207  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
208  void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
209  const Eigen::MatrixBase<Tangent_t> & v,
210  const Eigen::MatrixBase<JacobianIn_t> & Jin,
211  const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
230  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
231  void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
232  const Eigen::MatrixBase<Tangent_t> & v,
233  const Eigen::MatrixBase<JacobianIn_t> & Jin,
234  const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
235 
236 
254  template<class Config_t, class Tangent_t, class Jacobian_t>
255  void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
256  const Eigen::MatrixBase<Tangent_t> & v,
257  const Eigen::MatrixBase<Jacobian_t> & J,
258  const ArgumentPosition arg) const;
259 
278  template <class Config_t, class Tangent_t, class Jacobian_t>
279  void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
280  const Eigen::MatrixBase<Tangent_t> & v,
281  const Eigen::MatrixBase<Jacobian_t> & J) const;
300  template <class Config_t, class Tangent_t, class Jacobian_t>
301  void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
302  const Eigen::MatrixBase<Tangent_t> & v,
303  const Eigen::MatrixBase<Jacobian_t> & J) const;
304 
314  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
315  void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
316  const Eigen::MatrixBase<ConfigR_t> & q1,
317  const Scalar& u,
318  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
319 
326  template <class Config_t>
327  void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
328 
337  template <class Config_t>
338  void random(const Eigen::MatrixBase<Config_t> & qout) const;
339 
349  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
350  void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
351  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
352  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
353 
364  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
365  void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
366  const Eigen::MatrixBase<ConfigR_t> & q1,
367  const Eigen::MatrixBase<Tangent_t> & v) const;
368 
389  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
390  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
391  const Eigen::MatrixBase<ConfigR_t> & q1,
392  const Eigen::MatrixBase<JacobianOut_t> & J) const;
393 // {
394 // PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
395 // return dDifference(q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
396 // }
397 
409  template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
410  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
411  const Eigen::MatrixBase<ConfigR_t> & q1,
412  const Eigen::MatrixBase<JacobianOut_t> & J,
413  const ArgumentPosition arg) const;
422  template <class ConfigL_t, class ConfigR_t>
423  Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
424  const Eigen::MatrixBase<ConfigR_t> & q1) const;
425 
434  template <class ConfigL_t, class ConfigR_t>
435  Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
436  const Eigen::MatrixBase<ConfigR_t> & q1) const;
437 
446  template <class ConfigL_t, class ConfigR_t>
447  bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
448  const Eigen::MatrixBase<ConfigR_t> & q1,
449  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
451 
454 
455  template <class Config_t, class Tangent_t>
456  ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
457  const Eigen::MatrixBase<Tangent_t> & v) const ;
458 
459  template <class ConfigL_t, class ConfigR_t>
460  ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
461  const Eigen::MatrixBase<ConfigR_t> & q1,
462  const Scalar& u) const;
463 
464  ConfigVector_t random() const;
465 
466  template <class ConfigL_t, class ConfigR_t>
467  ConfigVector_t randomConfiguration
468  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
469  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
470 
471  template <class ConfigL_t, class ConfigR_t>
472  TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
473  const Eigen::MatrixBase<ConfigR_t> & q1) const;
475 
476 
479 
480  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
481  void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
482  const Eigen::MatrixBase<ConfigR_t> & q1,
483  const Scalar& u,
484  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
485 
486  template <class Config_t>
487  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
488 
489  template <class ConfigL_t, class ConfigR_t>
490  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
491  const Eigen::MatrixBase<ConfigR_t> & q1) const;
492 
493  template <class ConfigL_t, class ConfigR_t>
494  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
495  const Eigen::MatrixBase<ConfigR_t> & q1,
496  const Scalar & prec) const;
501  Index nq () const;
503  Index nv () const;
505  ConfigVector_t neutral () const;
506 
508  std::string name () const;
509 
510  Derived& derived ()
511  {
512  return static_cast <Derived&> (*this);
513  }
514 
515  const Derived& derived () const
516  {
517  return static_cast <const Derived&> (*this);
518  }
520 
521  protected:
526 
530  LieGroupBase( const LieGroupBase & /*clone*/) {}
531  }; // struct LieGroupBase
532 
533 } // namespace pinocchio
534 
535 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
536 
537 #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:50
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...
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:35
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.