pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
13namespace 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>
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::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::operator*=
CartesianProductOperationVariantTpl & operator*=(const CartesianProductOperationVariantTpl &other)
Append other to *this.
pinocchio::CartesianProductOperationVariantTpl::operator*=
CartesianProductOperationVariantTpl & operator*=(const LieGroupBase< LieGroupDerived > &lg)
Append a Lie group to *this.
Definition cartesian-product-variant.hpp:150
pinocchio::CartesianProductOperationVariantTpl::append
void append(const LieGroupBase< LieGroupDerived > &lg)
Append a Lie group to the Cartesian product.
Definition cartesian-product-variant.hpp:108
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72