pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
liegroup-base.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_lie_group_operation_base_hpp__
6 #define __pinocchio_lie_group_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 
95  template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
96  void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
97  const Eigen::MatrixBase<Tangent_t> & v,
98  const Eigen::MatrixBase<JacobianOut_t> & J) const
99  {
100  PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
101  return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
102  }
103 
116  template<class Config_t, class Tangent_t, class JacobianOut_t>
117  void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
118  const Eigen::MatrixBase<Tangent_t> & v,
119  const Eigen::MatrixBase<JacobianOut_t> & J,
120  const ArgumentPosition arg) const;
121 
133  template <class Config_t, class Tangent_t, class JacobianOut_t>
134  void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
135  const Eigen::MatrixBase<Tangent_t> & v,
136  const Eigen::MatrixBase<JacobianOut_t> & J) const;
137 
149  template <class Config_t, class Tangent_t, class JacobianOut_t>
150  void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
151  const Eigen::MatrixBase<Tangent_t> & v,
152  const Eigen::MatrixBase<JacobianOut_t> & J) const;
153 
163  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
164  void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
165  const Eigen::MatrixBase<ConfigR_t> & q1,
166  const Scalar& u,
167  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
168 
175  template <class Config_t>
176  void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
177 
186  template <class Config_t>
187  void random(const Eigen::MatrixBase<Config_t> & qout) const;
188 
198  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
199  void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
200  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
201  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
202 
213  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
214  void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
215  const Eigen::MatrixBase<ConfigR_t> & q1,
216  const Eigen::MatrixBase<Tangent_t> & v) const;
217 
238  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
239  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
240  const Eigen::MatrixBase<ConfigR_t> & q1,
241  const Eigen::MatrixBase<JacobianOut_t> & J) const;
242 // {
243 // PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
244 // return dDifference(q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
245 // }
246 
258  template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
259  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
260  const Eigen::MatrixBase<ConfigR_t> & q1,
261  const Eigen::MatrixBase<JacobianOut_t> & J,
262  const ArgumentPosition arg) const;
271  template <class ConfigL_t, class ConfigR_t>
272  Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
273  const Eigen::MatrixBase<ConfigR_t> & q1) const;
274 
283  template <class ConfigL_t, class ConfigR_t>
284  Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
285  const Eigen::MatrixBase<ConfigR_t> & q1) const;
286 
295  template <class ConfigL_t, class ConfigR_t>
296  bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
297  const Eigen::MatrixBase<ConfigR_t> & q1,
298  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
300 
303 
304  template <class Config_t, class Tangent_t>
305  ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
306  const Eigen::MatrixBase<Tangent_t> & v) const ;
307 
308  template <class ConfigL_t, class ConfigR_t>
309  ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
310  const Eigen::MatrixBase<ConfigR_t> & q1,
311  const Scalar& u) const;
312 
313  ConfigVector_t random() const;
314 
315  template <class ConfigL_t, class ConfigR_t>
316  ConfigVector_t randomConfiguration
317  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
318  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
319 
320  template <class ConfigL_t, class ConfigR_t>
321  TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
322  const Eigen::MatrixBase<ConfigR_t> & q1) const;
324 
325 
328 
329  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
330  void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
331  const Eigen::MatrixBase<ConfigR_t> & q1,
332  const Scalar& u,
333  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
334 
335  template <class Config_t>
336  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
337 
338  template <class ConfigL_t, class ConfigR_t>
339  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
340  const Eigen::MatrixBase<ConfigR_t> & q1) const;
341 
342  template <class ConfigL_t, class ConfigR_t>
343  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
344  const Eigen::MatrixBase<ConfigR_t> & q1,
345  const Scalar & prec) const;
350  Index nq () const;
352  Index nv () const;
354  ConfigVector_t neutral () const;
355 
357  std::string name () const;
358 
359  Derived& derived ()
360  {
361  return static_cast <Derived&> (*this);
362  }
363 
364  const Derived& derived () const
365  {
366  return static_cast <const Derived&> (*this);
367  }
369 
370  protected:
375 
379  LieGroupBase( const LieGroupBase & /*clone*/) {}
380  }; // struct LieGroupBase
381 
382 } // namespace pinocchio
383 
384 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
385 
386 #endif // ifndef __pinocchio_lie_group_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 dIntegrate(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tan...
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 dIntegrate_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of a small variation of the tangent vector into tangent space at identity...
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 dIntegrate_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of a small variation of the configuration vector into tangent space at identity...
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
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 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.