| Directory: | ./ |
|---|---|
| File: | tests/device.cc |
| Date: | 2025-05-04 12:09:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 82 | 82 | 100.0% |
| Branches: | 350 | 724 | 48.3% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2017, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 3 | // | ||
| 4 | |||
| 5 | // Redistribution and use in source and binary forms, with or without | ||
| 6 | // modification, are permitted provided that the following conditions are | ||
| 7 | // met: | ||
| 8 | // | ||
| 9 | // 1. Redistributions of source code must retain the above copyright | ||
| 10 | // notice, this list of conditions and the following disclaimer. | ||
| 11 | // | ||
| 12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 13 | // notice, this list of conditions and the following disclaimer in the | ||
| 14 | // documentation and/or other materials provided with the distribution. | ||
| 15 | // | ||
| 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 27 | // DAMAGE. | ||
| 28 | |||
| 29 | #define BOOST_TEST_MODULE tframe | ||
| 30 | |||
| 31 | #include <boost/archive/xml_iarchive.hpp> | ||
| 32 | #include <boost/archive/xml_oarchive.hpp> | ||
| 33 | #include <boost/test/unit_test.hpp> | ||
| 34 | #include <hpp/pinocchio/configuration.hh> | ||
| 35 | #include <hpp/pinocchio/device.hh> | ||
| 36 | #include <hpp/pinocchio/fwd.hh> | ||
| 37 | #include <hpp/pinocchio/humanoid-robot.hh> | ||
| 38 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 39 | #include <hpp/pinocchio/joint.hh> | ||
| 40 | #include <hpp/pinocchio/liegroup-space.hh> | ||
| 41 | #include <hpp/pinocchio/serialization.hh> | ||
| 42 | #include <hpp/pinocchio/simple-device.hh> | ||
| 43 | #include <hpp/pinocchio/urdf/util.hh> | ||
| 44 | #include <pinocchio/algorithm/check.hpp> | ||
| 45 | #include <pinocchio/fwd.hpp> | ||
| 46 | #include <pinocchio/multibody/model.hpp> | ||
| 47 | #include <sstream> | ||
| 48 | |||
| 49 | using namespace hpp::pinocchio; | ||
| 50 | |||
| 51 | 3 | void displayAABB(const coal::AABB& aabb) { | |
| 52 |
10/20✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
|
3 | BOOST_TEST_MESSAGE("Bounding box is\n" |
| 53 | << aabb.min_.transpose() << '\n' | ||
| 54 | << aabb.max_.transpose()); | ||
| 55 | 3 | } | |
| 56 | |||
| 57 |
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(computeAABB) { |
| 58 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
4 | DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidSimple); |
| 59 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_REQUIRE(robot); |
| 60 | |||
| 61 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | robot->rootJoint()->lowerBounds(vector3_t::Constant(-0)); |
| 62 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | robot->rootJoint()->upperBounds(vector3_t::Constant(0)); |
| 63 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | coal::AABB aabb0 = robot->computeAABB(); |
| 64 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | displayAABB(aabb0); |
| 65 | |||
| 66 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | robot->rootJoint()->lowerBounds(vector3_t(-1, -1, 0)); |
| 67 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | robot->rootJoint()->upperBounds(vector3_t(1, 1, 0)); |
| 68 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | coal::AABB aabb1 = robot->computeAABB(); |
| 69 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | displayAABB(aabb1); |
| 70 | |||
| 71 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | robot->rootJoint()->lowerBounds(vector3_t(-2, -2, 0)); |
| 72 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | robot->rootJoint()->upperBounds(vector3_t(-1, -1, 0)); |
| 73 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | coal::AABB aabb2 = robot->computeAABB(); |
| 74 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | displayAABB(aabb2); |
| 75 | 2 | } | |
| 76 | /* -------------------------------------------------------------------------- */ | ||
| 77 |
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(unit_test_device) { |
| 78 | 2 | DevicePtr_t robot; | |
| 79 | 2 | LiegroupSpacePtr_t space; | |
| 80 | |||
| 81 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | robot = Device::create("robot"); |
| 82 |
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 15 taken 1 times.
✗ Branch 16 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 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK(pinocchio::checkData(robot->model(), robot->data())); |
| 83 | |||
| 84 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | robot = unittest::makeDevice(unittest::HumanoidSimple); |
| 85 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | space = LiegroupSpace::createCopy(robot->configSpace()); |
| 86 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | space->mergeVectorSpaces(); |
| 87 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(space->name(), "SE(3)*R^26"); |
| 88 | |||
| 89 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | space = LiegroupSpace::createCopy(robot->RnxSOnConfigSpace()); |
| 90 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | space->mergeVectorSpaces(); |
| 91 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(space->name(), "R^3*SO(3)*R^26"); |
| 92 | |||
| 93 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | robot->setDimensionExtraConfigSpace(3); |
| 94 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->numberDof(), 32 + 3); |
| 95 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->configSize(), 33 + 3); |
| 96 | |||
| 97 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | space = LiegroupSpace::createCopy(robot->configSpace()); |
| 98 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | space->mergeVectorSpaces(); |
| 99 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(space->name(), "SE(3)*R^29"); |
| 100 | |||
| 101 |
5/10✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | BOOST_TEST_MESSAGE(*robot); |
| 102 |
6/12✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✓ Branch 19 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
|
8 | robot->removeJoints({"rleg2_joint", "rarm1_joint"}, |
| 103 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | robot->neutralConfiguration()); |
| 104 |
5/10✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | BOOST_TEST_MESSAGE(*robot); |
| 105 | |||
| 106 |
12/36✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 49 taken 1 times.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 1 times.
✗ Branch 65 not taken.
✓ Branch 67 taken 1 times.
✗ Branch 68 not taken.
✗ Branch 72 not taken.
✓ Branch 73 taken 1 times.
|
8 | BOOST_CHECK_THROW(robot->getJointByName("rleg2_joint"), std::runtime_error); |
| 107 |
12/36✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 49 taken 1 times.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 1 times.
✗ Branch 65 not taken.
✓ Branch 67 taken 1 times.
✗ Branch 68 not taken.
✗ Branch 72 not taken.
✓ Branch 73 taken 1 times.
|
8 | BOOST_CHECK_THROW(robot->getJointByName("rarm1_joint"), std::runtime_error); |
| 108 | |||
| 109 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | robot = unittest::makeDevice(unittest::CarLike); |
| 110 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | space = LiegroupSpace::createCopy(robot->configSpace()); |
| 111 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | space->mergeVectorSpaces(); |
| 112 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(space->name(), "SE(2)*R^2"); |
| 113 | |||
| 114 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | robot = unittest::makeDevice(unittest::ManipulatorArm2); |
| 115 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | space = LiegroupSpace::createCopy(robot->configSpace()); |
| 116 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | space->mergeVectorSpaces(); |
| 117 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(space->name(), "R^19"); |
| 118 | 2 | } | |
| 119 | |||
| 120 |
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(load_neutral_configuration) { |
| 121 | std::string urdf( | ||
| 122 | "<robot name='test'>" | ||
| 123 | "<link name='base_link'/>" | ||
| 124 | "<link name='link1'/>" | ||
| 125 | "<joint name='joint1' type='revolute'>" | ||
| 126 | " <parent link='base_link'/>" | ||
| 127 | " <child link='link1'/>" | ||
| 128 | " <limit effort='30' velocity='1.0' />" | ||
| 129 | "</joint>" | ||
| 130 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | "</robot>"); |
| 131 | std::string srdf( | ||
| 132 | "<robot name='test'>" | ||
| 133 | "<group name='all'/>" | ||
| 134 | "<group_state name='half_sitting' group='all'>" | ||
| 135 | "<joint name='joint1' value='1.' />" | ||
| 136 | "</group_state>" | ||
| 137 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | "</robot>"); |
| 138 | |||
| 139 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
4 | DevicePtr_t device = Device::create("test"); |
| 140 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | urdf::loadModelFromString(device, 0, "", "anchor", urdf, srdf); |
| 141 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK(device); |
| 142 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 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_EQUAL(device->neutralConfiguration().size(), |
| 143 | device->configSize()); | ||
| 144 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 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 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK_MESSAGE(device->neutralConfiguration().isZero(1e-12), |
| 145 | "neutral configuration - wrong results"); | ||
| 146 | |||
| 147 | /* | ||
| 148 | // TODO When neutral configuration can be read from XML string, this test | ||
| 149 | should be updated in order to | ||
| 150 | // read a URDF and SRDF string rather than a file in a different package. | ||
| 151 | Eigen::VectorXd expected(device->configSize()); | ||
| 152 | expected.setOnes(); | ||
| 153 | |||
| 154 | const Model& model = device->model(); | ||
| 155 | Model::ConfigVectorMap::const_iterator half_sitting = | ||
| 156 | model.referenceConfigurations.find ("half_sitting"); | ||
| 157 | BOOST_REQUIRE(half_sitting != model.referenceConfigurations.end()); | ||
| 158 | BOOST_CHECK_MESSAGE(half_sitting->second.isApprox (expected, 1e-12), | ||
| 159 | "reference configuration - wrong results\ngot: " | ||
| 160 | << half_sitting->second.transpose() << "\nexpected: " | ||
| 161 | << expected.transpose()); | ||
| 162 | */ | ||
| 163 | 2 | } | |
| 164 | |||
| 165 |
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(serialization) { |
| 166 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
4 | DevicePtr_t device = unittest::makeDevice(unittest::HumanoidRomeo); |
| 167 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | JointPtr_t joint = Joint::create(device, 1); |
| 168 | |||
| 169 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::stringstream ss; |
| 170 | { | ||
| 171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | hpp::serialization::xml_oarchive oa(ss); |
| 172 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | oa.initialize(); |
| 173 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | oa << boost::serialization::make_nvp("device", device); |
| 174 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | oa << boost::serialization::make_nvp("joint", joint); |
| 175 | 2 | } | |
| 176 | |||
| 177 |
6/12✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
|
2 | BOOST_TEST_MESSAGE(ss.str()); |
| 178 | |||
| 179 | 2 | DevicePtr_t device2; | |
| 180 | 2 | JointPtr_t joint2; | |
| 181 | { | ||
| 182 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | hpp::serialization::xml_iarchive ia(ss); |
| 183 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ia.insert(device->name(), device.get()); |
| 184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ia.initialize(); |
| 185 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | ia >> boost::serialization::make_nvp("device", device2); |
| 186 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | ia >> boost::serialization::make_nvp("joint", joint2); |
| 187 | 2 | } | |
| 188 | |||
| 189 |
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(device == device2); |
| 190 |
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(joint2->robot() == device2); |
| 191 | 2 | } | |
| 192 |