GCC Code Coverage Report

Directory: ./
File: tests/test-continuous-validation.cc
Date: 2024-12-13 16:14:03
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