pinocchio  2.7.1
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<typename Scalar, int Options = 0,
17  template<typename,int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
18  struct CartesianProductOperationVariantTpl;
19  typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl> CartesianProductOperationVariant;
20 
21  template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
22  struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
23  {
24  typedef _Scalar Scalar;
25  enum {
26  Options = _Options,
27  NQ = Eigen::Dynamic,
28  NV = Eigen::Dynamic
29  };
30  };
31 
35  template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
36  struct CartesianProductOperationVariantTpl
37  : public LieGroupBase<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
38  {
39  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40 
41  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
42 
43  typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
44  typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
45  typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
46 
48  CartesianProductOperationVariantTpl()
49  : m_nq(0), m_nv(0)
50  , lg_nqs(0), lg_nvs(0)
51  , m_neutral(0)
52  {};
53 
59  explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
60  : m_nq(0), m_nv(0)
61  , lg_nqs(0), lg_nvs(0)
62  , m_neutral(0)
63  {
64  append(lg);
65  };
66 
73  CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1,
74  const LieGroupGeneric & lg2)
75  : m_nq(0), m_nv(0)
76  , lg_nqs(0), lg_nvs(0)
77  , m_neutral(0)
78  {
79  append(lg1); append(lg2);
80  };
81 
87  void append(const LieGroupGeneric & lg);
88 
94  template<typename LieGroupDerived>
95  void append (const LieGroupBase<LieGroupDerived>& lg)
96  {
97  LieGroupGeneric lgGeneric (lg);
98  append(lgGeneric);
99  }
100 
108  CartesianProductOperationVariantTpl operator* (const CartesianProductOperationVariantTpl& other) const;
109 
115  CartesianProductOperationVariantTpl& operator*= (const CartesianProductOperationVariantTpl& other);
116 
122  inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)
123  {
124  append(lg);
125  return *this;
126  }
127 
133  template<typename LieGroupDerived>
134  inline CartesianProductOperationVariantTpl& operator*= (const LieGroupBase<LieGroupDerived>& lg)
135  {
136  append<LieGroupDerived>(lg);
137  return *this;
138  }
139 
140  int nq() const { return m_nq; }
141  int nv() const { return m_nv; }
142 
143  std::string name() const { return m_name; }
144 
145  ConfigVector_t neutral() const { return m_neutral; }
146 
147  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
148  void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
149  const Eigen::MatrixBase<ConfigR_t> & q1,
150  const Eigen::MatrixBase<Tangent_t> & d) const;
151 
152  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
153  void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
154  const Eigen::MatrixBase<ConfigR_t> & q1,
155  const Eigen::MatrixBase<JacobianOut_t> & J) const;
156 
157  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
158  void dDifference_product_impl(const ConfigL_t & q0,
159  const ConfigR_t & q1,
160  const JacobianIn_t & Jin,
161  JacobianOut_t & Jout,
162  bool dDifferenceOnTheLeft,
163  const AssignmentOperatorType op) const;
164 
165  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
166  void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
167  const Eigen::MatrixBase<Velocity_t> & v,
168  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
169 
170  template <class Config_t, class Jacobian_t>
171  void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
172  const Eigen::MatrixBase<Jacobian_t> & J) const;
173 
174  template <class Config_t, class Tangent_t, class JacobianOut_t>
175  void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
176  const Eigen::MatrixBase<Tangent_t> & v,
177  const Eigen::MatrixBase<JacobianOut_t> & J,
178  const AssignmentOperatorType op=SETTO) const;
179 
180  template <class Config_t, class Tangent_t, class JacobianOut_t>
181  void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
182  const Eigen::MatrixBase<Tangent_t> & v,
183  const Eigen::MatrixBase<JacobianOut_t> & J,
184  const AssignmentOperatorType op=SETTO) const;
185 
186  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
187  void dIntegrate_product_impl(const Config_t & q,
188  const Tangent_t & v,
189  const JacobianIn_t & Jin,
190  JacobianOut_t & Jout,
191  bool dIntegrateOnTheLeft,
192  const ArgumentPosition arg,
193  const AssignmentOperatorType op) const;
194 
195  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
196  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
197  const Eigen::MatrixBase<Tangent_t> & v,
198  const Eigen::MatrixBase<JacobianIn_t> & J_in,
199  const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
200 
201  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
202  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
203  const Eigen::MatrixBase<Tangent_t> & v,
204  const Eigen::MatrixBase<JacobianIn_t> & J_in,
205  const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
206 
207  template <class Config_t, class Tangent_t, class JacobianOut_t>
208  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
209  const Eigen::MatrixBase<Tangent_t> & v,
210  const Eigen::MatrixBase<JacobianOut_t> & J) const;
211 
212  template <class Config_t, class Tangent_t, class JacobianOut_t>
213  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
214  const Eigen::MatrixBase<Tangent_t> & v,
215  const Eigen::MatrixBase<JacobianOut_t> & J) const;
216 
217  template <class ConfigL_t, class ConfigR_t>
218  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
219  const Eigen::MatrixBase<ConfigR_t> & q1) const;
220 
221  template <class Config_t>
222  void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;
223 
224  template <class Config_t>
225  bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qout,
226  const Scalar& prec) const;
227 
228  template <class Config_t>
229  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;
230 
231  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
232  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
233  const Eigen::MatrixBase<ConfigR_t> & upper,
234  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
235 
236  template <class ConfigL_t, class ConfigR_t>
237  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
238  const Eigen::MatrixBase<ConfigR_t> & q1,
239  const Scalar & prec) const;
240 
241  bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;
242 
243  template <typename LieGroup1, typename LieGroup2>
244  bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
245 
246  protected:
247 
248  PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
249  Index m_nq, m_nv;
250  std::vector<Index> lg_nqs, lg_nvs;
251  std::string m_name;
252 
253  ConfigVector_t m_neutral;
254 
255  };
256 
257 } // namespace pinocchio
258 
259 #include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
260 
261 #endif // ifndef __pinocchio_cartesian_product_variant_hpp__
pinocchio::LieGroupGenericTpl
Definition: fwd.hpp:12
pinocchio::CartesianProductOperationVariantTpl::operator*=
CartesianProductOperationVariantTpl & operator*=(const CartesianProductOperationVariantTpl &other)
Append other to *this.
pinocchio::CartesianProductOperationVariantTpl
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
Definition: cartesian-product-variant.hpp:18
pinocchio::CartesianProductOperationVariantTpl::CartesianProductOperationVariantTpl
CartesianProductOperationVariantTpl()
Default constructor.
Definition: cartesian-product-variant.hpp:48
pinocchio::CartesianProductOperationVariantTpl::CartesianProductOperationVariantTpl
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg1, const LieGroupGeneric &lg2)
Constructor with two Lie groups.
Definition: cartesian-product-variant.hpp:73
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:59
pinocchio::CartesianProductOperationVariantTpl::append
void append(const LieGroupGeneric &lg)
Append a Lie group to the Cartesian product.
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
pinocchio::LieGroupBase
Definition: liegroup-base.hpp:40
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::CartesianProductOperationVariantTpl::append
void append(const LieGroupBase< LieGroupDerived > &lg)
Append a Lie group to the Cartesian product.
Definition: cartesian-product-variant.hpp:95
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11