| 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 |