pinocchio  2.2.1-dirty
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__
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...
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 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.
std::string name() const
Get name of instance.
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.
Index nv() const
Get dimension of Lie Group tangent space.
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...
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
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...
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: spatial/fwd.hpp:47
LieGroupBase(const LieGroupBase &)
Main pinocchio namespace.
Definition: treeview.dox:24
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.
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
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 normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
ConfigVector_t neutral() const
Get neutral element as a vector.
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...