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