cartesian-product-variant.hpp
Go to the documentation of this file.
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 
42 
43  typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
44  typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
46 
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>
96  {
97  LieGroupGeneric lgGeneric (lg);
98  append(lgGeneric);
99  }
100 
109 
116 
122  inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)
123  {
124  append(lg);
125  return *this;
126  }
127 
133  template<typename LieGroupDerived>
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>
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 
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__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
result_of::push_front< V const, T >::type append(T const &t, V const &v)
Append the element T at the front of boost fusion vector V.
Definition: fusion.hpp:24
int NQ
Definition: dpendulum.py:8
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: src/fwd.hpp:59
LieGroupGenericTpl< LieGroupCollection > LieGroupGeneric
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)
LieGroupCollectionTpl< Scalar, Options > LieGroupCollection
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
CartesianProductOperationVariantTpl< double, 0, LieGroupCollectionDefaultTpl > CartesianProductOperationVariant
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
SE3::Scalar Scalar
Definition: conversions.cpp:13
AssignmentOperatorType
Definition: src/fwd.hpp:68
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
void append(const LieGroupBase< LieGroupDerived > &lg)
Append a Lie group to the Cartesian product.
FCL_REAL d
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg)
Constructor with one single Lie group.
Main pinocchio namespace.
Definition: timings.cpp:28
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
std::size_t Index
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg1, const LieGroupGeneric &lg2)
Constructor with two Lie groups.
NV
Definition: dcrba.py:444
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28