| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_multibody_liegroup_liegroup_algo_hxx__ |
||
6 |
#define __pinocchio_multibody_liegroup_liegroup_algo_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/multibody/joint/joint-composite.hpp" |
||
10 |
|||
11 |
namespace pinocchio |
||
12 |
{ |
||
13 |
|||
14 |
namespace details |
||
15 |
{ |
||
16 |
template<typename Algo> |
||
17 |
struct Dispatch |
||
18 |
{ |
||
19 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsType> |
||
20 |
5 |
static void run(const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
|
21 |
ArgsType args) |
||
22 |
{ |
||
23 |
✓✓ | 17 |
for (size_t i = 0; i < jmodel.joints.size(); ++i) |
24 |
✓✗ | 12 |
Algo::run(jmodel.joints[i], args); |
25 |
5 |
} |
|
26 |
}; |
||
27 |
|||
28 |
#define PINOCCHIO_DETAILS_WRITE_ARGS_0(JM) const JointModelBase<JM> & jmodel |
||
29 |
#define PINOCCHIO_DETAILS_WRITE_ARGS_1(JM) PINOCCHIO_DETAILS_WRITE_ARGS_0(JM), typename boost::fusion::result_of::at_c<ArgsType, 0>::type a0 |
||
30 |
#define PINOCCHIO_DETAILS_WRITE_ARGS_2(JM) PINOCCHIO_DETAILS_WRITE_ARGS_1(JM), typename boost::fusion::result_of::at_c<ArgsType, 1>::type a1 |
||
31 |
#define PINOCCHIO_DETAILS_WRITE_ARGS_3(JM) PINOCCHIO_DETAILS_WRITE_ARGS_2(JM), typename boost::fusion::result_of::at_c<ArgsType, 2>::type a2 |
||
32 |
#define PINOCCHIO_DETAILS_WRITE_ARGS_4(JM) PINOCCHIO_DETAILS_WRITE_ARGS_3(JM), typename boost::fusion::result_of::at_c<ArgsType, 3>::type a3 |
||
33 |
#define PINOCCHIO_DETAILS_WRITE_ARGS_5(JM) PINOCCHIO_DETAILS_WRITE_ARGS_4(JM), typename boost::fusion::result_of::at_c<ArgsType, 4>::type a4 |
||
34 |
|||
35 |
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(Algo) \ |
||
36 |
template <typename Visitor, typename JointCollection> \ |
||
37 |
struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > { \ |
||
38 |
typedef typename Visitor::ArgsType ArgsType; \ |
||
39 |
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModelCompositeTpl<JointCollection>)) \ |
||
40 |
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0)); } \ |
||
41 |
} |
||
42 |
|||
43 |
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_2(Algo) \ |
||
44 |
template <typename Visitor, typename JointCollection> \ |
||
45 |
struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > { \ |
||
46 |
typedef typename Visitor::ArgsType ArgsType; \ |
||
47 |
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_2(JointModelCompositeTpl<JointCollection>)) \ |
||
48 |
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1)); } \ |
||
49 |
} |
||
50 |
|||
51 |
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(Algo) \ |
||
52 |
template <typename Visitor, typename JointCollection> \ |
||
53 |
struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > { \ |
||
54 |
typedef typename Visitor::ArgsType ArgsType; \ |
||
55 |
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_3(JointModelCompositeTpl<JointCollection>)) \ |
||
56 |
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2)); } \ |
||
57 |
} |
||
58 |
|||
59 |
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(Algo) \ |
||
60 |
template <typename Visitor, typename JointCollection> \ |
||
61 |
struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > { \ |
||
62 |
typedef typename Visitor::ArgsType ArgsType; \ |
||
63 |
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_4(JointModelCompositeTpl<JointCollection>)) \ |
||
64 |
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3)); } \ |
||
65 |
} |
||
66 |
|||
67 |
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(Algo) \ |
||
68 |
template <typename Visitor, typename JointCollection> \ |
||
69 |
struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > { \ |
||
70 |
typedef typename Visitor::ArgsType ArgsType; \ |
||
71 |
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_5(JointModelCompositeTpl<JointCollection>)) \ |
||
72 |
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3,a4)); } \ |
||
73 |
} |
||
74 |
|||
75 |
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(Algo, Visitor) \ |
||
76 |
typedef LieGroup_t LieGroupMap; \ |
||
77 |
template<typename JointModel> \ |
||
78 |
static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModel)) \ |
||
79 |
{ AlgoDispatch<JointModel>::run(jmodel, a0); } \ |
||
80 |
template<typename JointModel> \ |
||
81 |
struct AlgoDispatch : Algo<Visitor, JointModel> \ |
||
82 |
{ using Algo<Visitor, JointModel>::run; }; |
||
83 |
|||
84 |
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_2(Algo, Visitor) \ |
||
85 |
typedef LieGroup_t LieGroupMap; \ |
||
86 |
template<typename JointModel> \ |
||
87 |
static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_2(JointModel)) \ |
||
88 |
{ AlgoDispatch<JointModel>::run(jmodel, a0, a1); } \ |
||
89 |
template<typename JointModel> \ |
||
90 |
struct AlgoDispatch : Algo<Visitor, JointModel> \ |
||
91 |
{ using Algo<Visitor, JointModel>::run; }; |
||
92 |
|||
93 |
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(Algo, Visitor) \ |
||
94 |
typedef LieGroup_t LieGroupMap; \ |
||
95 |
template<typename JointModel> \ |
||
96 |
static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_3(JointModel)) \ |
||
97 |
{ AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2); } \ |
||
98 |
template<typename JointModel> \ |
||
99 |
struct AlgoDispatch : Algo<Visitor, JointModel> \ |
||
100 |
{ using Algo<Visitor, JointModel>::run; }; |
||
101 |
|||
102 |
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(Algo, Visitor) \ |
||
103 |
typedef LieGroup_t LieGroupMap; \ |
||
104 |
template<typename JointModel> \ |
||
105 |
static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_4(JointModel)) \ |
||
106 |
{ AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2, a3); } \ |
||
107 |
template<typename JointModel> \ |
||
108 |
struct AlgoDispatch : Algo<Visitor, JointModel> \ |
||
109 |
{ using Algo<Visitor, JointModel>::run; }; |
||
110 |
|||
111 |
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(Algo, Visitor) \ |
||
112 |
typedef LieGroup_t LieGroupMap; \ |
||
113 |
template<typename JointModel> \ |
||
114 |
static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_5(JointModel)) \ |
||
115 |
{ AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2, a3, a4); } \ |
||
116 |
template<typename JointModel> \ |
||
117 |
struct AlgoDispatch : Algo<Visitor, JointModel> \ |
||
118 |
{ using Algo<Visitor, JointModel>::run; }; |
||
119 |
|||
120 |
} // namespace details |
||
121 |
|||
122 |
template<typename Visitor, typename JointModel> struct IntegrateStepAlgo; |
||
123 |
|||
124 |
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename ConfigVectorOut> |
||
125 |
struct IntegrateStep |
||
126 |
: public fusion::JointUnaryVisitorBase< IntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,ConfigVectorOut> > |
||
127 |
{ |
||
128 |
typedef boost::fusion::vector<const ConfigVectorIn &, |
||
129 |
const TangentVectorIn &, |
||
130 |
ConfigVectorOut & |
||
131 |
> ArgsType; |
||
132 |
|||
133 |
99146 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(IntegrateStepAlgo, IntegrateStep) |
|
134 |
}; |
||
135 |
|||
136 |
template<typename Visitor, typename JointModel> |
||
137 |
struct IntegrateStepAlgo |
||
138 |
{ |
||
139 |
template<typename ConfigVectorIn, typename TangentVector, typename ConfigVectorOut> |
||
140 |
99146 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
141 |
const Eigen::MatrixBase<ConfigVectorIn> & q, |
||
142 |
const Eigen::MatrixBase<TangentVector> & v, |
||
143 |
const Eigen::MatrixBase<ConfigVectorOut> & result) |
||
144 |
{ |
||
145 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
146 |
|||
147 |
✓✗ | 99146 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
148 |
✓✗✓✗ ✓✗✓✗ |
99146 |
lgo.integrate(jmodel.jointConfigSelector (q.derived()), |
149 |
jmodel.jointVelocitySelector(v.derived()), |
||
150 |
99146 |
jmodel.jointConfigSelector (PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,result))); |
|
151 |
} |
||
152 |
}; |
||
153 |
|||
154 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IntegrateStepAlgo); |
||
155 |
|||
156 |
template<typename Visitor, typename JointModel> struct dIntegrateStepAlgo; |
||
157 |
|||
158 |
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType> |
||
159 |
struct dIntegrateStep |
||
160 |
: public fusion::JointUnaryVisitorBase< dIntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> > |
||
161 |
{ |
||
162 |
typedef boost::fusion::vector<const ConfigVectorIn &, |
||
163 |
const TangentVectorIn &, |
||
164 |
JacobianMatrixType &, |
||
165 |
const ArgumentPosition &, |
||
166 |
const AssignmentOperatorType& |
||
167 |
> ArgsType; |
||
168 |
|||
169 |
396 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(dIntegrateStepAlgo, dIntegrateStep) |
|
170 |
}; |
||
171 |
|||
172 |
template<typename Visitor, typename JointModel> |
||
173 |
struct dIntegrateStepAlgo |
||
174 |
{ |
||
175 |
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType> |
||
176 |
396 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
177 |
const Eigen::MatrixBase<ConfigVectorIn> & q, |
||
178 |
const Eigen::MatrixBase<TangentVector> & v, |
||
179 |
const Eigen::MatrixBase<JacobianMatrixType> & mat, |
||
180 |
const ArgumentPosition & arg, |
||
181 |
const AssignmentOperatorType& op) |
||
182 |
{ |
||
183 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
184 |
|||
185 |
✓✗ | 396 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
186 |
✓✗✓✗ ✓✗✓✗ |
396 |
lgo.dIntegrate(jmodel.jointConfigSelector (q.derived()), |
187 |
jmodel.jointVelocitySelector(v.derived()), |
||
188 |
396 |
jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)), |
|
189 |
arg, op); |
||
190 |
} |
||
191 |
}; |
||
192 |
|||
193 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateStepAlgo); |
||
194 |
|||
195 |
template<typename Visitor, typename JointModel> struct dIntegrateTransportStepAlgo; |
||
196 |
|||
197 |
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixInType, typename JacobianMatrixOutType> |
||
198 |
struct dIntegrateTransportStep |
||
199 |
: public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixInType,JacobianMatrixOutType> > |
||
200 |
{ |
||
201 |
typedef boost::fusion::vector<const ConfigVectorIn &, |
||
202 |
const TangentVectorIn &, |
||
203 |
const JacobianMatrixInType &, |
||
204 |
JacobianMatrixOutType &, |
||
205 |
const ArgumentPosition & |
||
206 |
> ArgsType; |
||
207 |
|||
208 |
36 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(dIntegrateTransportStepAlgo, dIntegrateTransportStep) |
|
209 |
}; |
||
210 |
|||
211 |
template<typename Visitor, typename JointModel> |
||
212 |
struct dIntegrateTransportStepAlgo |
||
213 |
{ |
||
214 |
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixInType, typename JacobianMatrixOutType> |
||
215 |
36 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
216 |
const Eigen::MatrixBase<ConfigVectorIn> & q, |
||
217 |
const Eigen::MatrixBase<TangentVector> & v, |
||
218 |
const Eigen::MatrixBase<JacobianMatrixInType> & mat_in, |
||
219 |
const Eigen::MatrixBase<JacobianMatrixOutType> & mat_out, |
||
220 |
const ArgumentPosition & arg) |
||
221 |
{ |
||
222 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
223 |
|||
224 |
✓✗ | 36 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
225 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
36 |
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()), |
226 |
jmodel.jointVelocitySelector(v.derived()), |
||
227 |
jmodel.jointRows(mat_in.derived()), |
||
228 |
36 |
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType,mat_out)), |
|
229 |
arg); |
||
230 |
} |
||
231 |
}; |
||
232 |
|||
233 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateTransportStepAlgo); |
||
234 |
|||
235 |
|||
236 |
template<typename Visitor, typename JointModel> struct dIntegrateTransportInPlaceStepAlgo; |
||
237 |
|||
238 |
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType> |
||
239 |
struct dIntegrateTransportInPlaceStep |
||
240 |
: public fusion::JointUnaryVisitorBase< dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> > |
||
241 |
{ |
||
242 |
typedef boost::fusion::vector<const ConfigVectorIn &, |
||
243 |
const TangentVectorIn &, |
||
244 |
JacobianMatrixType &, |
||
245 |
const ArgumentPosition & |
||
246 |
> ArgsType; |
||
247 |
|||
248 |
36 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateTransportInPlaceStepAlgo, |
|
249 |
dIntegrateTransportInPlaceStep) |
||
250 |
}; |
||
251 |
|||
252 |
template<typename Visitor, typename JointModel> |
||
253 |
struct dIntegrateTransportInPlaceStepAlgo |
||
254 |
{ |
||
255 |
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType> |
||
256 |
36 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
257 |
const Eigen::MatrixBase<ConfigVectorIn> & q, |
||
258 |
const Eigen::MatrixBase<TangentVector> & v, |
||
259 |
const Eigen::MatrixBase<JacobianMatrixType> & mat, |
||
260 |
const ArgumentPosition & arg) |
||
261 |
{ |
||
262 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
263 |
|||
264 |
✓✗ | 36 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
265 |
✓✗✓✗ ✓✗✓✗ |
36 |
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()), |
266 |
jmodel.jointVelocitySelector(v.derived()), |
||
267 |
36 |
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)), |
|
268 |
arg); |
||
269 |
} |
||
270 |
}; |
||
271 |
|||
272 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateTransportInPlaceStepAlgo); |
||
273 |
|||
274 |
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo; |
||
275 |
|||
276 |
template<typename LieGroup_t, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix> |
||
277 |
struct dDifferenceStep |
||
278 |
: public fusion::JointUnaryVisitorBase< dDifferenceStep<LieGroup_t,ConfigVector1,ConfigVector2,JacobianMatrix> > |
||
279 |
{ |
||
280 |
typedef boost::fusion::vector<const ConfigVector1 &, |
||
281 |
const ConfigVector2 &, |
||
282 |
JacobianMatrix &, |
||
283 |
const ArgumentPosition & |
||
284 |
> ArgsType; |
||
285 |
|||
286 |
108 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dDifferenceStepAlgo, dDifferenceStep) |
|
287 |
}; |
||
288 |
|||
289 |
template<typename Visitor, typename JointModel> |
||
290 |
struct dDifferenceStepAlgo |
||
291 |
{ |
||
292 |
template<typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix> |
||
293 |
108 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
294 |
const Eigen::MatrixBase<ConfigVector1> & q0, |
||
295 |
const Eigen::MatrixBase<ConfigVector2> & q1, |
||
296 |
const Eigen::MatrixBase<JacobianMatrix> & mat, |
||
297 |
const ArgumentPosition & arg) |
||
298 |
{ |
||
299 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
300 |
|||
301 |
✓✗ | 108 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
302 |
✓✗✓✗ ✓✗✓✗ |
108 |
lgo.dDifference(jmodel.jointConfigSelector(q0.derived()), |
303 |
jmodel.jointConfigSelector(q1.derived()), |
||
304 |
108 |
jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,mat)), |
|
305 |
arg); |
||
306 |
} |
||
307 |
}; |
||
308 |
|||
309 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dDifferenceStepAlgo); |
||
310 |
|||
311 |
template<typename Visitor, typename JointModel> struct InterpolateStepAlgo; |
||
312 |
|||
313 |
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut> |
||
314 |
struct InterpolateStep |
||
315 |
: public fusion::JointUnaryVisitorBase< InterpolateStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar,ConfigVectorOut > > |
||
316 |
{ |
||
317 |
typedef boost::fusion::vector<const ConfigVectorIn1 &, |
||
318 |
const ConfigVectorIn2 &, |
||
319 |
const Scalar &, |
||
320 |
ConfigVectorOut & |
||
321 |
> ArgsType; |
||
322 |
|||
323 |
36 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(InterpolateStepAlgo, InterpolateStep) |
|
324 |
}; |
||
325 |
|||
326 |
template<typename Visitor, typename JointModel> |
||
327 |
struct InterpolateStepAlgo |
||
328 |
{ |
||
329 |
template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut> |
||
330 |
36 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
331 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
332 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
333 |
const Scalar & u, |
||
334 |
const Eigen::MatrixBase<ConfigVectorOut> & result) |
||
335 |
{ |
||
336 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
337 |
|||
338 |
✓✗ | 36 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
339 |
✓✗✓✗ ✓✗✓✗ |
36 |
lgo.interpolate(jmodel.jointConfigSelector(q0.derived()), |
340 |
jmodel.jointConfigSelector(q1.derived()), |
||
341 |
u, |
||
342 |
36 |
jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,result))); |
|
343 |
} |
||
344 |
}; |
||
345 |
|||
346 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStepAlgo); |
||
347 |
|||
348 |
template<typename Visitor, typename JointModel> struct DifferenceStepAlgo; |
||
349 |
|||
350 |
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename TangentVectorOut> |
||
351 |
struct DifferenceStep |
||
352 |
: public fusion::JointUnaryVisitorBase< DifferenceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,TangentVectorOut> > |
||
353 |
{ |
||
354 |
typedef boost::fusion::vector<const ConfigVectorIn1 &, |
||
355 |
const ConfigVectorIn2 &, |
||
356 |
TangentVectorOut & |
||
357 |
> ArgsType; |
||
358 |
|||
359 |
2070 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(DifferenceStepAlgo, DifferenceStep) |
|
360 |
}; |
||
361 |
|||
362 |
template<typename Visitor, typename JointModel> |
||
363 |
struct DifferenceStepAlgo |
||
364 |
{ |
||
365 |
template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename TangentVectorOut> |
||
366 |
2070 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
367 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
368 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
369 |
const Eigen::MatrixBase<TangentVectorOut> & result) |
||
370 |
{ |
||
371 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
372 |
|||
373 |
✓✗ | 2070 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
374 |
✓✗✓✗ ✓✗✓✗ |
2070 |
lgo.difference(jmodel.jointConfigSelector(q0.derived()), |
375 |
jmodel.jointConfigSelector(q1.derived()), |
||
376 |
2070 |
jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut,result))); |
|
377 |
} |
||
378 |
}; |
||
379 |
|||
380 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferenceStepAlgo); |
||
381 |
|||
382 |
template<typename Visitor, typename JointModel> struct SquaredDistanceStepAlgo; |
||
383 |
|||
384 |
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename DistanceVectorOut> |
||
385 |
struct SquaredDistanceStep |
||
386 |
: public fusion::JointUnaryVisitorBase<SquaredDistanceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,DistanceVectorOut> > |
||
387 |
{ |
||
388 |
typedef boost::fusion::vector<const JointIndex &, |
||
389 |
const ConfigVectorIn1 &, |
||
390 |
const ConfigVectorIn2 &, |
||
391 |
DistanceVectorOut & |
||
392 |
> ArgsType; |
||
393 |
|||
394 |
18 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(SquaredDistanceStepAlgo, SquaredDistanceStep) |
|
395 |
}; |
||
396 |
|||
397 |
template<typename Visitor, typename JointModel> |
||
398 |
struct SquaredDistanceStepAlgo |
||
399 |
{ |
||
400 |
template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename DistanceVectorOut> |
||
401 |
18 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
402 |
const JointIndex & i, |
||
403 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
404 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
405 |
const Eigen::MatrixBase<DistanceVectorOut> & distances) |
||
406 |
{ |
||
407 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
408 |
✓✗ | 18 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
409 |
18 |
DistanceVectorOut & distances_ = PINOCCHIO_EIGEN_CONST_CAST(DistanceVectorOut,distances); |
|
410 |
✓✗✓✗ ✓✗✓✗ |
18 |
distances_[(Eigen::DenseIndex)i] += lgo.squaredDistance(jmodel.jointConfigSelector(q0.derived()), |
411 |
jmodel.jointConfigSelector(q1.derived())); |
||
412 |
} |
||
413 |
}; |
||
414 |
|||
415 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(SquaredDistanceStepAlgo); |
||
416 |
|||
417 |
template<typename Visitor, typename JointModel> struct SquaredDistanceSumStepAlgo; |
||
418 |
|||
419 |
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar> |
||
420 |
struct SquaredDistanceSumStep |
||
421 |
: public fusion::JointUnaryVisitorBase<SquaredDistanceSumStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> > |
||
422 |
{ |
||
423 |
typedef boost::fusion::vector<const ConfigVectorIn1 &, |
||
424 |
const ConfigVectorIn2 &, |
||
425 |
Scalar & |
||
426 |
> ArgsType; |
||
427 |
|||
428 |
36 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(SquaredDistanceSumStepAlgo, SquaredDistanceSumStep) |
|
429 |
}; |
||
430 |
|||
431 |
template<typename Visitor, typename JointModel> |
||
432 |
struct SquaredDistanceSumStepAlgo |
||
433 |
{ |
||
434 |
template<typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
435 |
36 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
436 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
437 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
438 |
typename ConfigVectorIn1::Scalar & squaredDistance) |
||
439 |
{ |
||
440 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
441 |
✓✗ | 36 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
442 |
✓✗✓✗ ✓✗ |
36 |
squaredDistance += lgo.squaredDistance(jmodel.jointConfigSelector(q0.derived()), |
443 |
jmodel.jointConfigSelector(q1.derived())); |
||
444 |
} |
||
445 |
}; |
||
446 |
|||
447 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(SquaredDistanceSumStepAlgo); |
||
448 |
|||
449 |
template<typename Visitor, typename JointModel> struct RandomConfigurationStepAlgo; |
||
450 |
|||
451 |
template<typename LieGroup_t,typename ConfigVectorOut, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
452 |
struct RandomConfigurationStep |
||
453 |
: public fusion::JointUnaryVisitorBase< RandomConfigurationStep<LieGroup_t,ConfigVectorOut,ConfigVectorIn1,ConfigVectorIn2> > |
||
454 |
{ |
||
455 |
typedef boost::fusion::vector<ConfigVectorOut &, |
||
456 |
const ConfigVectorIn1 &, |
||
457 |
const ConfigVectorIn2 & |
||
458 |
> ArgsType; |
||
459 |
|||
460 |
4170 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(RandomConfigurationStepAlgo, RandomConfigurationStep) |
|
461 |
}; |
||
462 |
|||
463 |
template<typename Visitor, typename JointModel> |
||
464 |
struct RandomConfigurationStepAlgo |
||
465 |
{ |
||
466 |
template<typename ConfigVectorOut, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
467 |
4164 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
468 |
const Eigen::MatrixBase<ConfigVectorOut> & q, |
||
469 |
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
||
470 |
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits) |
||
471 |
{ |
||
472 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
473 |
|||
474 |
✓✗ | 4164 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
475 |
✓✗✓✗ ✓✗✓✗ |
4164 |
lgo.randomConfiguration(jmodel.jointConfigSelector(lowerLimits.derived()), |
476 |
jmodel.jointConfigSelector(upperLimits.derived()), |
||
477 |
4164 |
jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,q))); |
|
478 |
} |
||
479 |
}; |
||
480 |
|||
481 |
✓✗ | 4 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(RandomConfigurationStepAlgo); |
482 |
|||
483 |
template<typename Visitor, typename JointModel> struct NormalizeStepAlgo; |
||
484 |
|||
485 |
template<typename LieGroup_t, typename ConfigVectorType> |
||
486 |
struct NormalizeStep |
||
487 |
: public fusion::JointUnaryVisitorBase< NormalizeStep<LieGroup_t,ConfigVectorType> > |
||
488 |
{ |
||
489 |
typedef boost::fusion::vector<ConfigVectorType &> ArgsType; |
||
490 |
|||
491 |
306 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(NormalizeStepAlgo, NormalizeStep) |
|
492 |
}; |
||
493 |
|||
494 |
template<typename Visitor, typename JointModel> |
||
495 |
struct NormalizeStepAlgo |
||
496 |
{ |
||
497 |
template<typename ConfigVectorType> |
||
498 |
306 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
499 |
const Eigen::MatrixBase<ConfigVectorType> & qout) |
||
500 |
{ |
||
501 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
502 |
|||
503 |
✓✗ | 306 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
504 |
✓✗✓✗ |
306 |
lgo.normalize(jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout))); |
505 |
} |
||
506 |
}; |
||
507 |
|||
508 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStepAlgo); |
||
509 |
|||
510 |
template<typename Visitor, typename JointModel> struct IsNormalizedStepAlgo; |
||
511 |
|||
512 |
template<typename LieGroup_t, typename ConfigVectorIn, typename Scalar> |
||
513 |
struct IsNormalizedStep |
||
514 |
: public fusion::JointUnaryVisitorBase< IsNormalizedStep<LieGroup_t,ConfigVectorIn,Scalar> > |
||
515 |
{ |
||
516 |
typedef boost::fusion::vector<const ConfigVectorIn &, |
||
517 |
const Scalar&, |
||
518 |
bool &> ArgsType; |
||
519 |
|||
520 |
130 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(IsNormalizedStepAlgo, IsNormalizedStep) |
|
521 |
}; |
||
522 |
|||
523 |
template<typename Visitor, typename JointModel> |
||
524 |
struct IsNormalizedStepAlgo |
||
525 |
{ |
||
526 |
template<typename ConfigVectorIn> |
||
527 |
130 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
528 |
const Eigen::MatrixBase<ConfigVectorIn> & q, |
||
529 |
const typename ConfigVectorIn::Scalar& prec, |
||
530 |
bool& res) |
||
531 |
{ |
||
532 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
533 |
|||
534 |
✓✗ | 130 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
535 |
✓✗✓✗ |
130 |
res &= lgo.isNormalized(jmodel.jointConfigSelector(q.derived()), prec); |
536 |
} |
||
537 |
}; |
||
538 |
|||
539 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IsNormalizedStepAlgo); |
||
540 |
|||
541 |
template<typename Visitor, typename JointModel> struct IsSameConfigurationStepAlgo; |
||
542 |
|||
543 |
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar> |
||
544 |
struct IsSameConfigurationStep |
||
545 |
: public fusion::JointUnaryVisitorBase< IsSameConfigurationStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> > |
||
546 |
{ |
||
547 |
typedef boost::fusion::vector<bool &, |
||
548 |
const ConfigVectorIn1 &, |
||
549 |
const ConfigVectorIn2 &, |
||
550 |
const Scalar &> ArgsType; |
||
551 |
|||
552 |
54 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(IsSameConfigurationStepAlgo, IsSameConfigurationStep) |
|
553 |
}; |
||
554 |
|||
555 |
template<typename Visitor, typename JointModel> |
||
556 |
struct IsSameConfigurationStepAlgo |
||
557 |
{ |
||
558 |
template<typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
559 |
54 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
560 |
bool & isSame, |
||
561 |
const Eigen::MatrixBase<ConfigVectorIn1> & q1, |
||
562 |
const Eigen::MatrixBase<ConfigVectorIn2> & q2, |
||
563 |
const typename ConfigVectorIn1::Scalar & prec) |
||
564 |
{ |
||
565 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
566 |
|||
567 |
✓✗ | 54 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
568 |
✓✗✓✗ ✓✗ |
54 |
isSame &= lgo.isSameConfiguration(jmodel.jointConfigSelector(q1.derived()), |
569 |
jmodel.jointConfigSelector(q2.derived()), |
||
570 |
prec); |
||
571 |
} |
||
572 |
}; |
||
573 |
|||
574 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(IsSameConfigurationStepAlgo); |
||
575 |
|||
576 |
template<typename Visitor, typename JointModel> struct NeutralStepAlgo; |
||
577 |
|||
578 |
template<typename LieGroup_t, typename ConfigVectorType> |
||
579 |
struct NeutralStep |
||
580 |
: public fusion::JointUnaryVisitorBase< NeutralStep<LieGroup_t,ConfigVectorType> > |
||
581 |
{ |
||
582 |
typedef boost::fusion::vector<ConfigVectorType &> ArgsType; |
||
583 |
|||
584 |
1212 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(NeutralStepAlgo, NeutralStep) |
|
585 |
}; |
||
586 |
|||
587 |
template<typename Visitor, typename JointModel> |
||
588 |
struct NeutralStepAlgo |
||
589 |
{ |
||
590 |
template<typename ConfigVectorType> |
||
591 |
1212 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
592 |
const Eigen::MatrixBase<ConfigVectorType> & neutral_elt) |
||
593 |
{ |
||
594 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
595 |
|||
596 |
✓✗ | 1212 |
typename LieGroupMap::template operation<JointModel>::type lgo; |
597 |
✓✗✓✗ ✓✗ |
1212 |
jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,neutral_elt)) = lgo.neutral(); |
598 |
} |
||
599 |
}; |
||
600 |
|||
601 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStepAlgo); |
||
602 |
|||
603 |
template<typename Visitor, typename JointModel> struct IntegrateCoeffWiseJacobianStepAlgo; |
||
604 |
|||
605 |
template<typename LieGroup_t, typename ConfigVectorType, typename JacobianMatrix> |
||
606 |
struct IntegrateCoeffWiseJacobianStep |
||
607 |
: public fusion::JointUnaryVisitorBase< IntegrateCoeffWiseJacobianStep<LieGroup_t,ConfigVectorType,JacobianMatrix> > |
||
608 |
{ |
||
609 |
typedef boost::fusion::vector<const ConfigVectorType &, JacobianMatrix &> ArgsType; |
||
610 |
|||
611 |
54 |
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_2(IntegrateCoeffWiseJacobianStepAlgo, |
|
612 |
IntegrateCoeffWiseJacobianStep) |
||
613 |
}; |
||
614 |
|||
615 |
template<typename Visitor, typename JointModel> |
||
616 |
struct IntegrateCoeffWiseJacobianStepAlgo |
||
617 |
{ |
||
618 |
template<typename ConfigVectorType, typename JacobianMatrix> |
||
619 |
54 |
static void run(const JointModelBase<JointModel> & jmodel, |
|
620 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
621 |
const Eigen::MatrixBase<JacobianMatrix> & jacobian) |
||
622 |
{ |
||
623 |
typedef typename Visitor::LieGroupMap LieGroupMap; |
||
624 |
|||
625 |
typedef typename LieGroupMap::template operation<JointModel>::type LieGroup; |
||
626 |
✓✗ | 54 |
LieGroup lgo; |
627 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
54 |
lgo.integrateCoeffWiseJacobian(jmodel.jointConfigSelector(q.derived()), |
628 |
54 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian).template block<LieGroup::NQ,LieGroup::NV>(jmodel.idx_q(),jmodel.idx_v(),jmodel.nq(),jmodel.nv())); |
|
629 |
} |
||
630 |
}; |
||
631 |
|||
632 |
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_2(IntegrateCoeffWiseJacobianStepAlgo); |
||
633 |
|||
634 |
} |
||
635 |
|||
636 |
#endif // ifndef __pinocchio_multibody_liegroup_liegroup_algo_hxx__ |
| Generated by: GCOVR (Version 4.2) |