| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018 CNRS |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_lie_group_variant_visitor_hxx__ |
||
6 |
#define __pinocchio_lie_group_variant_visitor_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/liegroup/liegroup-base.hpp" |
||
9 |
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp" |
||
10 |
#include "pinocchio/multibody/visitor.hpp" |
||
11 |
|||
12 |
#include <string> |
||
13 |
|||
14 |
#define LIE_GROUP_VISITOR(VISITOR) \ |
||
15 |
VISITOR(ArgsType & args) : args(args) {} \ |
||
16 |
ArgsType & args |
||
17 |
|||
18 |
namespace pinocchio |
||
19 |
{ |
||
20 |
|||
21 |
namespace visitor |
||
22 |
{ |
||
23 |
namespace bf = boost::fusion; |
||
24 |
|||
25 |
template<typename Visitor> |
||
26 |
struct LieGroupVisitorBase : public boost::static_visitor<> |
||
27 |
{ |
||
28 |
template<typename LieGroupDerived> |
||
29 |
352 |
void operator() (const LieGroupBase<LieGroupDerived> & lg) const |
|
30 |
{ |
||
31 |
✓✗ | 352 |
bf::invoke(&Visitor::template algo<LieGroupDerived>, |
32 |
✓✗ | 352 |
bf::append(boost::ref(lg), |
33 |
352 |
static_cast<const Visitor*>(this)->args)); |
|
34 |
352 |
} |
|
35 |
|||
36 |
template<typename LieGroupCollection, typename ArgsTmp> |
||
37 |
352 |
static void run(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
38 |
ArgsTmp args) |
||
39 |
{ |
||
40 |
✓✗ | 352 |
return boost::apply_visitor(Visitor(args),lg); |
41 |
} |
||
42 |
}; |
||
43 |
|||
44 |
template<typename Scalar, int Options> |
||
45 |
struct LieGroupEqual : public boost::static_visitor<bool> |
||
46 |
{ |
||
47 |
template <typename T, typename U> |
||
48 |
bool operator()( const T &, const U & ) const |
||
49 |
{ |
||
50 |
// Should we handle |
||
51 |
// - dynamic size vector space versus static size vector space |
||
52 |
// - dynamic versus static cartesian product |
||
53 |
return false; |
||
54 |
} |
||
55 |
|||
56 |
template <typename T> |
||
57 |
18 |
bool operator()( const T & lhs, const T & rhs ) const |
|
58 |
{ |
||
59 |
18 |
return lhs == rhs; |
|
60 |
} |
||
61 |
|||
62 |
/// \name Vector space comparison |
||
63 |
/// \{ |
||
64 |
1 |
bool operator() (const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs, |
|
65 |
const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs ) const |
||
66 |
{ |
||
67 |
1 |
return lhs == rhs; |
|
68 |
} |
||
69 |
|||
70 |
template <int Dim> |
||
71 |
4 |
bool operator() (const VectorSpaceOperationTpl<Dim, Scalar, Options> & lhs, |
|
72 |
const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs ) const |
||
73 |
{ |
||
74 |
4 |
return lhs.nq() == rhs.nq(); |
|
75 |
} |
||
76 |
|||
77 |
template <int Dim> |
||
78 |
bool operator() (const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs, |
||
79 |
const VectorSpaceOperationTpl<Dim, Scalar, Options> & rhs ) const |
||
80 |
{ |
||
81 |
return lhs.nq() == rhs.nq(); |
||
82 |
} |
||
83 |
/// \} |
||
84 |
|||
85 |
template <typename LieGroup1, typename LieGroup2, |
||
86 |
template<typename,int> class LieGroupCollectionTpl> |
||
87 |
bool operator() (const CartesianProductOperation<LieGroup1, LieGroup2> & lhs, |
||
88 |
const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & rhs ) const |
||
89 |
{ |
||
90 |
return rhs.isEqual(lhs); |
||
91 |
} |
||
92 |
|||
93 |
template <typename LieGroup1, typename LieGroup2, |
||
94 |
template<typename,int> class LieGroupCollectionTpl> |
||
95 |
bool operator() (const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & lhs, |
||
96 |
const CartesianProductOperation<LieGroup1, LieGroup2> & rhs) const |
||
97 |
{ |
||
98 |
return lhs.isEqual(rhs); |
||
99 |
} |
||
100 |
}; |
||
101 |
} |
||
102 |
|||
103 |
#define PINOCCHIO_LG_CHECK_VECTOR_SIZE(type,var,exp_size) \ |
||
104 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(type); \ |
||
105 |
assert(var.size() == exp_size) |
||
106 |
#define PINOCCHIO_LG_CHECK_MATRIX_SIZE(var,nr,nc) \ |
||
107 |
assert(var.rows() == nr); assert(var.cols() == nc) |
||
108 |
|||
109 |
#define PINOCCHIO_LG_VISITOR(Name,type,_method) \ |
||
110 |
/** @brief Lie Group visitor of the _method */ \ |
||
111 |
struct LieGroup ## Name ## Visitor: public boost::static_visitor<type> \ |
||
112 |
{ \ |
||
113 |
template<typename LieGroupDerived> \ |
||
114 |
type operator()(const LieGroupBase<LieGroupDerived> & lg) const \ |
||
115 |
{ return lg._method(); } \ |
||
116 |
\ |
||
117 |
template<typename LieGroupCollection> \ |
||
118 |
static type run(const LieGroupGenericTpl<LieGroupCollection> & lg) \ |
||
119 |
{ return boost::apply_visitor( LieGroup ## Name ## Visitor(), lg ); } \ |
||
120 |
}; \ |
||
121 |
\ |
||
122 |
template<typename LieGroupCollection> \ |
||
123 |
inline type _method(const LieGroupGenericTpl<LieGroupCollection> & lg) \ |
||
124 |
{ return LieGroup ## Name ## Visitor::run(lg); } |
||
125 |
|||
126 |
✓✗ | 2004 |
PINOCCHIO_LG_VISITOR(Nq,int,nq) |
127 |
✓✗ | 492 |
PINOCCHIO_LG_VISITOR(Nv,int,nv) |
128 |
✓✗ | 84 |
PINOCCHIO_LG_VISITOR(Name,std::string,name) |
129 |
#undef PINOCCHIO_LG_VISITOR |
||
130 |
|||
131 |
/** |
||
132 |
* @brief Visitor of the Lie Group neutral element |
||
133 |
*/ |
||
134 |
template<typename Vector> |
||
135 |
struct LieGroupNeutralVisitor: public boost::static_visitor<Vector> |
||
136 |
{ |
||
137 |
template<typename LieGroupDerived> |
||
138 |
56 |
Vector operator()(const LieGroupBase<LieGroupDerived> & lg) const |
|
139 |
✓✗ | 56 |
{ return lg.neutral(); } |
140 |
|||
141 |
template<typename LieGroupCollection> |
||
142 |
28 |
static Vector run(const LieGroupGenericTpl<LieGroupCollection> & lg) |
|
143 |
✓✗ | 28 |
{ return boost::apply_visitor( LieGroupNeutralVisitor(), lg ); } |
144 |
}; |
||
145 |
|||
146 |
template<typename LieGroupCollection> |
||
147 |
inline Eigen::Matrix<typename LieGroupCollection::Scalar,Eigen::Dynamic,1,LieGroupCollection::Options> |
||
148 |
28 |
neutral(const LieGroupGenericTpl<LieGroupCollection> & lg) |
|
149 |
{ |
||
150 |
typedef Eigen::Matrix<typename LieGroupCollection::Scalar,Eigen::Dynamic,1,LieGroupCollection::Options> ReturnType; |
||
151 |
28 |
return LieGroupNeutralVisitor<ReturnType>::run(lg); |
|
152 |
} |
||
153 |
|||
154 |
#define PINOCCHIO_LG_VISITOR(Name,_method) \ |
||
155 |
/** @brief Visitor of the Lie Group _method method */ \ |
||
156 |
template <class M_t> \ |
||
157 |
struct LieGroup ## Name ## Visitor \ |
||
158 |
: visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<M_t> > \ |
||
159 |
{ \ |
||
160 |
typedef boost::fusion::vector<const Eigen::MatrixBase<M_t> &> ArgsType; \ |
||
161 |
LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor); \ |
||
162 |
template<typename LieGroupDerived> \ |
||
163 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, \ |
||
164 |
const Eigen::MatrixBase<M_t>& m1) \ |
||
165 |
{ lg._method(m1); } \ |
||
166 |
} |
||
167 |
|||
168 |
104 |
PINOCCHIO_LG_VISITOR(Random, random); |
|
169 |
36 |
PINOCCHIO_LG_VISITOR(Normalize, normalize); |
|
170 |
|||
171 |
template<typename LieGroupCollection, class Config_t> |
||
172 |
36 |
inline void random(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
173 |
const Eigen::MatrixBase<Config_t> & qout) |
||
174 |
{ |
||
175 |
✗✓ | 36 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qout, nq(lg)); |
176 |
|||
177 |
typedef LieGroupRandomVisitor<Config_t> Operation; |
||
178 |
✓✗ | 36 |
Operation::run(lg,typename Operation::ArgsType(qout)); |
179 |
36 |
} |
|
180 |
|||
181 |
template<typename LieGroupCollection, class Config_t> |
||
182 |
28 |
inline void normalize(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
183 |
const Eigen::MatrixBase<Config_t> & qout) |
||
184 |
{ |
||
185 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qout, nq(lg)); |
186 |
|||
187 |
typedef LieGroupNormalizeVisitor<Config_t> Operation; |
||
188 |
✓✗ | 28 |
Operation::run(lg,typename Operation::ArgsType(qout)); |
189 |
28 |
} |
|
190 |
|||
191 |
#undef PINOCCHIO_LG_VISITOR |
||
192 |
|||
193 |
/** @brief Visitor of the Lie Group isNormalized method */ |
||
194 |
template <class Matrix_t> |
||
195 |
struct LieGroupIsNormalizedVisitor |
||
196 |
: visitor::LieGroupVisitorBase< LieGroupIsNormalizedVisitor<Matrix_t> > |
||
197 |
{ |
||
198 |
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix_t> &, |
||
199 |
const typename Matrix_t::Scalar&, |
||
200 |
bool&> ArgsType; |
||
201 |
8 |
LIE_GROUP_VISITOR(LieGroupIsNormalizedVisitor); |
|
202 |
template<typename LieGroupDerived> |
||
203 |
16 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
|
204 |
const Eigen::MatrixBase<Matrix_t>& qin, |
||
205 |
const typename Matrix_t::Scalar& prec, |
||
206 |
bool& res) |
||
207 |
{ |
||
208 |
16 |
res = lg.isNormalized(qin, prec); |
|
209 |
16 |
} |
|
210 |
}; |
||
211 |
|||
212 |
template<typename LieGroupCollection, class Config_t> |
||
213 |
8 |
inline bool isNormalized(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
214 |
const Eigen::MatrixBase<Config_t> & qin, |
||
215 |
const typename Config_t::Scalar& prec) |
||
216 |
{ |
||
217 |
✓✗✓✗ ✗✓ |
8 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qin, nq(lg)); |
218 |
|||
219 |
bool res; |
||
220 |
typedef LieGroupIsNormalizedVisitor<Config_t> Operation; |
||
221 |
✓✗✓✗ |
8 |
Operation::run(lg,typename Operation::ArgsType(qin,prec,res)); |
222 |
8 |
return res; |
|
223 |
} |
||
224 |
|||
225 |
/** @brief Visitor of the Lie Group isSameConfiguration method */ |
||
226 |
template <class Matrix1_t, class Matrix2_t> |
||
227 |
struct LieGroupIsSameConfigurationVisitor |
||
228 |
: visitor::LieGroupVisitorBase< LieGroupIsSameConfigurationVisitor<Matrix1_t, Matrix2_t> > |
||
229 |
{ |
||
230 |
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &, |
||
231 |
const Eigen::MatrixBase<Matrix2_t> &, |
||
232 |
const typename Matrix1_t::Scalar &, |
||
233 |
bool&> ArgsType; |
||
234 |
20 |
LIE_GROUP_VISITOR(LieGroupIsSameConfigurationVisitor); |
|
235 |
template<typename LieGroupDerived> |
||
236 |
20 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
|
237 |
const Eigen::MatrixBase<Matrix1_t>& q0, |
||
238 |
const Eigen::MatrixBase<Matrix2_t>& q1, |
||
239 |
const typename Matrix1_t::Scalar & prec, |
||
240 |
bool& res) |
||
241 |
{ |
||
242 |
20 |
res = lg.isSameConfiguration(q0, q1, prec); |
|
243 |
} |
||
244 |
}; |
||
245 |
|||
246 |
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t> |
||
247 |
20 |
inline bool isSameConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
248 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
249 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
250 |
const typename ConfigL_t::Scalar& prec) |
||
251 |
{ |
||
252 |
✓✗✓✗ ✗✓ |
20 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
253 |
✓✗✓✗ ✗✓ |
20 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
254 |
|||
255 |
bool res; |
||
256 |
typedef LieGroupIsSameConfigurationVisitor<ConfigL_t,ConfigR_t> Operation; |
||
257 |
✓✗✓✗ |
20 |
Operation::run(lg,typename Operation::ArgsType(q0,q1,prec,res)); |
258 |
20 |
return res; |
|
259 |
} |
||
260 |
|||
261 |
#define PINOCCHIO_LG_VISITOR(Name,_method) \ |
||
262 |
/** @brief Visitor of the Lie Group _method method */ \ |
||
263 |
template <class Matrix1_t, class Matrix2_t> \ |
||
264 |
struct LieGroup ## Name ## Visitor \ |
||
265 |
: visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<Matrix1_t, Matrix2_t> > \ |
||
266 |
{ \ |
||
267 |
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &, \ |
||
268 |
const Eigen::MatrixBase<Matrix2_t> &, \ |
||
269 |
typename Matrix1_t::Scalar &> ArgsType; \ |
||
270 |
LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor); \ |
||
271 |
template<typename LieGroupDerived> \ |
||
272 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, \ |
||
273 |
const Eigen::MatrixBase<Matrix1_t>& m1, \ |
||
274 |
const Eigen::MatrixBase<Matrix2_t>& m2, \ |
||
275 |
typename Matrix1_t::Scalar& res) \ |
||
276 |
{ \ |
||
277 |
res = lg._method(m1, m2); \ |
||
278 |
} \ |
||
279 |
} |
||
280 |
|||
281 |
//PINOCCHIO_LG_VISITOR(Distance, distance); |
||
282 |
112 |
PINOCCHIO_LG_VISITOR(SquaredDistance, squaredDistance); |
|
283 |
|||
284 |
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t> |
||
285 |
inline typename ConfigL_t::Scalar |
||
286 |
48 |
squaredDistance(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
287 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
288 |
const Eigen::MatrixBase<ConfigR_t> & q1) |
||
289 |
{ |
||
290 |
✓✗✓✗ ✗✓ |
48 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
291 |
✓✗✓✗ ✗✓ |
48 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
292 |
|||
293 |
typedef LieGroupSquaredDistanceVisitor<ConfigL_t,ConfigR_t> Operation; |
||
294 |
typename ConfigL_t::Scalar d2; |
||
295 |
✓✗✓✗ |
48 |
Operation::run(lg,typename Operation::ArgsType(q0,q1,d2)); |
296 |
48 |
return d2; |
|
297 |
} |
||
298 |
|||
299 |
#undef PINOCCHIO_LG_VISITOR |
||
300 |
|||
301 |
#define PINOCCHIO_LG_VISITOR(Name,_method) \ |
||
302 |
/** @brief Visitor of the Lie Group _method method */ \ |
||
303 |
template <class Matrix1_t, class Matrix2_t, class Matrix3_t> \ |
||
304 |
struct LieGroup ## Name ## Visitor \ |
||
305 |
: visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<Matrix1_t, Matrix2_t, Matrix3_t> > \ |
||
306 |
{ \ |
||
307 |
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &, \ |
||
308 |
const Eigen::MatrixBase<Matrix2_t> &, \ |
||
309 |
const Eigen::MatrixBase<Matrix3_t> &> ArgsType; \ |
||
310 |
LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor); \ |
||
311 |
template<typename LieGroupDerived> \ |
||
312 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, \ |
||
313 |
const Eigen::MatrixBase<Matrix1_t>& m1, \ |
||
314 |
const Eigen::MatrixBase<Matrix2_t>& m2, \ |
||
315 |
const Eigen::MatrixBase<Matrix3_t>& m3) \ |
||
316 |
{ \ |
||
317 |
lg._method(m1, m2, m3); \ |
||
318 |
} \ |
||
319 |
} |
||
320 |
|||
321 |
72 |
PINOCCHIO_LG_VISITOR(Integrate, integrate); |
|
322 |
36 |
PINOCCHIO_LG_VISITOR(Difference, difference); |
|
323 |
20 |
PINOCCHIO_LG_VISITOR(RandomConfiguration, randomConfiguration); |
|
324 |
|||
325 |
template<typename LieGroupCollection, class ConfigIn_t, class Tangent_t, class ConfigOut_t> |
||
326 |
28 |
inline void integrate(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
327 |
const Eigen::MatrixBase<ConfigIn_t> & q, |
||
328 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
329 |
const Eigen::MatrixBase<ConfigOut_t>& qout) |
||
330 |
{ |
||
331 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigIn_t, q, nq(lg)); |
332 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
333 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg)); |
334 |
|||
335 |
typedef LieGroupIntegrateVisitor<ConfigIn_t,Tangent_t,ConfigOut_t> Operation; |
||
336 |
✓✗ | 28 |
Operation::run(lg,typename Operation::ArgsType(q,v,qout)); |
337 |
28 |
} |
|
338 |
|||
339 |
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class Tangent_t> |
||
340 |
28 |
inline void difference(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
341 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
342 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
343 |
const Eigen::MatrixBase<Tangent_t> & v) |
||
344 |
{ |
||
345 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
346 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
347 |
✗✓ | 28 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
348 |
|||
349 |
typedef LieGroupDifferenceVisitor<ConfigL_t,ConfigR_t,Tangent_t> Operation; |
||
350 |
✓✗ | 28 |
Operation::run(lg,typename Operation::ArgsType(q0,q1,v)); |
351 |
28 |
} |
|
352 |
|||
353 |
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
354 |
20 |
inline void randomConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
355 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
356 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
357 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
358 |
{ |
||
359 |
✗✓ | 20 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
360 |
✗✓ | 20 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
361 |
✗✓ | 20 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg)); |
362 |
|||
363 |
typedef LieGroupRandomConfigurationVisitor<ConfigL_t,ConfigR_t,ConfigOut_t> Operation; |
||
364 |
✓✗ | 20 |
Operation::run(lg,typename Operation::ArgsType(q0,q1,qout)); |
365 |
20 |
} |
|
366 |
|||
367 |
#undef PINOCCHIO_LG_VISITOR |
||
368 |
|||
369 |
/** @brief Visitor of the Lie Group interpolate method */ |
||
370 |
template <class Matrix1_t, class Matrix2_t, class Matrix3_t> |
||
371 |
struct LieGroupInterpolateVisitor |
||
372 |
: visitor::LieGroupVisitorBase< LieGroupInterpolateVisitor<Matrix1_t, Matrix2_t, Matrix3_t> > |
||
373 |
{ |
||
374 |
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &, |
||
375 |
const Eigen::MatrixBase<Matrix2_t> &, |
||
376 |
const typename Matrix1_t::Scalar &, |
||
377 |
const Eigen::MatrixBase<Matrix3_t> &> ArgsType; |
||
378 |
LIE_GROUP_VISITOR(LieGroupInterpolateVisitor); |
||
379 |
template<typename LieGroupDerived> |
||
380 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
||
381 |
const Eigen::MatrixBase<Matrix1_t>& q0, |
||
382 |
const Eigen::MatrixBase<Matrix2_t>& q1, |
||
383 |
const typename Matrix1_t::Scalar & u, |
||
384 |
const Eigen::MatrixBase<Matrix3_t>& qout) |
||
385 |
{ |
||
386 |
lg.interpolate(q0, q1, u, qout); |
||
387 |
} |
||
388 |
}; |
||
389 |
|||
390 |
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
391 |
inline void interpolate(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
392 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
393 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
394 |
const typename ConfigL_t::Scalar& u, |
||
395 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
396 |
{ |
||
397 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
||
398 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
||
399 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg)); |
||
400 |
|||
401 |
typedef LieGroupInterpolateVisitor<ConfigL_t,ConfigR_t,ConfigOut_t> Operation; |
||
402 |
Operation::run(lg,typename Operation::ArgsType(q0,q1,u,qout)); |
||
403 |
} |
||
404 |
|||
405 |
#define PINOCCHIO_LG_VISITOR(Name,_method,HasArgPos) \ |
||
406 |
/** @brief Visitor of the Lie Group _method method */ \ |
||
407 |
template <class Matrix1_t, class Matrix2_t, class Matrix3_t> \ |
||
408 |
struct LieGroup ## Name ## Visitor \ |
||
409 |
: visitor::LieGroupVisitorBase< LieGroup ## Name ## Visitor<Matrix1_t, Matrix2_t, Matrix3_t> > \ |
||
410 |
{ \ |
||
411 |
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix1_t> &, \ |
||
412 |
const Eigen::MatrixBase<Matrix2_t> &, \ |
||
413 |
const Eigen::MatrixBase<Matrix3_t> &, \ |
||
414 |
const ArgumentPosition BOOST_PP_COMMA_IF(HasArgPos) \ |
||
415 |
BOOST_PP_IIF(HasArgPos, const AssignmentOperatorType,)> ArgsType; \ |
||
416 |
LIE_GROUP_VISITOR(LieGroup ## Name ## Visitor); \ |
||
417 |
template<typename LieGroupDerived> \ |
||
418 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, \ |
||
419 |
const Eigen::MatrixBase<Matrix1_t>& m1, \ |
||
420 |
const Eigen::MatrixBase<Matrix2_t>& m2, \ |
||
421 |
const Eigen::MatrixBase<Matrix3_t>& m3, \ |
||
422 |
const ArgumentPosition arg BOOST_PP_COMMA_IF(HasArgPos) \ |
||
423 |
BOOST_PP_IF(HasArgPos, const AssignmentOperatorType op = SETTO,)) \ |
||
424 |
{ \ |
||
425 |
lg._method(m1, m2, m3, arg BOOST_PP_COMMA_IF(HasArgPos) BOOST_PP_IF(HasArgPos, op,)); \ |
||
426 |
} \ |
||
427 |
} |
||
428 |
|||
429 |
80 |
PINOCCHIO_LG_VISITOR(DIntegrate, dIntegrate, 1); |
|
430 |
40 |
PINOCCHIO_LG_VISITOR(DDifference, dDifference, 0); |
|
431 |
|||
432 |
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t> |
||
433 |
40 |
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
434 |
const Eigen::MatrixBase<Config_t > & q, |
||
435 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
436 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
437 |
const ArgumentPosition arg, |
||
438 |
const AssignmentOperatorType op) |
||
439 |
{ |
||
440 |
✗✓ | 40 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg)); |
441 |
✗✓ | 40 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
442 |
✗✓✗✓ |
40 |
PINOCCHIO_LG_CHECK_MATRIX_SIZE(J, nv(lg), nv(lg)); |
443 |
|||
444 |
typedef LieGroupDIntegrateVisitor<Config_t,Tangent_t,JacobianOut_t> Operation; |
||
445 |
✓✗ | 40 |
Operation::run(lg,typename Operation::ArgsType(q,v,J,arg,op)); |
446 |
40 |
} |
|
447 |
|||
448 |
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
||
449 |
40 |
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg, |
|
450 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
451 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
452 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
453 |
const ArgumentPosition arg) |
||
454 |
{ |
||
455 |
✗✓ | 40 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
456 |
✗✓ | 40 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
457 |
✗✓✗✓ |
40 |
PINOCCHIO_LG_CHECK_MATRIX_SIZE(J, nv(lg), nv(lg)); |
458 |
|||
459 |
typedef LieGroupDDifferenceVisitor<ConfigL_t,ConfigR_t,JacobianOut_t> Operation; |
||
460 |
✓✗ | 40 |
Operation::run(lg,typename Operation::ArgsType(q0,q1,J,arg)); |
461 |
40 |
} |
|
462 |
|||
463 |
#undef PINOCCHIO_LG_VISITOR |
||
464 |
|||
465 |
template <class M1_t, class M2_t, class M3_t, class M4_t, bool dIntegrateOnTheLeft> |
||
466 |
struct LieGroupDIntegrateProductVisitor |
||
467 |
: visitor::LieGroupVisitorBase< LieGroupDIntegrateProductVisitor<M1_t, M2_t, M3_t, M4_t, dIntegrateOnTheLeft> > |
||
468 |
{ |
||
469 |
typedef boost::fusion::vector<const M1_t &, |
||
470 |
const M2_t &, |
||
471 |
const M3_t &, |
||
472 |
M4_t &, |
||
473 |
const ArgumentPosition, |
||
474 |
const AssignmentOperatorType> ArgsType; |
||
475 |
|||
476 |
LIE_GROUP_VISITOR(LieGroupDIntegrateProductVisitor); |
||
477 |
|||
478 |
template<typename LieGroupDerived> |
||
479 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
||
480 |
const M1_t& q, |
||
481 |
const M2_t& v, |
||
482 |
const M3_t& J_in, |
||
483 |
M4_t& J_out, |
||
484 |
const ArgumentPosition arg, |
||
485 |
const AssignmentOperatorType op) |
||
486 |
{ |
||
487 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(arg==ARG0||arg==ARG1, "arg should be either ARG0 or ARG1"); |
||
488 |
switch (arg) { |
||
489 |
case ARG0: |
||
490 |
if(dIntegrateOnTheLeft) lg.dIntegrate_dq(q, v, SELF, J_in, J_out, op); |
||
491 |
else lg.dIntegrate_dq(q, v, J_in, SELF, J_out, op); |
||
492 |
return; |
||
493 |
case ARG1: |
||
494 |
if(dIntegrateOnTheLeft) lg.dIntegrate_dv(q, v, SELF, J_in, J_out, op); |
||
495 |
else lg.dIntegrate_dv(q, v, J_in, SELF, J_out, op); |
||
496 |
return; |
||
497 |
default: |
||
498 |
return; |
||
499 |
} |
||
500 |
} |
||
501 |
}; |
||
502 |
|||
503 |
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
504 |
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
505 |
const Eigen::MatrixBase<Config_t > & q, |
||
506 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
507 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
508 |
int self, |
||
509 |
const Eigen::MatrixBase<JacobianOut_t> & J_out, |
||
510 |
const ArgumentPosition arg, |
||
511 |
const AssignmentOperatorType op) |
||
512 |
{ |
||
513 |
PINOCCHIO_UNUSED_VARIABLE(self); |
||
514 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg)); |
||
515 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
||
516 |
|||
517 |
typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, false> Operation; |
||
518 |
Operation::run(lg,typename Operation::ArgsType( |
||
519 |
q.derived(),v.derived(),J_in.derived(), |
||
520 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op)); |
||
521 |
} |
||
522 |
|||
523 |
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
524 |
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
525 |
const Eigen::MatrixBase<Config_t > & q, |
||
526 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
527 |
int self, |
||
528 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
529 |
const Eigen::MatrixBase<JacobianOut_t> & J_out, |
||
530 |
const ArgumentPosition arg, |
||
531 |
const AssignmentOperatorType op) |
||
532 |
{ |
||
533 |
PINOCCHIO_UNUSED_VARIABLE(self); |
||
534 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg)); |
||
535 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
||
536 |
|||
537 |
typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, true> Operation; |
||
538 |
Operation::run(lg,typename Operation::ArgsType( |
||
539 |
q.derived(),v.derived(),J_in.derived(), |
||
540 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op)); |
||
541 |
} |
||
542 |
|||
543 |
template <class M1_t, class M2_t, class M3_t, class M4_t, bool dDifferenceOnTheLeft, ArgumentPosition arg> |
||
544 |
struct LieGroupDDifferenceProductVisitor |
||
545 |
: visitor::LieGroupVisitorBase< LieGroupDDifferenceProductVisitor<M1_t, M2_t, M3_t, M4_t, dDifferenceOnTheLeft, arg> > |
||
546 |
{ |
||
547 |
typedef boost::fusion::vector<const M1_t &, |
||
548 |
const M2_t &, |
||
549 |
const M3_t &, |
||
550 |
M4_t &, |
||
551 |
const AssignmentOperatorType> ArgsType; |
||
552 |
|||
553 |
LIE_GROUP_VISITOR(LieGroupDDifferenceProductVisitor); |
||
554 |
|||
555 |
template<typename LieGroupDerived> |
||
556 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
||
557 |
const M1_t& q0, |
||
558 |
const M2_t& q1, |
||
559 |
const M3_t& J_in, |
||
560 |
M4_t& J_out, |
||
561 |
const AssignmentOperatorType op) |
||
562 |
{ |
||
563 |
if(dDifferenceOnTheLeft) lg.template dDifference<arg>(q0, q1, SELF, J_in, J_out, op); |
||
564 |
else lg.template dDifference<arg>(q0, q1, J_in, SELF, J_out, op); |
||
565 |
} |
||
566 |
}; |
||
567 |
|||
568 |
template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t> |
||
569 |
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
570 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
571 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
572 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
573 |
int self, |
||
574 |
const Eigen::MatrixBase<JacobianOut_t> & J_out, |
||
575 |
const AssignmentOperatorType op) |
||
576 |
{ |
||
577 |
PINOCCHIO_UNUSED_VARIABLE(self); |
||
578 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
||
579 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
||
580 |
|||
581 |
typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, false, arg> Operation; |
||
582 |
Operation::run(lg,typename Operation::ArgsType( |
||
583 |
q0.derived(),q1.derived(),J_in.derived(), |
||
584 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op)); |
||
585 |
} |
||
586 |
|||
587 |
template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t> |
||
588 |
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
589 |
const Eigen::MatrixBase<ConfigL_t> & q0, |
||
590 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
591 |
int self, |
||
592 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
593 |
const Eigen::MatrixBase<JacobianOut_t> & J_out, |
||
594 |
const AssignmentOperatorType op) |
||
595 |
{ |
||
596 |
PINOCCHIO_UNUSED_VARIABLE(self); |
||
597 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg)); |
||
598 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg)); |
||
599 |
|||
600 |
typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, true, arg> Operation; |
||
601 |
Operation::run(lg,typename Operation::ArgsType( |
||
602 |
q0.derived(),q1.derived(),J_in.derived(), |
||
603 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op)); |
||
604 |
} |
||
605 |
|||
606 |
template <class M1_t, class M2_t, class M3_t, class M4_t> |
||
607 |
struct LieGroupDIntegrateTransportVisitor |
||
608 |
: visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, M4_t> > |
||
609 |
{ |
||
610 |
typedef boost::fusion::vector<const Eigen::MatrixBase<M1_t> &, |
||
611 |
const Eigen::MatrixBase<M2_t> &, |
||
612 |
const Eigen::MatrixBase<M3_t> &, |
||
613 |
const Eigen::MatrixBase<M4_t> &, |
||
614 |
const ArgumentPosition> ArgsType; |
||
615 |
|||
616 |
LIE_GROUP_VISITOR(LieGroupDIntegrateTransportVisitor); |
||
617 |
|||
618 |
template<typename LieGroupDerived> |
||
619 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
||
620 |
const Eigen::MatrixBase<M1_t>& q, |
||
621 |
const Eigen::MatrixBase<M2_t>& v, |
||
622 |
const Eigen::MatrixBase<M3_t>& J_in, |
||
623 |
const Eigen::MatrixBase<M4_t>& J_out, |
||
624 |
const ArgumentPosition arg) |
||
625 |
{ |
||
626 |
lg.dIntegrateTransport(q, v, J_in, J_out, arg); |
||
627 |
} |
||
628 |
}; |
||
629 |
|||
630 |
template <class M1_t, class M2_t, class M3_t> |
||
631 |
struct LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, void> |
||
632 |
: visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, void> > |
||
633 |
{ |
||
634 |
typedef boost::fusion::vector<const Eigen::MatrixBase<M1_t> &, |
||
635 |
const Eigen::MatrixBase<M2_t> &, |
||
636 |
const Eigen::MatrixBase<M3_t> &, |
||
637 |
const ArgumentPosition> ArgsType; |
||
638 |
|||
639 |
LIE_GROUP_VISITOR(LieGroupDIntegrateTransportVisitor); |
||
640 |
template<typename LieGroupDerived> |
||
641 |
static void algo(const LieGroupBase<LieGroupDerived> & lg, |
||
642 |
const Eigen::MatrixBase<M1_t>& q, |
||
643 |
const Eigen::MatrixBase<M2_t>& v, |
||
644 |
const Eigen::MatrixBase<M3_t>& J, |
||
645 |
const ArgumentPosition arg) |
||
646 |
{ |
||
647 |
lg.dIntegrateTransport(q, v, J, arg); |
||
648 |
} |
||
649 |
}; |
||
650 |
|||
651 |
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
652 |
void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
653 |
const Eigen::MatrixBase<Config_t > & q, |
||
654 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
655 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
656 |
const Eigen::MatrixBase<JacobianOut_t> & J_out, |
||
657 |
const ArgumentPosition arg) |
||
658 |
{ |
||
659 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg)); |
||
660 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
||
661 |
assert(J_in .rows() == nv(lg)); |
||
662 |
assert(J_out.rows() == nv(lg)); |
||
663 |
|||
664 |
typedef LieGroupDIntegrateTransportVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t> Operation; |
||
665 |
Operation::run(lg,typename Operation::ArgsType(q,v,J_in,J_out,arg)); |
||
666 |
} |
||
667 |
|||
668 |
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t> |
||
669 |
void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg, |
||
670 |
const Eigen::MatrixBase<Config_t > & q, |
||
671 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
672 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
673 |
const ArgumentPosition arg) |
||
674 |
{ |
||
675 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg)); |
||
676 |
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg)); |
||
677 |
assert(J.rows() == nv(lg)); |
||
678 |
|||
679 |
typedef LieGroupDIntegrateTransportVisitor<Config_t, Tangent_t, JacobianOut_t, void> Operation; |
||
680 |
Operation::run(lg,typename Operation::ArgsType(q,v,J,arg)); |
||
681 |
} |
||
682 |
|||
683 |
} |
||
684 |
|||
685 |
#undef PINOCCHIO_LG_CHECK_VECTOR_SIZE |
||
686 |
|||
687 |
#endif // ifndef __pinocchio_lie_group_variant_visitor_hxx__ |
||
688 |
| Generated by: GCOVR (Version 4.2) |