| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2014, LAAS-CNRS | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 3 | // | ||
| 4 | // Redistribution and use in source and binary forms, with or without | ||
| 5 | // modification, are permitted provided that the following conditions are | ||
| 6 | // met: | ||
| 7 | // | ||
| 8 | // 1. Redistributions of source code must retain the above copyright | ||
| 9 | // notice, this list of conditions and the following disclaimer. | ||
| 10 | // | ||
| 11 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 12 | // notice, this list of conditions and the following disclaimer in the | ||
| 13 | // documentation and/or other materials provided with the distribution. | ||
| 14 | // | ||
| 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 16 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 17 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 18 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 21 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 23 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 24 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 26 | // DAMAGE. | ||
| 27 | |||
| 28 | #define BOOST_TEST_MODULE Parser | ||
| 29 | #include <pinocchio/fwd.hpp> | ||
| 30 | #include <boost/test/included/unit_test.hpp> | ||
| 31 | |||
| 32 | #include <pinocchio/algorithm/frames.hpp> | ||
| 33 | #include <pinocchio/algorithm/joint-configuration.hpp> | ||
| 34 | #include <pinocchio/algorithm/kinematics.hpp> | ||
| 35 | #include <pinocchio/multibody/model.hpp> | ||
| 36 | #include <hpp/util/debug.hh> | ||
| 37 | #include <hpp/pinocchio/gripper.hh> | ||
| 38 | #include <hpp/pinocchio/joint.hh> | ||
| 39 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 40 | #include <hpp/pinocchio/urdf/util.hh> | ||
| 41 | #include "hpp/manipulation/device.hh" | ||
| 42 | #include "hpp/manipulation/parser/parser.hh" | ||
| 43 | #include "hpp/manipulation/srdf/factories.hh" | ||
| 44 | #include "hpp/manipulation/srdf/util.hh" | ||
| 45 | |||
| 46 | using namespace hpp::manipulation::srdf; | ||
| 47 | using namespace hpp::manipulation::parser; | ||
| 48 | using hpp::pinocchio::Transform3s; | ||
| 49 | using hpp::pinocchio::value_type; | ||
| 50 | using hpp::pinocchio::matrix3_t; | ||
| 51 | using hpp::pinocchio::vector3_t; | ||
| 52 | using hpp::pinocchio::vector_t; | ||
| 53 | using hpp::pinocchio::FrameIndex; | ||
| 54 | |||
| 55 | class Test { | ||
| 56 | public: | ||
| 57 | int i; | ||
| 58 | double d; | ||
| 59 | std::string name; | ||
| 60 | }; | ||
| 61 | |||
| 62 | class TestDFactory : public SequenceFactory <double> { | ||
| 63 | public: | ||
| 64 | 2 | static ObjectFactory* create (ObjectFactory* parent = NULL, const XMLElement* element = NULL) | |
| 65 | { | ||
| 66 |
1/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | return new TestDFactory (parent, element); |
| 67 | } | ||
| 68 | |||
| 69 | protected: | ||
| 70 | 2 | TestDFactory (ObjectFactory* parent, const XMLElement* element) | |
| 71 | 2 | : SequenceFactory <double> (parent, element, 1) {} | |
| 72 | }; | ||
| 73 | |||
| 74 | class TestFactory : public ObjectFactory { | ||
| 75 | public: | ||
| 76 | 2 | static ObjectFactory* create (ObjectFactory* parent = NULL, const XMLElement* element = NULL) | |
| 77 | { | ||
| 78 |
1/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | return new TestFactory (parent, element); |
| 79 | } | ||
| 80 | |||
| 81 | /// Called for each attribute | ||
| 82 | 3 | virtual void impl_setAttribute (const XMLAttribute* attr) | |
| 83 | { | ||
| 84 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
|
3 | if (strcmp (attr->Name (), "othername") == 0) |
| 85 | 1 | obj_.name = attr->Value (); | |
| 86 | 3 | } | |
| 87 | |||
| 88 | 2 | virtual void finishTags () { | |
| 89 | 2 | ObjectFactory* o (NULL); | |
| 90 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
6 | if (!getChildOfType ("tag1", o)) { |
| 91 | // There is more than one tag <position> | ||
| 92 | // o is a pointer to the first one. | ||
| 93 | } | ||
| 94 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | TestDFactory* sf = o->as <TestDFactory > (); |
| 95 | 2 | obj_.d = sf->values ().front (); | |
| 96 | 2 | } | |
| 97 | |||
| 98 | Test* object () | ||
| 99 | { | ||
| 100 | return &obj_; | ||
| 101 | } | ||
| 102 | |||
| 103 | protected: | ||
| 104 | 2 | TestFactory (ObjectFactory* parent, const XMLElement* element) : | |
| 105 | 2 | ObjectFactory (parent, element) {} | |
| 106 | |||
| 107 | private: | ||
| 108 | Test obj_; | ||
| 109 | }; | ||
| 110 | |||
| 111 |
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 (testparser) |
| 112 | { | ||
| 113 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Parser p (false); |
| 114 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | p.addObjectFactory ("test", TestFactory::create); |
| 115 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | p.addObjectFactory ("tag1", TestDFactory::create); |
| 116 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | p.parseString ( |
| 117 | "<?xml version=\"1.0\"?> \n" | ||
| 118 | "<robot name=\"box\"> \n" | ||
| 119 | " <test name=\"handle\"> \n" | ||
| 120 | " <tag1>0</tag1> \n" | ||
| 121 | " <tag2 name=\"base_link\"/> \n" | ||
| 122 | " </test> \n" | ||
| 123 | " <test name=\"handle2\" othername=\"test\">\n" | ||
| 124 | " <tag1>0.54326 </tag1> \n" | ||
| 125 | " <tag2 name=\"test\"/> \n" | ||
| 126 | " </test> \n" | ||
| 127 | "</robot> \n", | ||
| 128 | 4 | hpp::manipulation::DevicePtr_t()); | |
| 129 | |||
| 130 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
|
2 | BOOST_CHECK_EQUAL (p.objectFactories().size(), 7); |
| 131 | |||
| 132 |
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 (p.objectFactories()[1]->as<TestFactory>()); |
| 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 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 (p.objectFactories()[2]->as<TestDFactory>()); |
| 134 |
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 (p.objectFactories()[4]->as<TestFactory>()); |
| 135 |
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 (p.objectFactories()[5]->as<TestDFactory>()); |
| 136 | |||
| 137 |
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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
|
2 | BOOST_TEST_MESSAGE (p); |
| 138 | 2 | } | |
| 139 | |||
| 140 | 5 | void checkPosition (const std::string xmlstring, const Transform3s& result) | |
| 141 | { | ||
| 142 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | Parser p (false); |
| 143 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | p.addObjectFactory ("position", create <PositionFactory>); |
| 144 |
2/4✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
|
15 | p.parseString (xmlstring.c_str(), hpp::manipulation::DevicePtr_t()); |
| 145 |
5/10✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 5 times.
|
5 | BOOST_REQUIRE_EQUAL (p.objectFactories().size(), 1); |
| 146 | |||
| 147 | 5 | ObjectFactory* objectFactory = p.objectFactories().front(); | |
| 148 |
6/12✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 5 times.
|
5 | BOOST_REQUIRE (objectFactory); |
| 149 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | PositionFactory* positionFactory = objectFactory->as<PositionFactory>(); |
| 150 |
6/12✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 5 times.
|
5 | BOOST_REQUIRE (positionFactory); |
| 151 | |||
| 152 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | Transform3s M = positionFactory->position(); |
| 153 |
10/20✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 5 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 5 times.
✗ Branch 29 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 5 times.
|
5 | BOOST_CHECK_MESSAGE (M.isApprox (result, 1e-5), |
| 154 | "Different transforms:\n" | ||
| 155 | << M << '\n' | ||
| 156 | << result); | ||
| 157 | 5 | } | |
| 158 | |||
| 159 |
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 (position) |
| 160 | { | ||
| 161 | 2 | value_type w = 0.786066629137, | |
| 162 | 2 | x = 0.167518791246, | |
| 163 | 2 | y = 0.570941471358, | |
| 164 | 2 | z = 0.167518791246; | |
| 165 |
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 | Transform3s result (Eigen::Quaternion<value_type>(w, x, y, z).matrix(), vector3_t (0,1,0)); |
| 166 | |||
| 167 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | std::ostringstream oss1,oss2; |
| 168 |
7/14✓ 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.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | oss1 << w << ' ' << x << ' ' << y << ' ' << z; |
| 169 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string wxyz = oss1.str(); |
| 170 |
7/14✓ 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.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | oss2 << x << ' ' << y << ' ' << z << ' ' << w; |
| 171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string xyzw = oss2.str(); |
| 172 | |||
| 173 | |||
| 174 |
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 | checkPosition ("<position>0 1 0 " + wxyz + "</position>", result); |
| 175 |
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 | checkPosition ("<position xyz='0 1 0' wxyz='" + wxyz + "' />", result); |
| 176 |
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 | checkPosition ("<position xyz='0 1 0' xyzw='" + xyzw + "' />", result); |
| 177 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkPosition ("<position xyz='0 1 0' rpy='1 1 1' />", result); |
| 178 | |||
| 179 |
5/10✓ 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.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
4 | checkPosition ("<position xyz='0 1 0' />", Transform3s (matrix3_t::Identity(), vector3_t (0,1,0))); |
| 180 | 2 | } | |
| 181 | |||
| 182 |
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 (srdfparser) |
| 183 | { | ||
| 184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Parser p (false); |
| 185 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | p.addObjectFactory ("handle", create <HandleFactory>); |
| 186 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | p.addObjectFactory ("local_position", create <PositionFactory>); |
| 187 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | p.parseFile ("/root/robotpkg/path/hpp-manipulation-urdf/work/hpp-manipulation-urdf-6.0.0/tests/ressources/box.srdf", |
| 188 | 4 | hpp::manipulation::DevicePtr_t()); | |
| 189 | |||
| 190 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::cout << p; |
| 191 | 2 | } | |
| 192 | |||
| 193 |
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(append_model) { |
| 194 | using hpp::pinocchio::GripperPtr_t; | ||
| 195 | using hpp::pinocchio::urdf::loadModel; | ||
| 196 | using hpp::pinocchio::urdf::loadModelFromString; | ||
| 197 | using hpp::manipulation::DevicePtr_t; | ||
| 198 | using hpp::manipulation::Device; | ||
| 199 | |||
| 200 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | DevicePtr_t robot = Device::create("test"); |
| 201 | 2 | FrameIndex baseFrame = 0; | |
| 202 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | matrix3_t R; |
| 203 |
9/18✓ 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.
✓ Branch 13 taken 1 times.
✗ Branch 14 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.
|
2 | R << 0, -1, 0, 1, 0, 0, 0, 0, 1; |
| 204 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | vector3_t t; |
| 205 |
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 | t << 1, 2, 3; |
| 206 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ::pinocchio::SE3 wMr(R, t), Id; |
| 207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Id.setIdentity(); |
| 208 | std::string urdfPath( | ||
| 209 | "package://example-robot-data/robots/ur_description/urdf/" | ||
| 210 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | "ur5_gripper.urdf"); |
| 211 | std::string srdfPath( | ||
| 212 | "package://example-robot-data/robots/ur_description/srdf/" | ||
| 213 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | "ur5_gripper.srdf"); |
| 214 | std::string boxUrdf( | ||
| 215 | "<robot name='box'>" | ||
| 216 | " <link name='base_link'>" | ||
| 217 | " <inertial>" | ||
| 218 | " <origin xyz='0.0 0.0 0.0' rpy='0 0 0' />" | ||
| 219 | " <mass value='0.6'/>" | ||
| 220 | " <inertia ixx='0.001' ixy='0.0' ixz='0.0'" | ||
| 221 | " iyy='0.001' iyz='0.0'" | ||
| 222 | " izz='0.001' />" | ||
| 223 | " </inertial>" | ||
| 224 | " <visual>" | ||
| 225 | " <origin xyz='0 0 0' rpy='0 0 0' />" | ||
| 226 | " <geometry>" | ||
| 227 | " <box size='0.03 0.05 0.05'/>" | ||
| 228 | " </geometry>" | ||
| 229 | " <material name='white'>" | ||
| 230 | " <color rgba='1 1 1 1'/>" | ||
| 231 | " </material>" | ||
| 232 | " </visual>" | ||
| 233 | " <collision>" | ||
| 234 | " <origin xyz='0 0 0' rpy='0 0 0' />" | ||
| 235 | " <geometry>" | ||
| 236 | " <box size='0.03 0.05 0.05'/>" | ||
| 237 | " </geometry>" | ||
| 238 | " </collision>" | ||
| 239 | " </link>" | ||
| 240 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | "</robot>"); |
| 241 | std::string boxSrdf( | ||
| 242 | "<robot name='box'>" | ||
| 243 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | "</robot>"); |
| 244 | |||
| 245 | // Load first ur5 | ||
| 246 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
8 | loadModel(robot, baseFrame, "r0", "anchor", urdfPath, srdfPath, wMr); |
| 247 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | hpp::manipulation::srdf::loadModelFromFile(robot, "r0", srdfPath); |
| 248 | // Load second ur5 | ||
| 249 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
6 | loadModel(robot, baseFrame, "r1", "anchor", urdfPath, srdfPath, |
| 250 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | ::pinocchio::SE3::Identity()); |
| 251 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | hpp::manipulation::srdf::loadModelFromFile(robot, "r1", srdfPath); |
| 252 | // append box | ||
| 253 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
8 | loadModelFromString(robot, baseFrame, "box", "freeflyer", boxUrdf, |
| 254 | boxSrdf, Id); | ||
| 255 | |||
| 256 | // Check kinematic chain | ||
| 257 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[1], "r0/shoulder_pan_joint"); |
| 258 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[2], "r0/shoulder_lift_joint"); |
| 259 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[3], "r0/elbow_joint"); |
| 260 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[4], "r0/wrist_1_joint"); |
| 261 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[5], "r0/wrist_2_joint"); |
| 262 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[6], "r0/wrist_3_joint"); |
| 263 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[7], "r1/shoulder_pan_joint"); |
| 264 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[8], "r1/shoulder_lift_joint"); |
| 265 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[9], "r1/elbow_joint"); |
| 266 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[10], "r1/wrist_1_joint"); |
| 267 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[11], "r1/wrist_2_joint"); |
| 268 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[12], "r1/wrist_3_joint"); |
| 269 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->model().names[13], "box/root_joint"); |
| 270 | |||
| 271 | // Test that first robot is placed at the specified pose | ||
| 272 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | vector_t q(robot->model().nq); |
| 273 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | ::pinocchio::neutral(robot->model(), q); |
| 274 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | ::pinocchio::forwardKinematics(robot->model(), robot->data(), q); |
| 275 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | ::pinocchio::updateFramePlacements(robot->model(), robot->data()); |
| 276 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
6 | FrameIndex i(robot->model().getFrameId("r0/base_link")); |
| 277 |
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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(robot->data().oMf[i], wMr); |
| 278 | |||
| 279 | // Test that grippers are correctly defined | ||
| 280 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
6 | GripperPtr_t gripper(robot->grippers.get("r0/gripper")); |
| 281 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(gripper->name(), "r0/gripper"); |
| 282 |
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(gripper); |
| 283 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | Transform3s T(gripper->objectPositionInJoint()); |
| 284 |
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 | matrix3_t R1; R1 << 0, 1, 0, |
| 285 |
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 | 1, 0, 0, |
| 286 |
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 | 0, 0,-1; |
| 287 |
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 | vector3_t t1; t1 << 0, 0.1373, 0; |
| 288 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Transform3s Texp(R1, t1); |
| 289 |
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 | std::cout << "T=" << T << std::endl; |
| 290 |
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 | std::cout << "Texp=" << Texp << std::endl; |
| 291 |
10/20✓ 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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK_SMALL(log6(Texp.inverse() * T).toVector().norm(), 1e-8); |
| 292 |
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(gripper->joint()->name(), "r0/wrist_3_joint"); |
| 293 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
6 | gripper = robot->grippers.get("r1/gripper"); |
| 294 |
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(gripper); |
| 295 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(gripper->name(), "r1/gripper"); |
| 296 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | T = gripper->objectPositionInJoint(); |
| 297 |
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 | std::cout << "T=" << T << std::endl; |
| 298 |
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 | std::cout << "Texp=" << Texp << std::endl; |
| 299 |
10/20✓ 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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK_SMALL(log6(Texp.inverse() * T).toVector().norm(), 1e-8); |
| 300 |
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(gripper->joint()->name(), "r1/wrist_3_joint"); |
| 301 | // Test structure of pinocchio model | ||
| 302 |
3/6✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
6 | FrameIndex frameId = robot->model().getFrameId("r0/gripper"); |
| 303 |
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 | std::cout << "Frame id of \"r0/gripper\"=" << frameId << std::endl; |
| 304 | 2 | const ::pinocchio::Frame& frame0 = robot->model().frames[frameId]; | |
| 305 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | std::cout << "Parent of \"r0/gripper\" is " << robot->model().names[frame0.parent] |
| 306 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << std::endl; |
| 307 |
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 | std::cout << "Pose of \"r0/gripper\" in parent joint = " << frame0.placement << std::endl; |
| 308 |
3/6✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
6 | frameId = robot->model().getFrameId("r1/gripper"); |
| 309 |
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 | std::cout << "Frame id of \"r1/gripper\"=" << frameId << std::endl; |
| 310 | 2 | const ::pinocchio::Frame& frame1 = robot->model().frames[frameId]; | |
| 311 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | std::cout << "Parent of \"r1/gripper\" is " << robot->model().names[frame1.parent] |
| 312 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << std::endl; |
| 313 |
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 | std::cout << "Pose of \"r1/gripper\" in parent joint = " << frame1.placement << std::endl; |
| 314 | 2 | } | |
| 315 |