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 
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 
97 
104 
110  inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)
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__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg1, const LieGroupGeneric &lg2)
Constructor with two Lie groups.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
Main pinocchio namespace.
Definition: treeview.dox:24
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg)
Constructor with one single Lie group.
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)