pinocchio  2.5.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<typename Scalar, int Options = 0,
17  template<typename,int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
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>
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 
49  : m_nq(0), m_nv(0)
50  , lg_nqs(0), lg_nvs(0)
51  , m_neutral(0)
52  {};
53 
60  : m_nq(0), m_nv(0)
61  , lg_nqs(0), lg_nvs(0)
62  , m_neutral(0)
63  {
64  append(lg);
65  };
66 
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 
97 
104 
111  {
112  append(lg);
113  return *this;
114  }
115 
116  int nq() const { return m_nq; }
117  int nv() const { return m_nv; }
118 
119  std::string name() const { return m_name; }
120 
121  ConfigVector_t neutral() const { return m_neutral; }
122 
123  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
124  void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
125  const Eigen::MatrixBase<ConfigR_t> & q1,
126  const Eigen::MatrixBase<Tangent_t> & d) const;
127 
128  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
129  void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
130  const Eigen::MatrixBase<ConfigR_t> & q1,
131  const Eigen::MatrixBase<JacobianOut_t> & J) const;
132 
133  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
134  void dDifference_product_impl(const ConfigL_t & q0,
135  const ConfigR_t & q1,
136  const JacobianIn_t & Jin,
137  JacobianOut_t & Jout,
138  bool dDifferenceOnTheLeft,
139  const AssignmentOperatorType op) const;
140 
141  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
142  void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
143  const Eigen::MatrixBase<Velocity_t> & v,
144  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
145 
146  template <class Config_t, class Jacobian_t>
147  void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
148  const Eigen::MatrixBase<Jacobian_t> & J) const;
149 
150  template <class Config_t, class Tangent_t, class JacobianOut_t>
151  void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
152  const Eigen::MatrixBase<Tangent_t> & v,
153  const Eigen::MatrixBase<JacobianOut_t> & J,
154  const AssignmentOperatorType op=SETTO) const;
155 
156  template <class Config_t, class Tangent_t, class JacobianOut_t>
157  void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
158  const Eigen::MatrixBase<Tangent_t> & v,
159  const Eigen::MatrixBase<JacobianOut_t> & J,
160  const AssignmentOperatorType op=SETTO) const;
161 
162  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
163  void dIntegrate_product_impl(const Config_t & q,
164  const Tangent_t & v,
165  const JacobianIn_t & Jin,
166  JacobianOut_t & Jout,
167  bool dIntegrateOnTheLeft,
168  const ArgumentPosition arg,
169  const AssignmentOperatorType op) const;
170 
171  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
172  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
173  const Eigen::MatrixBase<Tangent_t> & v,
174  const Eigen::MatrixBase<JacobianIn_t> & J_in,
175  const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
176 
177  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
178  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
179  const Eigen::MatrixBase<Tangent_t> & v,
180  const Eigen::MatrixBase<JacobianIn_t> & J_in,
181  const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
182 
183  template <class Config_t, class Tangent_t, class JacobianOut_t>
184  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
185  const Eigen::MatrixBase<Tangent_t> & v,
186  const Eigen::MatrixBase<JacobianOut_t> & J) const;
187 
188  template <class Config_t, class Tangent_t, class JacobianOut_t>
189  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
190  const Eigen::MatrixBase<Tangent_t> & v,
191  const Eigen::MatrixBase<JacobianOut_t> & J) const;
192 
193  template <class ConfigL_t, class ConfigR_t>
194  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
195  const Eigen::MatrixBase<ConfigR_t> & q1) const;
196 
197  template <class Config_t>
198  void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;
199 
200  template <class Config_t>
201  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;
202 
203  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
204  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
205  const Eigen::MatrixBase<ConfigR_t> & upper,
206  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
207 
208  template <class ConfigL_t, class ConfigR_t>
209  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
210  const Eigen::MatrixBase<ConfigR_t> & q1,
211  const Scalar & prec) const;
212 
213  bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;
214 
215  template <typename LieGroup1, typename LieGroup2>
216  bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
217 
218  protected:
219 
220  PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
221  Index m_nq, m_nv;
222  std::vector<Index> lg_nqs, lg_nvs;
223  std::string m_name;
224 
225  ConfigVector_t m_neutral;
226 
227  };
228 
229 } // namespace pinocchio
230 
231 #include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
232 
233 #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
Main pinocchio namespace.
Definition: treeview.dox:24