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