| Directory: | ./ |
|---|---|
| File: | unittest/rnea.cpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 231 | 231 | 100.0% |
| Branches: | 717 | 1424 | 50.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | /* | ||
| 6 | * Unittest of the RNE algorithm. The code simply test that the algorithm does | ||
| 7 | * not cause any serious errors. The numerical values are not cross validated | ||
| 8 | * in any way. | ||
| 9 | * | ||
| 10 | */ | ||
| 11 | |||
| 12 | #include "pinocchio/multibody/model.hpp" | ||
| 13 | #include "pinocchio/multibody/data.hpp" | ||
| 14 | #include "pinocchio/algorithm/model.hpp" | ||
| 15 | #include "pinocchio/spatial/fwd.hpp" | ||
| 16 | #include "pinocchio/algorithm/rnea.hpp" | ||
| 17 | #include "pinocchio/algorithm/jacobian.hpp" | ||
| 18 | #include "pinocchio/algorithm/center-of-mass.hpp" | ||
| 19 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| 20 | #include "pinocchio/algorithm/crba.hpp" | ||
| 21 | #include "pinocchio/algorithm/centroidal.hpp" | ||
| 22 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 23 | #include "pinocchio/utils/timer.hpp" | ||
| 24 | |||
| 25 | #include <iostream> | ||
| 26 | |||
| 27 | // #define __SSE3__ | ||
| 28 | #include <fenv.h> | ||
| 29 | |||
| 30 | #ifdef __SSE3__ | ||
| 31 | #include <pmmintrin.h> | ||
| 32 | #endif | ||
| 33 | |||
| 34 | #include <boost/test/unit_test.hpp> | ||
| 35 | #include <boost/utility/binary.hpp> | ||
| 36 | |||
| 37 | #include "utils/model-generator.hpp" | ||
| 38 | |||
| 39 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
| 40 | |||
| 41 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_rnea) |
| 42 | { | ||
| 43 | #ifdef __SSE3__ | ||
| 44 | _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON); | ||
| 45 | #endif | ||
| 46 | using namespace Eigen; | ||
| 47 | using namespace pinocchio; | ||
| 48 | |||
| 49 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 50 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 51 | |||
| 52 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 53 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 54 | |||
| 55 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Data data(model); |
| 56 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | data.v[0] = Motion::Zero(); |
| 57 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | data.a[0] = -model.gravity; |
| 58 | |||
| 59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 60 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 61 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd a = VectorXd::Random(model.nv); |
| 62 | |||
| 63 | #ifdef NDEBUG | ||
| 64 | const size_t NBT = 10000; | ||
| 65 | #else | ||
| 66 | 2 | const size_t NBT = 1; | |
| 67 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::cout << "(the time score in debug mode is not relevant) "; |
| 68 | #endif | ||
| 69 | |||
| 70 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PinocchioTicToc timer(PinocchioTicToc::US); |
| 71 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | timer.tic(); |
| 72 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
4 | SMOOTH(NBT) |
| 73 | { | ||
| 74 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | rnea(model, data, q, v, a); |
| 75 | } | ||
| 76 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | timer.toc(std::cout, NBT); |
| 77 | 2 | } | |
| 78 | |||
| 79 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_nle_vs_rnea) |
| 80 | { | ||
| 81 | using namespace Eigen; | ||
| 82 | using namespace pinocchio; | ||
| 83 | |||
| 84 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 86 | |||
| 87 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 88 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 89 | |||
| 90 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Data data_nle(model); |
| 91 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Data data_rnea(model); |
| 92 | |||
| 93 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 94 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 95 | |||
| 96 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau_nle(VectorXd::Zero(model.nv)); |
| 97 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau_rnea(VectorXd::Zero(model.nv)); |
| 98 | |||
| 99 | // ------- | ||
| 100 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | q.tail(model.nq - 7).setZero(); |
| 101 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | v.setZero(); |
| 102 | |||
| 103 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | tau_nle = nonLinearEffects(model, data_nle, q, v); |
| 104 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv)); |
| 105 | |||
| 106 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12)); |
| 107 | |||
| 108 | // ------- | ||
| 109 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | q.tail(model.nq - 7).setZero(); |
| 110 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | v.setOnes(); |
| 111 | |||
| 112 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | tau_nle = nonLinearEffects(model, data_nle, q, v); |
| 113 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv)); |
| 114 | |||
| 115 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12)); |
| 116 | |||
| 117 | // ------- | ||
| 118 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | q.tail(model.nq - 7).setOnes(); |
| 119 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | v.setOnes(); |
| 120 | |||
| 121 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | tau_nle = nonLinearEffects(model, data_nle, q, v); |
| 122 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv)); |
| 123 | |||
| 124 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12)); |
| 125 | |||
| 126 | // ------- | ||
| 127 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q = randomConfiguration(model); |
| 128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | v.setRandom(); |
| 129 | |||
| 130 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | tau_nle = nonLinearEffects(model, data_nle, q, v); |
| 131 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv)); |
| 132 | |||
| 133 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12)); |
| 134 | 2 | } | |
| 135 | |||
| 136 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_rnea_with_fext) |
| 137 | { | ||
| 138 | using namespace Eigen; | ||
| 139 | using namespace pinocchio; | ||
| 140 | |||
| 141 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 142 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 143 | |||
| 144 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 145 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 146 | |||
| 147 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_rnea_fext(model); |
| 148 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_rnea(model); |
| 149 | |||
| 150 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 151 | |||
| 152 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v(VectorXd::Random(model.nv)); |
| 153 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd a(VectorXd::Random(model.nv)); |
| 154 | |||
| 155 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
2 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero()); |
| 156 | |||
| 157 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | JointIndex rf = model.getJointId("rleg6_joint"); |
| 158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Force Frf = Force::Random(); |
| 159 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | fext[rf] = Frf; |
| 160 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | JointIndex lf = model.getJointId("lleg6_joint"); |
| 161 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Force Flf = Force::Random(); |
| 162 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | fext[lf] = Flf; |
| 163 | |||
| 164 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | rnea(model, data_rnea, q, v, a); |
| 165 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_ref(data_rnea.tau); |
| 166 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x Jrf(Data::Matrix6x::Zero(6, model.nv)); |
| 167 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeJointJacobian(model, data_rnea, q, rf, Jrf); |
| 168 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | tau_ref -= Jrf.transpose() * Frf.toVector(); |
| 169 | |||
| 170 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x Jlf(Data::Matrix6x::Zero(6, model.nv)); |
| 171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeJointJacobian(model, data_rnea, q, lf, Jlf); |
| 172 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | tau_ref -= Jlf.transpose() * Flf.toVector(); |
| 173 | |||
| 174 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | rnea(model, data_rnea_fext, q, v, a, fext); |
| 175 | |||
| 176 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau)); |
| 177 | 2 | } | |
| 178 | |||
| 179 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_rnea_with_armature) |
| 180 | { | ||
| 181 | using namespace Eigen; | ||
| 182 | using namespace pinocchio; | ||
| 183 | |||
| 184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 186 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); |
| 187 | |||
| 188 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 189 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 190 | |||
| 191 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 192 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_ref(model); |
| 193 | |||
| 194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 195 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v(VectorXd::Random(model.nv)); |
| 196 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd a(VectorXd::Random(model.nv)); |
| 197 | |||
| 198 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_ref, q, Convention::WORLD); |
| 199 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<StrictlyLower>() = |
| 200 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data_ref.M.transpose().triangularView<StrictlyLower>(); |
| 201 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const VectorXd nle = nonLinearEffects(model, data_ref, q, v); |
| 202 | |||
| 203 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const VectorXd tau_ref = data_ref.M * a + nle; |
| 204 | |||
| 205 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | rnea(model, data, q, v, a); |
| 206 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(tau_ref.isApprox(data.tau)); |
| 207 | 2 | } | |
| 208 | |||
| 209 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_compute_gravity) |
| 210 | { | ||
| 211 | using namespace Eigen; | ||
| 212 | using namespace pinocchio; | ||
| 213 | |||
| 214 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 215 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 216 | |||
| 217 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 218 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 219 | |||
| 220 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_rnea(model); |
| 221 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 222 | |||
| 223 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 224 | |||
| 225 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | rnea(model, data_rnea, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv)); |
| 226 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeGeneralizedGravity(model, data, q); |
| 227 | |||
| 228 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_rnea.tau.isApprox(data.g)); |
| 229 | |||
| 230 | // Compare with Jcom | ||
| 231 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_rnea, q, Convention::WORLD); |
| 232 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix3x Jcom = getJacobianComFromCrba(model, data_rnea); |
| 233 | |||
| 234 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
2 | VectorXd g_ref(-data_rnea.mass[0] * Jcom.transpose() * Model::gravity981); |
| 235 | |||
| 236 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(g_ref.isApprox(data.g)); |
| 237 | 2 | } | |
| 238 | |||
| 239 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_compute_static_torque) |
| 240 | { | ||
| 241 | using namespace Eigen; | ||
| 242 | using namespace pinocchio; | ||
| 243 | |||
| 244 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 245 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 246 | |||
| 247 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 248 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 249 | |||
| 250 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_rnea(model); |
| 251 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 252 | |||
| 253 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 254 | |||
| 255 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; | ||
| 256 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | ForceVector fext((size_t)model.njoints); |
| 257 |
2/2✓ Branch 4 taken 28 times.
✓ Branch 5 taken 1 times.
|
58 | for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
| 258 |
1/2✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
|
56 | (*it).setRandom(); |
| 259 | |||
| 260 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | rnea(model, data_rnea, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), fext); |
| 261 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeStaticTorque(model, data, q, fext); |
| 262 | |||
| 263 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_rnea.tau.isApprox(data.tau)); |
| 264 | |||
| 265 | // Compare with Jcom + Jacobian of joint | ||
| 266 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_rnea, q, Convention::WORLD); |
| 267 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix3x Jcom = getJacobianComFromCrba(model, data_rnea); |
| 268 | |||
| 269 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
2 | VectorXd static_torque_ref = -data_rnea.mass[0] * Jcom.transpose() * Model::gravity981; |
| 270 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeJointJacobians(model, data_rnea, q); |
| 271 | |||
| 272 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_local(6, model.nv); |
| 273 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id) |
| 274 | { | ||
| 275 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | J_local.setZero(); |
| 276 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local); |
| 277 |
4/8✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
|
54 | static_torque_ref -= J_local.transpose() * fext[joint_id].toVector(); |
| 278 | } | ||
| 279 | |||
| 280 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(static_torque_ref.isApprox(data.tau)); |
| 281 | 2 | } | |
| 282 | |||
| 283 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_compute_coriolis) |
| 284 | { | ||
| 285 | using namespace Eigen; | ||
| 286 | using namespace pinocchio; | ||
| 287 | |||
| 288 | 2 | const double prec = Eigen::NumTraits<double>::dummy_precision(); | |
| 289 | |||
| 290 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 291 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 292 | |||
| 293 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 294 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 295 | |||
| 296 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_ref(model); |
| 297 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 298 | |||
| 299 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 300 | |||
| 301 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v(VectorXd::Random(model.nv)); |
| 302 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeCoriolisMatrix(model, data, q, Eigen::VectorXd::Zero(model.nv)); |
| 303 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data.C.isZero(prec)); |
| 304 | |||
| 305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model.gravity.setZero(); |
| 306 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | rnea(model, data_ref, q, v, VectorXd::Zero(model.nv)); |
| 307 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeJointJacobiansTimeVariation(model, data_ref, q, v); |
| 308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeCoriolisMatrix(model, data, q, v); |
| 309 | |||
| 310 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
| 311 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.J.isApprox(data_ref.J)); |
| 312 | |||
| 313 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = data.C * v; |
| 314 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(tau.isApprox(data_ref.tau, prec)); |
| 315 | |||
| 316 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dccrba(model, data_ref, q, v); |
| 317 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_ref, q, Convention::WORLD); |
| 318 | |||
| 319 | 2 | const Data::Vector3 & com = data_ref.com[0]; | |
| 320 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | Motion vcom(data_ref.vcom[0], Data::Vector3::Zero()); |
| 321 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | SE3 cM1(data.oMi[1]); |
| 322 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | cM1.translation() -= com; |
| 323 | |||
| 324 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
|
2 | BOOST_CHECK((cM1.toDualActionMatrix() * data_ref.M.topRows<6>()).isApprox(data_ref.Ag, prec)); |
| 325 | |||
| 326 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Force dh_ref = cM1.act(Force(data_ref.tau.head<6>())); |
| 327 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Force dh(data_ref.dAg * v); |
| 328 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dh.isApprox(dh_ref, prec)); |
| 329 | |||
| 330 | { | ||
| 331 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data_ref(model), data_ref_plus(model); |
| 332 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Eigen::MatrixXd dM(data.C + data.C.transpose()); |
| 333 | |||
| 334 | 2 | const double alpha = 1e-8; | |
| 335 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd q_plus(model.nq); |
| 336 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | q_plus = integrate(model, q, alpha * v); |
| 337 | |||
| 338 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_ref, q, Convention::WORLD); |
| 339 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 340 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 341 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_ref_plus, q_plus, Convention::WORLD); |
| 342 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = |
| 343 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 344 | |||
| 345 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M) / alpha; |
| 346 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dM.isApprox(dM_ref, sqrt(alpha))); |
| 347 | 2 | } | |
| 348 | 2 | } | |
| 349 | |||
| 350 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_rnea_mimic) |
| 351 | { | ||
| 352 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
14 | for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++) |
| 353 | { | ||
| 354 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const pinocchio::MimicTestCases mimic_test_case(i); |
| 355 | 12 | const pinocchio::Model & model_mimic = mimic_test_case.model_mimic; | |
| 356 | 12 | const pinocchio::Model & model_full = mimic_test_case.model_full; | |
| 357 | 12 | const Eigen::MatrixXd & G = mimic_test_case.G; | |
| 358 | |||
| 359 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_nle_mimic(model_mimic); |
| 360 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_rnea_mimic(model_mimic); |
| 361 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_full(model_full); |
| 362 | |||
| 363 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic); |
| 364 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd v = Eigen::VectorXd::Random(model_mimic.nv); |
| 365 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd a = Eigen::VectorXd::Random(model_mimic.nv); |
| 366 | |||
| 367 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Eigen::VectorXd q_full(model_full.nq); |
| 368 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd v_full = G * v; |
| 369 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd a_full = G * v; |
| 370 | |||
| 371 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | mimic_test_case.toFull(q, q_full); |
| 372 | |||
| 373 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::crba(model_full, data_full, q_full, pinocchio::Convention::WORLD); |
| 374 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | data_full.M.triangularView<Eigen::StrictlyLower>() = |
| 375 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
24 | data_full.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 376 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | const Eigen::VectorXd nle = pinocchio::nonLinearEffects(model_full, data_full, q_full, v_full); |
| 377 | |||
| 378 | // // Use equation of motion to compute tau from a_reduced | ||
| 379 |
8/16✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
|
12 | Eigen::VectorXd tau_ref = G.transpose() * data_full.M * G * a + (G.transpose() * nle); |
| 380 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::rnea(model_mimic, data_rnea_mimic, q, v, a); |
| 381 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 6 times.
|
12 | BOOST_CHECK(tau_ref.isApprox(data_rnea_mimic.tau)); |
| 382 | |||
| 383 | // NLE | ||
| 384 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_nle(model_mimic); |
| 385 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_ref_nle(model_mimic); |
| 386 | Eigen::VectorXd tau_ref_nle = | ||
| 387 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | pinocchio::rnea(model_mimic, data_ref_nle, q, v, Eigen::VectorXd::Zero(model_mimic.nv)); |
| 388 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd tau_nle = pinocchio::nonLinearEffects(model_mimic, data_nle, q, v); |
| 389 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 6 times.
|
12 | BOOST_CHECK(tau_nle.isApprox(tau_ref_nle)); |
| 390 | |||
| 391 | // Generalized Gravity | ||
| 392 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_ref_gg(model_mimic); |
| 393 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_gg(model_mimic); |
| 394 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Eigen::VectorXd tau_ref_gg = pinocchio::rnea( |
| 395 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | model_mimic, data_ref_gg, q, Eigen::VectorXd::Zero(model_mimic.nv), |
| 396 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
24 | Eigen::VectorXd::Zero(model_mimic.nv)); |
| 397 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd tau_gg = pinocchio::computeGeneralizedGravity(model_mimic, data_gg, q); |
| 398 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 6 times.
|
12 | BOOST_CHECK(tau_gg.isApprox(tau_ref_gg)); |
| 399 | |||
| 400 | // Static Torque | ||
| 401 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::Force) ForceVector; | ||
| 402 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
12 | ForceVector fext((size_t)model_mimic.njoints); |
| 403 |
2/2✓ Branch 4 taken 168 times.
✓ Branch 5 taken 6 times.
|
348 | for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
| 404 |
1/2✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
|
336 | (*it).setRandom(); |
| 405 | |||
| 406 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_ref_st(model_mimic); |
| 407 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::Data data_st(model_mimic); |
| 408 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | pinocchio::rnea( |
| 409 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | model_mimic, data_ref_st, q, Eigen::VectorXd::Zero(model_mimic.nv), |
| 410 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Eigen::VectorXd::Zero(model_mimic.nv), fext); |
| 411 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Eigen::VectorXd tau_st = pinocchio::computeStaticTorque(model_mimic, data_gg, q, fext); |
| 412 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 6 times.
|
12 | BOOST_CHECK(tau_st.isApprox(data_ref_st.tau)); |
| 413 | 12 | } | |
| 414 | 2 | } | |
| 415 | |||
| 416 | BOOST_AUTO_TEST_SUITE_END() | ||
| 417 |