pinocchio  2.4.4
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  {
42  Options = traits<LieGroupDerived>::Options,
43  NQ = traits<LieGroupDerived>::NQ,
44  NV = traits<LieGroupDerived>::NV
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:
525  LieGroupBase() {}
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__
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:97
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_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...
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::name
std::string name() const
Get name of instance.
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&#39;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::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::nv
Index nv() const
Get dimension of Lie Group tangent space.
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::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::squaredDistance
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:50
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::LieGroupBase::LieGroupBase
LieGroupBase(const LieGroupBase &)
Definition: liegroup-base.hpp:530
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
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::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&#39;s configuration with a tangent vector during one unit time duration.
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
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::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::neutral
ConfigVector_t neutral() const
Get neutral element as a vector.