| Directory: | ./ |
|---|---|
| File: | unittest/data.cpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 107 | 107 | 100.0% |
| Branches: | 484 | 942 | 51.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019-2020 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #include "pinocchio/multibody/data.hpp" | ||
| 6 | #include "pinocchio/multibody/model.hpp" | ||
| 7 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 8 | |||
| 9 | #include "pinocchio/algorithm/check.hpp" | ||
| 10 | |||
| 11 | #include <boost/test/unit_test.hpp> | ||
| 12 | #include <boost/utility/binary.hpp> | ||
| 13 | |||
| 14 | #include "utils/model-generator.hpp" | ||
| 15 | |||
| 16 | using namespace pinocchio; | ||
| 17 | |||
| 18 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
| 19 | |||
| 20 |
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_empty_model) |
| 21 | { | ||
| 22 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model empty_model; |
| 23 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data empty_data(empty_model); |
| 24 | |||
| 25 |
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(empty_model.check(empty_data)); |
| 26 | 2 | } | |
| 27 | |||
| 28 |
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_data_start_idx_v_fromRow) |
| 29 | { | ||
| 30 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 31 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model); |
| 32 | |||
| 33 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 34 | |||
| 35 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (Model::JointIndex joint_id = 1; joint_id < (Model::JointIndex)model.njoints; ++joint_id) |
| 36 | { | ||
| 37 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
54 | const int nv_joint = model.joints[joint_id].nv(); |
| 38 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
54 | const int idx_joint = model.joints[joint_id].idx_v(); |
| 39 | |||
| 40 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 27 times.
|
118 | for (int k = 0; k < nv_joint; ++k) |
| 41 | { | ||
| 42 |
6/12✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 32 times.
|
64 | BOOST_CHECK(data.start_idx_v_fromRow[(size_t)(idx_joint + k)] == idx_joint); |
| 43 |
6/12✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 32 times.
|
64 | BOOST_CHECK(data.end_idx_v_fromRow[(size_t)(idx_joint + k)] == idx_joint + nv_joint - 1); |
| 44 | } | ||
| 45 | } | ||
| 46 | 2 | } | |
| 47 | |||
| 48 |
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_data_supports_fromRow) |
| 49 | { | ||
| 50 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 51 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model); |
| 52 | |||
| 53 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 54 | |||
| 55 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (size_t k = 0; k < (size_t)model.nv; ++k) |
| 56 | { | ||
| 57 | 64 | const std::vector<int> & support = data.supports_fromRow[k]; | |
| 58 | 64 | const int parent_id = data.parents_fromRow[k]; | |
| 59 | |||
| 60 |
2/2✓ Branch 0 taken 31 times.
✓ Branch 1 taken 1 times.
|
64 | if (parent_id >= 0) |
| 61 | { | ||
| 62 | 62 | const std::vector<int> & support_parent = data.supports_fromRow[(size_t)parent_id]; | |
| 63 |
6/12✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 31 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 31 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 31 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 31 times.
|
62 | BOOST_CHECK(support.size() == support_parent.size() + 1); |
| 64 |
2/2✓ Branch 1 taken 256 times.
✓ Branch 2 taken 31 times.
|
574 | for (size_t j = 0; j < support_parent.size(); ++j) |
| 65 | { | ||
| 66 |
6/12✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 256 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 256 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 256 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 256 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 256 times.
|
512 | BOOST_CHECK(support[j] == support_parent[j]); |
| 67 | } | ||
| 68 | } | ||
| 69 | |||
| 70 |
6/12✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 32 times.
|
64 | BOOST_CHECK(support.back() == (int)k); |
| 71 | } | ||
| 72 | 2 | } | |
| 73 | |||
| 74 |
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_data_mimic_idx_vExtended_to_idx_v_fromRow) |
| 75 | { | ||
| 76 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
14 | for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++) |
| 77 | { | ||
| 78 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const pinocchio::MimicTestCases mimic_test_case(i); |
| 79 | 12 | const pinocchio::Model & model_mimic = mimic_test_case.model_mimic; | |
| 80 | 12 | const pinocchio::Model & model_full = mimic_test_case.model_full; | |
| 81 | |||
| 82 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Data data_mimic(model_mimic); |
| 83 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Data data_full(model_full); |
| 84 | |||
| 85 |
2/2✓ Branch 0 taken 162 times.
✓ Branch 1 taken 6 times.
|
336 | for (size_t joint_id = 1; joint_id < model_mimic.njoints; joint_id++) |
| 86 | { | ||
| 87 |
1/2✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
|
324 | const int idx_vj = model_mimic.joints[joint_id].idx_v(); |
| 88 |
1/2✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
|
324 | const int idx_vExtended_j = model_mimic.joints[joint_id].idx_vExtended(); |
| 89 |
1/2✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
|
324 | const int nvExtended_j = model_mimic.joints[joint_id].nvExtended(); |
| 90 |
2/2✓ Branch 0 taken 192 times.
✓ Branch 1 taken 162 times.
|
708 | for (int v = 0; v < nvExtended_j; v++) |
| 91 | { | ||
| 92 |
6/12✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 192 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 192 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 192 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 192 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 192 times.
|
384 | BOOST_CHECK( |
| 93 | data_mimic.idx_vExtended_to_idx_v_fromRow[size_t(idx_vExtended_j + v)] == idx_vj + v); | ||
| 94 |
6/12✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 192 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 192 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 192 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 192 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 192 times.
|
384 | BOOST_CHECK( |
| 95 | data_full.idx_vExtended_to_idx_v_fromRow[size_t(idx_vExtended_j + v)] | ||
| 96 | == idx_vExtended_j + v); | ||
| 97 | } | ||
| 98 | } | ||
| 99 | 12 | } | |
| 100 | 2 | } | |
| 101 | |||
| 102 |
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_data_mimic_mimic_parents_fromRow) |
| 103 | { | ||
| 104 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
14 | for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++) |
| 105 | { | ||
| 106 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const pinocchio::MimicTestCases mimic_test_case(i); |
| 107 | 12 | const pinocchio::Model & model_mimic = mimic_test_case.model_mimic; | |
| 108 | |||
| 109 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Data data_mimic(model_mimic); |
| 110 | |||
| 111 |
2/2✓ Branch 0 taken 162 times.
✓ Branch 1 taken 6 times.
|
336 | for (size_t joint_id = 1; joint_id < model_mimic.njoints; joint_id++) |
| 112 | { | ||
| 113 |
1/2✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
|
324 | const int idx_vExtended_j = model_mimic.joints[joint_id].idx_vExtended(); |
| 114 |
1/2✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
|
324 | const int nvExtended_j = model_mimic.joints[joint_id].nvExtended(); |
| 115 | |||
| 116 | // If the parent from row is not the universe, it should be either mimic or non mimic - not | ||
| 117 | // both | ||
| 118 | 324 | const bool parent_is_universe = (data_mimic.parents_fromRow[idx_vExtended_j] == -1); | |
| 119 | const bool parent_is_mimic = | ||
| 120 | 324 | (data_mimic.mimic_parents_fromRow[idx_vExtended_j] | |
| 121 | 324 | == data_mimic.parents_fromRow[idx_vExtended_j]); | |
| 122 | const bool parent_is_not_mimic = | ||
| 123 | 324 | (data_mimic.non_mimic_parents_fromRow[idx_vExtended_j] | |
| 124 | 324 | == data_mimic.parents_fromRow[idx_vExtended_j]); | |
| 125 |
9/16✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 162 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 162 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 156 times.
✓ Branch 14 taken 6 times.
✓ Branch 15 taken 156 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 162 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 162 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 162 times.
|
324 | BOOST_CHECK(parent_is_universe || (parent_is_mimic != parent_is_not_mimic)); |
| 126 | |||
| 127 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 162 times.
|
384 | for (int v = 1; v < nvExtended_j; v++) |
| 128 | { | ||
| 129 |
6/12✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 30 times.
|
60 | BOOST_CHECK( |
| 130 | data_mimic.mimic_parents_fromRow[size_t(idx_vExtended_j + v)] == idx_vExtended_j + v - 1); | ||
| 131 | } | ||
| 132 | } | ||
| 133 | 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_copy_and_equal_op) |
| 137 | { | ||
| 138 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 139 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model); |
| 140 | |||
| 141 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 142 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_copy = data; |
| 143 | |||
| 144 |
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 == data); |
| 145 |
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 == data_copy); |
| 146 | |||
| 147 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | data_copy.oMi[0].setRandom(); |
| 148 |
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 != data_copy); |
| 149 | 2 | } | |
| 150 | |||
| 151 |
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_container_aligned_vector) |
| 152 | { | ||
| 153 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 154 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model); |
| 155 | |||
| 156 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 157 | |||
| 158 | 2 | container::aligned_vector<Data::Force> & f = data.f; | |
| 159 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | data.f[0].setRandom(); |
| 160 | |||
| 161 |
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
|
2 | BOOST_CHECK(data.f[0] == f[0]); |
| 162 | 2 | } | |
| 163 | |||
| 164 |
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_std_vector_of_Data) |
| 165 | { | ||
| 166 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 167 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model); |
| 168 | |||
| 169 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_ALIGNED_STD_VECTOR(Data) datas; |
| 170 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 1 times.
|
42 | for (size_t k = 0; k < 20; ++k) |
| 171 |
2/4✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
40 | datas.push_back(Data(model)); |
| 172 | 2 | } | |
| 173 | |||
| 174 |
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_mimic_subtree) |
| 175 | { | ||
| 176 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::manipulator(model); |
| 178 | // Direct parent/Child | ||
| 179 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
4 | std::vector<pinocchio::JointIndex> mimicked = {model.getJointId("shoulder1_joint")}; |
| 180 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
4 | std::vector<pinocchio::JointIndex> mimicking = {model.getJointId("shoulder2_joint")}; |
| 181 | |||
| 182 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::vector<double> ratio = {2.5}; |
| 183 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::vector<double> offset = {0.75}; |
| 184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model_mimic; |
| 185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildMimicModel(model, mimicked, mimicking, ratio, offset, model_mimic); |
| 186 | |||
| 187 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_mimic(model_mimic); |
| 188 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 189 | |||
| 190 | // No mimic so should not be filled | ||
| 191 |
6/12✓ 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 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(data.mimic_subtree_joint.size() == 0); |
| 192 | |||
| 193 | // it's a linear model with RX and RY so the joint after shoulder2 is not a mimic and is in its | ||
| 194 | // subtree | ||
| 195 |
8/16✓ 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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(data_mimic.mimic_subtree_joint[0] == model_mimic.getJointId("shoulder3_joint")); |
| 196 | |||
| 197 | // Test when mimic is terminal | ||
| 198 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model man_mimic; |
| 199 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::manipulator(man_mimic, true); |
| 200 | |||
| 201 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_man_mimic(man_mimic); |
| 202 |
6/12✓ 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 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(data_man_mimic.mimic_subtree_joint[0] == 0); |
| 203 | 2 | } | |
| 204 | |||
| 205 | BOOST_AUTO_TEST_SUITE_END() | ||
| 206 |