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