pinocchio  3.4.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
cartesian-product-variant.hpp
1 //
2 // Copyright (c) 2018 CNRS
3 //
4 
5 #ifndef __pinocchio_cartesian_product_variant_hpp__
6 #define __pinocchio_cartesian_product_variant_hpp__
7 
8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
10 
11 #include "pinocchio/container/aligned-vector.hpp"
12 
13 namespace pinocchio
14 {
15 
16  template<
17  typename Scalar,
18  int Options = context::Options,
19  template<typename, int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
20  struct CartesianProductOperationVariantTpl;
21  typedef CartesianProductOperationVariantTpl<
22  context::Scalar,
23  context::Options,
24  LieGroupCollectionDefaultTpl>
25  CartesianProductOperationVariant;
26 
27  template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
28  struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>>
29  {
30  typedef _Scalar Scalar;
31  enum
32  {
33  Options = _Options,
34  NQ = Eigen::Dynamic,
35  NV = Eigen::Dynamic
36  };
37  };
38 
42  template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
43  struct CartesianProductOperationVariantTpl
44  : public LieGroupBase<
45  CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>>
46  {
47  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 
49  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
50 
51  typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
52  typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
53  typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
54 
56  CartesianProductOperationVariantTpl()
57  : m_nq(0)
58  , m_nv(0)
59  , lg_nqs(0)
60  , lg_nvs(0)
61  , m_neutral(0) {};
62 
68  explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
69  : m_nq(0)
70  , m_nv(0)
71  , lg_nqs(0)
72  , lg_nvs(0)
73  , m_neutral(0)
74  {
75  append(lg);
76  };
77 
84  CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1, const LieGroupGeneric & lg2)
85  : m_nq(0)
86  , m_nv(0)
87  , lg_nqs(0)
88  , lg_nvs(0)
89  , m_neutral(0)
90  {
91  append(lg1);
92  append(lg2);
93  };
94 
100  void append(const LieGroupGeneric & lg);
101 
107  template<typename LieGroupDerived>
108  void append(const LieGroupBase<LieGroupDerived> & lg)
109  {
110  LieGroupGeneric lgGeneric(lg);
111  append(lgGeneric);
112  }
113 
121  CartesianProductOperationVariantTpl
122  operator*(const CartesianProductOperationVariantTpl & other) const;
123 
129  CartesianProductOperationVariantTpl &
130  operator*=(const CartesianProductOperationVariantTpl & other);
131 
137  inline CartesianProductOperationVariantTpl & operator*=(const LieGroupGeneric & lg)
138  {
139  append(lg);
140  return *this;
141  }
142 
148  template<typename LieGroupDerived>
149  inline CartesianProductOperationVariantTpl &
150  operator*=(const LieGroupBase<LieGroupDerived> & lg)
151  {
152  append<LieGroupDerived>(lg);
153  return *this;
154  }
155 
156  int nq() const
157  {
158  return m_nq;
159  }
160  int nv() const
161  {
162  return m_nv;
163  }
164 
165  std::string name() const
166  {
167  return m_name;
168  }
169 
170  ConfigVector_t neutral() const
171  {
172  return m_neutral;
173  }
174 
175  template<class ConfigL_t, class ConfigR_t, class Tangent_t>
176  void difference_impl(
177  const Eigen::MatrixBase<ConfigL_t> & q0,
178  const Eigen::MatrixBase<ConfigR_t> & q1,
179  const Eigen::MatrixBase<Tangent_t> & d) const;
180 
181  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
182  void dDifference_impl(
183  const Eigen::MatrixBase<ConfigL_t> & q0,
184  const Eigen::MatrixBase<ConfigR_t> & q1,
185  const Eigen::MatrixBase<JacobianOut_t> & J) const;
186 
187  template<
188  ArgumentPosition arg,
189  class ConfigL_t,
190  class ConfigR_t,
191  class JacobianIn_t,
192  class JacobianOut_t>
193  void dDifference_product_impl(
194  const ConfigL_t & q0,
195  const ConfigR_t & q1,
196  const JacobianIn_t & Jin,
197  JacobianOut_t & Jout,
198  bool dDifferenceOnTheLeft,
199  const AssignmentOperatorType op) const;
200 
201  template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
202  void integrate_impl(
203  const Eigen::MatrixBase<ConfigIn_t> & q,
204  const Eigen::MatrixBase<Velocity_t> & v,
205  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
206 
207  template<class Config_t, class Jacobian_t>
208  void integrateCoeffWiseJacobian_impl(
209  const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const;
210 
211  template<class Config_t, class Tangent_t, class JacobianOut_t>
212  void dIntegrate_dq_impl(
213  const Eigen::MatrixBase<Config_t> & q,
214  const Eigen::MatrixBase<Tangent_t> & v,
215  const Eigen::MatrixBase<JacobianOut_t> & J,
216  const AssignmentOperatorType op = SETTO) const;
217 
218  template<class Config_t, class Tangent_t, class JacobianOut_t>
219  void dIntegrate_dv_impl(
220  const Eigen::MatrixBase<Config_t> & q,
221  const Eigen::MatrixBase<Tangent_t> & v,
222  const Eigen::MatrixBase<JacobianOut_t> & J,
223  const AssignmentOperatorType op = SETTO) const;
224 
225  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
226  void dIntegrate_product_impl(
227  const Config_t & q,
228  const Tangent_t & v,
229  const JacobianIn_t & Jin,
230  JacobianOut_t & Jout,
231  bool dIntegrateOnTheLeft,
232  const ArgumentPosition arg,
233  const AssignmentOperatorType op) const;
234 
235  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
236  void dIntegrateTransport_dq_impl(
237  const Eigen::MatrixBase<Config_t> & q,
238  const Eigen::MatrixBase<Tangent_t> & v,
239  const Eigen::MatrixBase<JacobianIn_t> & J_in,
240  const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
241 
242  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
243  void dIntegrateTransport_dv_impl(
244  const Eigen::MatrixBase<Config_t> & q,
245  const Eigen::MatrixBase<Tangent_t> & v,
246  const Eigen::MatrixBase<JacobianIn_t> & J_in,
247  const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
248 
249  template<class Config_t, class Tangent_t, class JacobianOut_t>
250  void dIntegrateTransport_dq_impl(
251  const Eigen::MatrixBase<Config_t> & q,
252  const Eigen::MatrixBase<Tangent_t> & v,
253  const Eigen::MatrixBase<JacobianOut_t> & J) const;
254 
255  template<class Config_t, class Tangent_t, class JacobianOut_t>
256  void dIntegrateTransport_dv_impl(
257  const Eigen::MatrixBase<Config_t> & q,
258  const Eigen::MatrixBase<Tangent_t> & v,
259  const Eigen::MatrixBase<JacobianOut_t> & J) const;
260 
261  template<class ConfigL_t, class ConfigR_t>
262  Scalar squaredDistance_impl(
263  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const;
264 
265  template<class Config_t>
266  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
267 
268  template<class Config_t>
269  bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qout, const Scalar & prec) const;
270 
271  template<class Config_t>
272  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const;
273 
274  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
275  void randomConfiguration_impl(
276  const Eigen::MatrixBase<ConfigL_t> & lower,
277  const Eigen::MatrixBase<ConfigR_t> & upper,
278  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
279 
280  template<class ConfigL_t, class ConfigR_t>
281  bool isSameConfiguration_impl(
282  const Eigen::MatrixBase<ConfigL_t> & q0,
283  const Eigen::MatrixBase<ConfigR_t> & q1,
284  const Scalar & prec) const;
285 
286  bool isEqual_impl(const CartesianProductOperationVariantTpl & other) const;
287 
288  template<typename LieGroup1, typename LieGroup2>
289  bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
290 
291  protected:
292  PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
293  Index m_nq, m_nv;
294  std::vector<Index> lg_nqs, lg_nvs;
295  std::string m_name;
296 
297  ConfigVector_t m_neutral;
298  };
299 
300 } // namespace pinocchio
301 
302 #include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
303 
304 #endif // ifndef __pinocchio_cartesian_product_variant_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:122
pinocchio::CartesianProductOperationVariantTpl
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
Definition: cartesian-product-variant.hpp:46
pinocchio::CartesianProductOperationVariantTpl::operator*=
CartesianProductOperationVariantTpl & operator*=(const CartesianProductOperationVariantTpl &other)
Append other to *this.
pinocchio::CartesianProductOperationVariantTpl::append
void append(const LieGroupGeneric &lg)
Append a Lie group to the Cartesian product.
pinocchio::CartesianProductOperationVariantTpl::operator*
CartesianProductOperationVariantTpl operator*(const CartesianProductOperationVariantTpl &other) const
Cartesian product between *this and other.
pinocchio::CartesianProductOperationVariantTpl::CartesianProductOperationVariantTpl
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg)
Constructor with one single Lie group.
Definition: cartesian-product-variant.hpp:68
pinocchio::CartesianProductOperationVariantTpl::CartesianProductOperationVariantTpl
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg1, const LieGroupGeneric &lg2)
Constructor with two Lie groups.
Definition: cartesian-product-variant.hpp:84
pinocchio::CartesianProductOperationVariantTpl::operator*=
CartesianProductOperationVariantTpl & operator*=(const LieGroupGeneric &lg)
Append a Lie group to *this.
Definition: cartesian-product-variant.hpp:137
pinocchio::CartesianProductOperationVariantTpl::append
void append(const LieGroupBase< LieGroupDerived > &lg)
Append a Lie group to the Cartesian product.
Definition: cartesian-product-variant.hpp:108
pinocchio::CartesianProductOperationVariantTpl::operator*=
CartesianProductOperationVariantTpl & operator*=(const LieGroupBase< LieGroupDerived > &lg)
Append a Lie group to *this.
Definition: cartesian-product-variant.hpp:150
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72