Directory: | ./ |
---|---|
File: | tests/test-continuous-validation.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 92 | 123 | 74.8% |
Branches: | 246 | 648 | 38.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2017, LAAS-CNRS | ||
2 | // Authors: Florent Lamiraux | ||
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 ContinuousValidation | ||
30 | #include <boost/date_time/posix_time/posix_time.hpp> | ||
31 | #include <boost/test/included/unit_test.hpp> | ||
32 | #include <pinocchio/fwd.hpp> | ||
33 | namespace bpt = boost::posix_time; | ||
34 | |||
35 | #ifdef _OPENMP | ||
36 | #include <omp.h> | ||
37 | #endif | ||
38 | |||
39 | #include <hpp/core/collision-validation.hh> | ||
40 | #include <hpp/core/configuration-shooter/uniform.hh> | ||
41 | #include <hpp/core/continuous-validation/dichotomy.hh> | ||
42 | #include <hpp/core/continuous-validation/progressive.hh> | ||
43 | #include <hpp/core/path-validation-report.hh> | ||
44 | #include <hpp/core/path-validation/discretized-collision-checking.hh> | ||
45 | #include <hpp/core/path/spline.hh> | ||
46 | #include <hpp/core/problem.hh> | ||
47 | #include <hpp/core/steering-method/spline.hh> | ||
48 | #include <hpp/core/steering-method/straight.hh> | ||
49 | #include <hpp/pinocchio/configuration.hh> | ||
50 | #include <hpp/pinocchio/device.hh> | ||
51 | #include <hpp/pinocchio/urdf/util.hh> | ||
52 | |||
53 | using hpp::pinocchio::Device; | ||
54 | using hpp::pinocchio::DevicePtr_t; | ||
55 | |||
56 | using hpp::pinocchio::urdf::loadModel; | ||
57 | |||
58 | using hpp::core::CollisionValidation; | ||
59 | using hpp::core::Configuration_t; | ||
60 | using hpp::core::ConfigurationShooterPtr_t; | ||
61 | using hpp::core::ConfigValidationPtr_t; | ||
62 | using hpp::core::matrix_t; | ||
63 | using hpp::core::PathPtr_t; | ||
64 | using hpp::core::PathValidationPtr_t; | ||
65 | using hpp::core::PathValidationReportPtr_t; | ||
66 | using hpp::core::Problem; | ||
67 | using hpp::core::ProblemPtr_t; | ||
68 | using hpp::core::size_type; | ||
69 | using hpp::core::SteeringMethodPtr_t; | ||
70 | using hpp::core::ValidationReportPtr_t; | ||
71 | using hpp::core::vector_t; | ||
72 | using hpp::core::configurationShooter::Uniform; | ||
73 | using hpp::core::continuousCollisionChecking::Dichotomy; | ||
74 | using hpp::core::continuousCollisionChecking::Progressive; | ||
75 | using hpp::core::pathValidation::createDiscretizedCollisionChecking; | ||
76 | using hpp::core::steeringMethod::Straight; | ||
77 | |||
78 | static size_type i1 = 0, n1 = 100; | ||
79 | static size_type i2 = 0, n2 = 10; | ||
80 | |||
81 | BOOST_AUTO_TEST_SUITE(test_hpp_core) | ||
82 | |||
83 | ✗ | matrix_t generateRandomConfig(const DevicePtr_t& robot, size_type n) { | |
84 | // Create configuration shooter | ||
85 | ✗ | ConfigurationShooterPtr_t shooter(Uniform::create(robot)); | |
86 | ✗ | matrix_t m(n, robot->configSize()); | |
87 | ✗ | for (size_type i = 0; i < n; ++i) { | |
88 | ✗ | m.row(i) = shooter->shoot(); | |
89 | } | ||
90 | ✗ | return m; | |
91 | } | ||
92 | |||
93 | ✗ | matrix_t generateRandomVelocities(const DevicePtr_t& robot, size_type n) { | |
94 | // Create configuration shooter | ||
95 | ✗ | ConfigurationShooterPtr_t shooter(Uniform::create(robot)); | |
96 | ✗ | matrix_t m(n, robot->numberDof()); | |
97 | ✗ | for (size_type i = 0; i < n; ++i) { | |
98 | ✗ | m.row(i) = vector_t::Random(robot->numberDof()); | |
99 | } | ||
100 | ✗ | return m; | |
101 | } | ||
102 | |||
103 | ✗ | void generate_random_numbers() { | |
104 | // Load robot model (ur5) | ||
105 | ✗ | DevicePtr_t robot(Device::create("ur5")); | |
106 | ✗ | loadModel(robot, 0, "", "anchor", | |
107 | "package://ur_description/" | ||
108 | "urdf/ur5_joint_limited_robot.urdf", | ||
109 | "package://ur_description/" | ||
110 | "srdf/ur5_joint_limited_robot.srdf"); | ||
111 | ✗ | matrix_t rand1 = generateRandomConfig(robot, 2 * (n1 + n2)); | |
112 | ✗ | matrix_t rand2 = generateRandomVelocities(robot, 2 * n2); | |
113 | |||
114 | Eigen::IOFormat f1(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", | ||
115 | ✗ | "", "", "", ";\n"); | |
116 | Eigen::IOFormat f2(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", | ||
117 | ✗ | "", "", "", ";\n"); | |
118 | ✗ | std::cout << "static matrix_t m1 (" << rand1.rows() << ", " << rand1.cols() | |
119 | ✗ | << "); m1 << " << rand1.format(f1); | |
120 | ✗ | std::cout << "static matrix_t m2 (" << rand2.rows() << ", " << rand2.cols() | |
121 | ✗ | << "); m2 << " << rand2.format(f2); | |
122 | } | ||
123 | |||
124 | #if 0 | ||
125 | BOOST_AUTO_TEST_CASE (random) | ||
126 | { | ||
127 | generate_random_numbers (); | ||
128 | } | ||
129 | #else | ||
130 | |||
131 | static matrix_t m1(220, 6); | ||
132 | static matrix_t m2(20, 6); | ||
133 | |||
134 |
33/66✓ Branch 1 taken 2 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(continuous_validation_straight) { |
135 | #include "../tests/random-numbers.hh" | ||
136 | 2 | i1 = 0; | |
137 | 2 | i2 = 0; | |
138 | |||
139 | // Load robot model (ur5) | ||
140 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
4 | DevicePtr_t robot(Device::create("ur5")); |
141 |
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 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
|
2 | loadModel(robot, 0, "", "anchor", |
142 | "package://ur_description/" | ||
143 | "urdf/ur5_joint_limited_robot.urdf", | ||
144 | "package://ur_description/" | ||
145 | "srdf/ur5_joint_limited_robot.srdf"); | ||
146 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | robot->numberDeviceData(4); |
147 | |||
148 | // create steering method | ||
149 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | ProblemPtr_t problem = Problem::create(robot); |
150 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | SteeringMethodPtr_t sm(Straight::create(problem)); |
151 | |||
152 | // create path validation objects | ||
153 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PathValidationPtr_t dichotomy(Dichotomy::create(robot, 0)); |
154 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PathValidationPtr_t progressive(Progressive::create(robot, 0.001)); |
155 | PathValidationPtr_t discretized( | ||
156 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | createDiscretizedCollisionChecking(robot, 0.05)); |
157 | // create configuration validation instance | ||
158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ConfigValidationPtr_t configValidation(CollisionValidation::create(robot)); |
159 | 2 | ValidationReportPtr_t collisionReport; | |
160 | // create random paths and test them with different validation instances | ||
161 |
4/8✓ 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.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | Configuration_t q1(robot->configSize()), q2(robot->configSize()); |
162 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | bpt::ptime t0 = bpt::microsec_clock::local_time(); |
163 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
|
202 | for (size_type i = 0; i < n1; ++i) { |
164 | { | ||
165 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
200 | q1 = m1.row(i1); |
166 | 200 | ++i1; | |
167 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
200 | q2 = m1.row(i1); |
168 | 200 | ++i1; | |
169 | } | ||
170 | 200 | PathValidationReportPtr_t report1; | |
171 | 200 | PathValidationReportPtr_t report2; | |
172 | 200 | PathValidationReportPtr_t report3; | |
173 | 200 | ValidationReportPtr_t report4; | |
174 | 200 | ValidationReportPtr_t report5; | |
175 | 200 | ValidationReportPtr_t report6; | |
176 |
3/6✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
400 | PathPtr_t path((*sm)(q1, q2)); |
177 |
2/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
|
200 | bool res4(discretized->validate(q1, report4)); |
178 |
2/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
|
200 | bool res5(progressive->validate(q1, report5)); |
179 |
2/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
|
200 | bool res6(dichotomy->validate(q1, report6)); |
180 | 200 | PathPtr_t validPart; | |
181 |
3/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 89 times.
✓ Branch 5 taken 11 times.
|
200 | if (configValidation->validate(q1, collisionReport)) { |
182 |
1/2✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
|
178 | bool res1(discretized->validate(path, false, validPart, report1)); |
183 |
1/2✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
|
178 | bool res2(progressive->validate(path, false, validPart, report2)); |
184 |
1/2✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
|
178 | bool res3(dichotomy->validate(path, false, validPart, report3)); |
185 | |||
186 | // Check that PathValidation::validate(ConfigurationIn_t,...) returns | ||
187 | // the same result as config validation. | ||
188 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 89 times.
|
178 | if (!res4) { |
189 | ✗ | std::cout << "q=" << hpp::pinocchio::displayConfig(q1) << std::endl; | |
190 | ✗ | std::cout << "report 4: " << *report4 << std::endl; | |
191 | } | ||
192 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 89 times.
|
178 | if (!res5) { |
193 | ✗ | std::cout << "q=" << hpp::pinocchio::displayConfig(q1) << std::endl; | |
194 | ✗ | std::cout << "report 5: " << *report5 << std::endl; | |
195 | } | ||
196 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 89 times.
|
178 | if (!res6) { |
197 | ✗ | std::cout << "q=" << hpp::pinocchio::displayConfig(q1) << std::endl; | |
198 | ✗ | std::cout << "report 6: " << *report6 << std::endl; | |
199 | } | ||
200 |
6/12✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 89 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 89 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 89 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 89 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 89 times.
|
178 | BOOST_CHECK(res4); |
201 |
6/12✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 89 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 89 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 89 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 89 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 89 times.
|
178 | BOOST_CHECK(res5); |
202 |
6/12✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 89 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 89 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 89 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 89 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 89 times.
|
178 | BOOST_CHECK(res6); |
203 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 73 times.
|
178 | if (!res1) { |
204 |
6/12✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 16 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 16 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 16 times.
|
32 | BOOST_CHECK(!res2); |
205 |
6/12✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 16 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 16 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 16 times.
|
32 | BOOST_CHECK(!res3); |
206 | if (res2) { | ||
207 | hppDout(error, "Progressive failed to detect collision for q1=" | ||
208 | << q1.transpose() << ", q2=" << q2.transpose()); | ||
209 | hppDout(error, *report1); | ||
210 | } | ||
211 | if (res3) { | ||
212 | hppDout(error, "Dichotomy failed to detect collision for q1=" | ||
213 | << q1.transpose() << ", q2=" << q2.transpose()); | ||
214 | hppDout(error, *report1); | ||
215 | } | ||
216 | } | ||
217 |
2/2✓ Branch 0 taken 73 times.
✓ Branch 1 taken 16 times.
|
178 | if (res1) { |
218 |
6/12✓ Branch 1 taken 73 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 73 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 73 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 73 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 73 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 73 times.
|
146 | BOOST_CHECK(res2); |
219 |
6/12✓ Branch 1 taken 73 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 73 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 73 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 73 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 73 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 73 times.
|
146 | BOOST_CHECK(res3); |
220 | 146 | if (!res2) { | |
221 | hppDout(info, | ||
222 | "Progressive found a collision where discretized did " | ||
223 | "not for q1 = " | ||
224 | << q1.transpose() << ", q2 = " << q2.transpose()); | ||
225 | hppDout(info, *report2); | ||
226 | } | ||
227 | 146 | if (!res3) { | |
228 | hppDout(info, | ||
229 | "Dichotomy found a collision where discretized did " | ||
230 | "not for q1 = " | ||
231 | << q1.transpose() << ", q2 = " << q2.transpose()); | ||
232 | hppDout(info, *report3); | ||
233 | } | ||
234 | } | ||
235 | } else { | ||
236 | // Check that PathValidation::validate(ConfigurationIn_t,...) returns | ||
237 | // the same result as config validation. | ||
238 |
3/6✓ Branch 0 taken 11 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 11 times.
|
22 | if (res4 || res5 || res6) { |
239 | ✗ | std::cout << "q=" << hpp::pinocchio::displayConfig(q1) << std::endl; | |
240 | ✗ | std::cout << "collisionReport: " << *collisionReport << std::endl; | |
241 | } | ||
242 |
6/12✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 11 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 11 times.
|
22 | BOOST_CHECK(!res4); |
243 |
6/12✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 11 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 11 times.
|
22 | BOOST_CHECK(!res5); |
244 |
6/12✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 11 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 11 times.
|
22 | BOOST_CHECK(!res6); |
245 | } | ||
246 |
3/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 86 times.
✓ Branch 5 taken 14 times.
|
200 | if (configValidation->validate(q2, collisionReport)) { |
247 |
1/2✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
|
172 | bool res1(discretized->validate(path, true, validPart, report1)); |
248 |
1/2✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
|
172 | bool res2(progressive->validate(path, true, validPart, report2)); |
249 |
1/2✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
|
172 | bool res3(dichotomy->validate(path, true, validPart, report3)); |
250 | |||
251 |
2/2✓ Branch 0 taken 13 times.
✓ Branch 1 taken 73 times.
|
172 | if (!res1) { |
252 |
6/12✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 13 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 13 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 13 times.
|
26 | BOOST_CHECK(!res2); |
253 |
6/12✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 13 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 13 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 13 times.
|
26 | BOOST_CHECK(!res3); |
254 | if (res2) { | ||
255 | hppDout(error, "Progressive failed to detect collision for q1=" | ||
256 | << q1.transpose() << ", q2=" << q2.transpose()); | ||
257 | hppDout(error, *report1); | ||
258 | } | ||
259 | if (res3) { | ||
260 | hppDout(error, "Dichotomy failed to detect collision for q1=" | ||
261 | << q1.transpose() << ", q2=" << q2.transpose()); | ||
262 | hppDout(error, *report1); | ||
263 | } | ||
264 | } | ||
265 |
2/2✓ Branch 0 taken 73 times.
✓ Branch 1 taken 13 times.
|
172 | if (res1) { |
266 | 146 | if (!res2) { | |
267 | hppDout(info, | ||
268 | "Progressive found a collision where discretized did " | ||
269 | "not for q1 = " | ||
270 | << q1.transpose() << ", q2 = " << q2.transpose()); | ||
271 | hppDout(info, *report2); | ||
272 | } | ||
273 | 146 | if (!res3) { | |
274 | hppDout(info, | ||
275 | "Dichotomy found a collision where discretized did " | ||
276 | "not for q1 = " | ||
277 | << q1.transpose() << ", q2 = " << q2.transpose()); | ||
278 | hppDout(info, *report3); | ||
279 | } | ||
280 | } | ||
281 | } | ||
282 | 200 | } | |
283 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | bpt::ptime t1 = bpt::microsec_clock::local_time(); |
284 |
9/18✓ 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.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
|
2 | BOOST_TEST_MESSAGE("Total time: " << (t1 - t0).total_milliseconds() << "ms"); |
285 | // delete problem | ||
286 | 2 | } | |
287 | |||
288 | template <typename SplineSteeringMethod> | ||
289 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | void test_spline_steering_method() { |
290 | #include "../tests/random-numbers.hh" | ||
291 | 1 | i1 = 0; | |
292 | 1 | i2 = 0; | |
293 | |||
294 | // Load robot model (ur5) | ||
295 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | DevicePtr_t robot(Device::create("ur5")); |
296 |
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 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
|
1 | loadModel(robot, 0, "", "anchor", |
297 | "package://ur_description/" | ||
298 | "urdf/ur5_joint_limited_robot.urdf", | ||
299 | "package://ur_description/" | ||
300 | "srdf/ur5_joint_limited_robot.srdf"); | ||
301 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | robot->numberDeviceData(4); |
302 | |||
303 | // create steering method | ||
304 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ProblemPtr_t problem = Problem::create(robot); |
305 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | typename SplineSteeringMethod::Ptr_t sm( |
306 | SplineSteeringMethod::create(problem)); | ||
307 | |||
308 | // create path validation objects | ||
309 | // PathValidationPtr_t dichotomy (Dichotomy::create (robot, 0)); | ||
310 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | PathValidationPtr_t progressive(Progressive::create(robot, 0.01)); |
311 | 1 | PathValidationPtr_t discretized( | |
312 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | createDiscretizedCollisionChecking(robot, 0.05)); |
313 | // create configuration validation instance | ||
314 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ConfigValidationPtr_t configValidation(CollisionValidation::create(robot)); |
315 | 1 | ValidationReportPtr_t collisionReport; | |
316 | |||
317 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::vector<int> orders(1, 1); |
318 | // create random paths and test them with different validation instances | ||
319 |
4/8✓ 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.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
1 | Configuration_t q1(robot->configSize()), q2(robot->configSize()); |
320 |
4/8✓ 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.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
1 | vector_t v1(robot->numberDof()), v2(robot->numberDof()); |
321 | 1 | int Nthreads = 1; | |
322 | |||
323 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | bpt::ptime t0 = bpt::microsec_clock::local_time(); |
324 | 1 | #pragma omp parallel for | |
325 | for (size_type i = 0; i < n2; ++i) { | ||
326 | #ifdef _OPENMP | ||
327 | Nthreads = omp_get_num_threads(); | ||
328 | #endif | ||
329 | #pragma omp critical | ||
330 | { | ||
331 | q1 = m1.row(i1++); | ||
332 | q2 = m1.row(i1++); | ||
333 | v1 = m2.row(i2++); | ||
334 | v2 = m2.row(i2++); | ||
335 | } | ||
336 | PathValidationReportPtr_t report1; | ||
337 | PathValidationReportPtr_t report2; | ||
338 | PathValidationReportPtr_t report3; | ||
339 | PathPtr_t path(sm->steer(q1, orders, v1, q2, orders, v2)); | ||
340 | PathPtr_t validPart; | ||
341 | if (configValidation->validate(q1, collisionReport)) { | ||
342 | bool res1(discretized->validate(path, false, validPart, report1)); | ||
343 | bool res2(progressive->validate(path, false, validPart, report2)); | ||
344 | // bool res3 (dichotomy->validate (path, false, validPart, report3)); | ||
345 | |||
346 | #pragma omp critical | ||
347 | if (!res1) { | ||
348 | BOOST_CHECK(!res2); | ||
349 | // BOOST_CHECK (!res3); | ||
350 | if (res2) { | ||
351 | hppDout(error, "Progressive failed to detect collision for q1=" | ||
352 | << q1.transpose() << ", q2=" << q2.transpose()); | ||
353 | hppDout(error, *report1); | ||
354 | } | ||
355 | /*if (res3) { | ||
356 | hppDout (error, "Dichotomy failed to detect collision for q1=" | ||
357 | << q1.transpose () << ", q2=" << q2.transpose ()); | ||
358 | hppDout (error, *report1); | ||
359 | }*/ | ||
360 | } | ||
361 | #pragma omp critical | ||
362 | if (res1) { | ||
363 | BOOST_CHECK(res2); | ||
364 | if (!res2) { | ||
365 | hppDout(info, | ||
366 | "Progressive found a collision where discretized did " | ||
367 | "not for q1 = " | ||
368 | << q1.transpose() << ", q2 = " << q2.transpose()); | ||
369 | hppDout(info, *report2); | ||
370 | } | ||
371 | /*if (!res3) { | ||
372 | hppDout (info, "Dichotomy found a collision where discretized did " | ||
373 | "not for q1 = " << q1.transpose () << ", q2 = " | ||
374 | << q2.transpose ()); | ||
375 | hppDout (info, *report3); | ||
376 | }*/ | ||
377 | } | ||
378 | } | ||
379 | if (configValidation->validate(q2, collisionReport)) { | ||
380 | bool res1(discretized->validate(path, true, validPart, report1)); | ||
381 | bool res2(progressive->validate(path, true, validPart, report2)); | ||
382 | // bool res3 (dichotomy->validate (path, true, validPart, report3)); | ||
383 | |||
384 | #pragma omp critical | ||
385 | if (!res1) { | ||
386 | BOOST_CHECK(!res2); | ||
387 | // BOOST_CHECK (!res3); | ||
388 | if (res2) { | ||
389 | hppDout(error, "Progressive failed to detect collision for q1=" | ||
390 | << q1.transpose() << ", q2=" << q2.transpose()); | ||
391 | hppDout(error, *report1); | ||
392 | } | ||
393 | /*if (res3) { | ||
394 | hppDout (error, "Dichotomy failed to detect collision for q1=" | ||
395 | << q1.transpose () << ", q2=" << q2.transpose ()); | ||
396 | hppDout (error, *report1); | ||
397 | }*/ | ||
398 | } | ||
399 | #pragma omp critical | ||
400 | if (res1) { | ||
401 | if (!res2) { | ||
402 | hppDout(info, | ||
403 | "Progressive found a collision where discretized did " | ||
404 | "not for q1 = " | ||
405 | << q1.transpose() << ", q2 = " << q2.transpose()); | ||
406 | hppDout(info, *report2); | ||
407 | } | ||
408 | /*if (!res3) { | ||
409 | hppDout (info, "Dichotomy found a collision where discretized did " | ||
410 | "not for q1 = " << q1.transpose () << ", q2 = " | ||
411 | << q2.transpose ()); | ||
412 | hppDout (info, *report3); | ||
413 | }*/ | ||
414 | } | ||
415 | } | ||
416 | } | ||
417 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | bpt::ptime t1 = bpt::microsec_clock::local_time(); |
418 |
11/22✓ 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.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 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.
|
1 | BOOST_TEST_MESSAGE("Total time (nthreads " |
419 | << Nthreads << "): " << (t1 - t0).total_milliseconds() | ||
420 | << "ms"); | ||
421 | // delete problem | ||
422 | 1 | } | |
423 | |||
424 |
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(continuous_validation_spline) { |
425 | test_spline_steering_method< | ||
426 | 2 | hpp::core::steeringMethod::Spline<hpp::core::path::BernsteinBasis, 3> >(); | |
427 | 2 | } | |
428 | #endif | ||
429 | |||
430 | BOOST_AUTO_TEST_SUITE_END() | ||
431 |