pinocchio  2.1.3
liegroup-base.hpp
1 //
2 // Copyright (c) 2016-2018 CNRS
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  {
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 
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,v,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
200  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
201  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
202  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
203 
212  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
213  void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
214  const Eigen::MatrixBase<ConfigR_t> & q1,
215  const Eigen::MatrixBase<Tangent_t>& v) const;
216 
217  template <class ConfigL_t, class ConfigR_t, class JacobianLOut_t, class JacobianROut_t>
218  void Jdifference(const Eigen::MatrixBase<ConfigL_t> & q0,
219  const Eigen::MatrixBase<ConfigR_t> & q1,
220  const Eigen::MatrixBase<JacobianLOut_t>& J0,
221  const Eigen::MatrixBase<JacobianROut_t>& J1) const
222  PINOCCHIO_DEPRECATED;
223 
233  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
234  void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
235  const Eigen::MatrixBase<ConfigR_t> & q1,
236  const Eigen::MatrixBase<JacobianOut_t>& J) const;
237 
246  template <class ConfigL_t, class ConfigR_t>
247  Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
248  const Eigen::MatrixBase<ConfigR_t> & q1) const;
249 
258  template <class ConfigL_t, class ConfigR_t>
259  Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
260  const Eigen::MatrixBase<ConfigR_t> & q1) const;
261 
268  template <class ConfigL_t, class ConfigR_t>
269  bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
270  const Eigen::MatrixBase<ConfigR_t> & q1,
271  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
273 
276 
277  template <class Config_t, class Tangent_t>
278  ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
279  const Eigen::MatrixBase<Tangent_t> & v) const ;
280 
281  template <class ConfigL_t, class ConfigR_t>
282  ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
283  const Eigen::MatrixBase<ConfigR_t> & q1,
284  const Scalar& u) const;
285 
286  ConfigVector_t random() const;
287 
288  template <class ConfigL_t, class ConfigR_t>
289  ConfigVector_t randomConfiguration
290  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
291  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
292 
293  template <class ConfigL_t, class ConfigR_t>
294  TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
295  const Eigen::MatrixBase<ConfigR_t> & q1) const;
297 
298 
301 
302  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
303  void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
304  const Eigen::MatrixBase<ConfigR_t> & q1,
305  const Scalar& u,
306  const Eigen::MatrixBase<ConfigOut_t>& qout) const;
307 
308  template <class Config_t>
309  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
310 
311  template <class ConfigL_t, class ConfigR_t>
312  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
313  const Eigen::MatrixBase<ConfigR_t> & q1) const;
314 
315  template <class ConfigL_t, class ConfigR_t>
316  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
317  const Eigen::MatrixBase<ConfigR_t> & q1,
318  const Scalar & prec) const;
323  Index nq () const;
325  Index nv () const;
327  ConfigVector_t neutral () const;
328 
330  std::string name () const;
331 
332  Derived& derived ()
333  {
334  return static_cast <Derived&> (*this);
335  }
336 
337  const Derived& derived () const
338  {
339  return static_cast <const Derived&> (*this);
340  }
342 
343  protected:
347  LieGroupBase() {}
348 
352  LieGroupBase( const LieGroupBase & /*clone*/) {}
353  }; // struct LieGroupBase
354 
355 } // namespace pinocchio
356 
357 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
358 
359 #endif // ifndef __pinocchio_lie_group_operation_base_hpp__
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::dIntegrate
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...
Definition: liegroup-base.hpp:96
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::squaredDistance
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
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::dIntegrate_dv
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...
pinocchio::LieGroupBase::neutral
ConfigVector_t neutral() const
Get neutral element as a vector.
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::LieGroupBase::name
std::string name() const
Get name of instance.
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
Computes the Jacobian of a small variation of the configuration vector into tangent space at identity...
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::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: spatial/fwd.hpp:39
pinocchio::LieGroupBase::LieGroupBase
LieGroupBase(const LieGroupBase &)
Definition: liegroup-base.hpp:352
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::LieGroupBase::nv
Index nv() const
Get dimension of Lie Group tangent space.
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::traits
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:29
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::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::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.