Directory: | ./ |
---|---|
File: | tests/path-projectors.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 104 | 136 | 76.5% |
Branches: | 166 | 445 | 37.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2016, Joseph Mirabel | ||
2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
3 | // | ||
4 | |||
5 | // Redistribution and use in source and binary forms, with or without | ||
6 | // modification, are permitted provided that the following conditions are | ||
7 | // met: | ||
8 | // | ||
9 | // 1. Redistributions of source code must retain the above copyright | ||
10 | // notice, this list of conditions and the following disclaimer. | ||
11 | // | ||
12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
13 | // notice, this list of conditions and the following disclaimer in the | ||
14 | // documentation and/or other materials provided with the distribution. | ||
15 | // | ||
16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
27 | // DAMAGE. | ||
28 | |||
29 | #define BOOST_TEST_MODULE pathProjector | ||
30 | #include <boost/mpl/list.hpp> | ||
31 | #include <boost/test/included/unit_test.hpp> | ||
32 | #include <boost/test/unit_test.hpp> | ||
33 | #include <pinocchio/fwd.hpp> | ||
34 | |||
35 | // Boost version 1.54 | ||
36 | // Cannot include boost CPU timers | ||
37 | // #include <boost/timer/timer.hpp> | ||
38 | // because the original timers are already included by | ||
39 | // the unit test framework | ||
40 | // #include <boost/timer.hh> | ||
41 | |||
42 | // Force benchmark output | ||
43 | #define HPP_ENABLE_BENCHMARK 1 | ||
44 | #include <hpp/constraints/differentiable-function.hh> | ||
45 | #include <hpp/constraints/implicit.hh> | ||
46 | #include <hpp/core/config-projector.hh> | ||
47 | #include <hpp/core/constraint-set.hh> | ||
48 | #include <hpp/core/interpolated-path.hh> | ||
49 | #include <hpp/core/path-projector/global.hh> | ||
50 | #include <hpp/core/path-projector/progressive.hh> | ||
51 | #include <hpp/core/path-projector/recursive-hermite.hh> | ||
52 | #include <hpp/core/path/hermite.hh> | ||
53 | #include <hpp/core/problem.hh> | ||
54 | #include <hpp/core/steering-method/hermite.hh> | ||
55 | #include <hpp/core/steering-method/straight.hh> | ||
56 | #include <hpp/core/straight-path.hh> | ||
57 | #include <hpp/pinocchio/device.hh> | ||
58 | #include <hpp/pinocchio/urdf/util.hh> | ||
59 | #include <hpp/util/timer.hh> | ||
60 | |||
61 | using hpp::constraints::EqualToZero; | ||
62 | using hpp::constraints::Implicit; | ||
63 | |||
64 | using namespace hpp::core; | ||
65 | using namespace hpp::pinocchio; | ||
66 | |||
67 | 6 | DevicePtr_t createRobot() { | |
68 | std::string urdf( | ||
69 | "<robot name='test'>" | ||
70 | "<link name='link1'/>" | ||
71 | "<link name='link2'/>" | ||
72 | "<link name='link3'/>" | ||
73 | "<joint name='tx' type='prismatic'>" | ||
74 | "<parent link='link1'/>" | ||
75 | "<child link='link2'/>" | ||
76 | "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>" | ||
77 | "</joint>" | ||
78 | "<joint name='ty' type='prismatic'>" | ||
79 | "<axis xyz='0 1 0'/>" | ||
80 | "<parent link='link2'/>" | ||
81 | "<child link='link3'/>" | ||
82 | "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>" | ||
83 | "</joint>" | ||
84 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | "</robot>"); |
85 | |||
86 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
12 | DevicePtr_t robot = Device::create("test"); |
87 |
5/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
|
6 | urdf::loadModelFromString(robot, 0, "", "anchor", urdf, ""); |
88 | 12 | return robot; | |
89 | 6 | } | |
90 | |||
91 | 6 | ConstraintSetPtr_t createConstraints(DevicePtr_t r) { | |
92 | ConfigProjectorPtr_t proj = | ||
93 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
12 | ConfigProjector::create(r, "Polynomial projector", 1e-4, 20); |
94 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
12 | ConstraintSetPtr_t set = ConstraintSet::create(r, "Set"); |
95 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | set->addConstraint(proj); |
96 | 12 | return set; | |
97 | 6 | } | |
98 | |||
99 | class Polynomial : public DifferentiableFunction { | ||
100 | public: | ||
101 | 6 | Polynomial(DevicePtr_t robot) | |
102 | 6 | : DifferentiableFunction(robot->configSize(), robot->numberDof(), | |
103 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | LiegroupSpace::R1(), "Polynomial"), |
104 |
7/14✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
|
18 | coefs_(vector_t::Ones(robot->configSize())) {} |
105 | |||
106 | vector_t coefs_; | ||
107 | |||
108 | protected: | ||
109 | 13935244 | void impl_compute(LiegroupElementRef result, vectorIn_t argument) const { | |
110 |
2/4✓ Branch 2 taken 13935244 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13935244 times.
✗ Branch 7 not taken.
|
13935244 | result.vector()[0] = argument.cwiseProduct(argument).dot(coefs_) - 1; |
111 | 13935244 | } | |
112 | 4650348 | void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const { | |
113 |
3/6✓ Branch 2 taken 4650348 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4650348 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4650348 times.
✗ Branch 9 not taken.
|
4650348 | jacobian.row(0) = 2 * arg.cwiseProduct(coefs_); |
114 | 4650348 | } | |
115 | }; | ||
116 | |||
117 | typedef std::shared_ptr<Polynomial> PolynomialPtr_t; | ||
118 | |||
119 | /* | ||
120 | bool checkContinuity (PathPtr_t path) { | ||
121 | const value_type stepPath = path->length () / (100 - 1); | ||
122 | Configuration_t q = path->initial(), qq = path->initial(); | ||
123 | vector_t v1 (func->outputSize()), v2(func->outputSize()); | ||
124 | std::cerr << std::fixed << std::showpos << std::setprecision (4); | ||
125 | const char* sep = "\t| "; | ||
126 | for (std::size_t i = 0; i < 100; ++i) { | ||
127 | if (!(*path) (q, (value_type)i * stepPath)) | ||
128 | std::cerr << "Could not project path at " << (value_type)i*stepPath | ||
129 | << "\n"; | ||
130 | if (!(*projection) (qq, (value_type) i * stepProj)) | ||
131 | std::cerr << "Could not project projection at " | ||
132 | << (value_type) i*stepProj << "\n"; | ||
133 | (*func) (v1, q); | ||
134 | (*func) (v2, qq); | ||
135 | std::cerr << q.transpose () << sep << v1 | ||
136 | << sep << qq.transpose () << sep << v2 << "\n"; | ||
137 | } | ||
138 | |||
139 | // Analyse projection | ||
140 | if (!HPP_DYNAMIC_PTR_CAST (InterpolatedPath, projection)) { | ||
141 | std::cout << "Path is not an InterpolatedPath\n"; | ||
142 | std::cerr | ||
143 | << projection->timeRange().first << sep << projection->initial | ||
144 | ().transpose() << '\n' | ||
145 | << projection->timeRange().first + projection->timeRange().second << sep | ||
146 | << projection->end ().transpose() << '\n'; return; | ||
147 | } | ||
148 | InterpolatedPath& p = *(HPP_DYNAMIC_PTR_CAST (InterpolatedPath, projection)); | ||
149 | typedef InterpolatedPath::InterpolationPoints_t InterpolationPoints_t; | ||
150 | const InterpolationPoints_t& points = p.interpolationPoints (); | ||
151 | std::cout << "InterpolatedPath: " << points.size() << " interpolation | ||
152 | points.\n"; for (InterpolationPoints_t::const_iterator it = points.begin(); it | ||
153 | != points.end(); ++it) { std::cerr << it->first << sep << it->second.transpose() | ||
154 | << '\n'; | ||
155 | } | ||
156 | } | ||
157 | // */ | ||
158 | ✗ | void displayPaths(PathPtr_t path, PathPtr_t projection, | |
159 | DifferentiableFunctionPtr_t func) { | ||
160 | ✗ | const value_type stepPath = path->length() / (100 - 1); | |
161 | ✗ | const value_type stepProj = projection->length() / (100 - 1); | |
162 | ✗ | Configuration_t q = path->initial(), qq = path->initial(); | |
163 | ✗ | LiegroupElement v1(func->outputSpace()), v2(func->outputSpace()); | |
164 | ✗ | std::cerr << std::fixed << std::showpos << std::setprecision(4); | |
165 | ✗ | const char* sep = "\t| "; | |
166 | ✗ | for (std::size_t i = 0; i < 100; ++i) { | |
167 | ✗ | if (!path->eval(q, (value_type)i * stepPath)) | |
168 | ✗ | std::cerr << "Could not project path at " << (value_type)i * stepPath | |
169 | ✗ | << "\n"; | |
170 | ✗ | if (!projection->eval(qq, (value_type)i * stepProj)) | |
171 | ✗ | std::cerr << "Could not project projection at " | |
172 | ✗ | << (value_type)i * stepProj << "\n"; | |
173 | ✗ | func->value(v1, q); | |
174 | ✗ | func->value(v2, qq); | |
175 | ✗ | std::cerr << q.transpose() << sep << v1 << sep << qq.transpose() << sep | |
176 | ✗ | << v2 << "\n"; | |
177 | } | ||
178 | |||
179 | // Analyse projection | ||
180 | ✗ | if (!HPP_DYNAMIC_PTR_CAST(InterpolatedPath, projection)) { | |
181 | ✗ | std::cout << "Path is not an InterpolatedPath\n"; | |
182 | ✗ | std::cerr << projection->timeRange().first << sep | |
183 | ✗ | << projection->initial().transpose() << '\n' | |
184 | ✗ | << projection->timeRange().first + projection->timeRange().second | |
185 | ✗ | << sep << projection->end().transpose() << '\n'; | |
186 | ✗ | return; | |
187 | } | ||
188 | ✗ | InterpolatedPath& p = *(HPP_DYNAMIC_PTR_CAST(InterpolatedPath, projection)); | |
189 | typedef InterpolatedPath::InterpolationPoints_t InterpolationPoints_t; | ||
190 | ✗ | const InterpolationPoints_t& points = p.interpolationPoints(); | |
191 | ✗ | std::cout << "InterpolatedPath: " << points.size() | |
192 | ✗ | << " interpolation points.\n"; | |
193 | ✗ | for (InterpolationPoints_t::const_iterator it = points.begin(); | |
194 | ✗ | it != points.end(); ++it) { | |
195 | ✗ | std::cerr << it->first << sep << it->second.transpose() << '\n'; | |
196 | } | ||
197 | } | ||
198 | |||
199 | struct traits_circle { | ||
200 | 3 | static DifferentiableFunctionPtr_t func(DevicePtr_t dev) { | |
201 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | return PolynomialPtr_t(new Polynomial(dev)); |
202 | } | ||
203 | 48 | static void make_conf(ConfigurationOut_t q1, ConfigurationOut_t q2, | |
204 | const int index) { | ||
205 |
8/9✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 6 times.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
48 | switch (index) { |
206 | 6 | case 0: | |
207 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 0, 1; |
208 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << 1, 0; |
209 | 6 | break; | |
210 | 6 | case 1: | |
211 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
212 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << -1, 0; |
213 | 6 | break; | |
214 | 6 | case 2: { | |
215 | 6 | double c = -0.99; | |
216 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
217 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << c, sqrt(1 - c * c); |
218 | 6 | break; | |
219 | } | ||
220 | 6 | case 3: { | |
221 | 6 | double c = -0.9; | |
222 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
223 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << c, sqrt(1 - c * c); |
224 | 6 | break; | |
225 | } | ||
226 | 6 | case 4: { | |
227 | 6 | double c = -0.85; | |
228 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
229 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << c, sqrt(1 - c * c); |
230 | 6 | break; | |
231 | } | ||
232 | 6 | case 5: { | |
233 | 6 | double c = -0.8; | |
234 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
235 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << c, sqrt(1 - c * c); |
236 | 6 | break; | |
237 | } | ||
238 | 6 | case 6: { | |
239 | 6 | double c = -0.75; | |
240 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
241 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << c, sqrt(1 - c * c); |
242 | 6 | break; | |
243 | } | ||
244 | 6 | case 7: { | |
245 | 6 | double c = -0.7; | |
246 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q1 << 1, 0; |
247 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | q2 << c, sqrt(1 - c * c); |
248 | 6 | break; | |
249 | } | ||
250 | } | ||
251 | 48 | } | |
252 | static const int NB_CONFS; | ||
253 | static const value_type K; | ||
254 | static const char* _func; | ||
255 | }; | ||
256 | struct traits_parabola { | ||
257 | 3 | static DifferentiableFunctionPtr_t func(DevicePtr_t dev) { | |
258 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | PolynomialPtr_t parabola(new Polynomial(dev)); |
259 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | parabola->coefs_(1) = 0; |
260 | 6 | return parabola; | |
261 | 3 | } | |
262 | 48 | static void make_conf(ConfigurationOut_t q1, ConfigurationOut_t q2, | |
263 | const int index) { | ||
264 |
2/4✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
|
48 | q1 << 1, 0; |
265 |
2/4✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
|
48 | q2 << -1, (value_type)(2 * index) / (NB_CONFS - 1); |
266 | // switch (index) { | ||
267 | // case 0: q1 << 1,0; q2 << -1,0; break; // Should be really fast | ||
268 | // case 1: q1 << 1,0; q2 << -1,1; break; // Should be slower | ||
269 | // } | ||
270 | 48 | } | |
271 | static const int NB_CONFS; | ||
272 | static const value_type K; | ||
273 | static const char* _func; | ||
274 | }; | ||
275 | const int traits_circle::NB_CONFS = 8; | ||
276 | const value_type traits_circle::K = 2.001; | ||
277 | const char* traits_circle::_func = "circle"; | ||
278 | const int traits_parabola::NB_CONFS = 8; | ||
279 | const value_type traits_parabola::K = 2 * sqrt(2); | ||
280 | const char* traits_parabola::_func = "parabola"; | ||
281 | |||
282 | struct traits_progressive { | ||
283 | typedef pathProjector::Progressive Proj_t; | ||
284 | typedef pathProjector::ProgressivePtr_t ProjPtr_t; | ||
285 | typedef steeringMethod::Straight SM_t; | ||
286 | static const value_type projection_step; | ||
287 | static const char* _proj; | ||
288 | }; | ||
289 | struct traits_global { | ||
290 | typedef pathProjector::Global Proj_t; | ||
291 | typedef pathProjector::GlobalPtr_t ProjPtr_t; | ||
292 | typedef steeringMethod::Straight SM_t; | ||
293 | static const value_type projection_step; | ||
294 | static const char* _proj; | ||
295 | }; | ||
296 | struct traits_hermite { | ||
297 | typedef pathProjector::RecursiveHermite Proj_t; | ||
298 | typedef pathProjector::RecursiveHermitePtr_t ProjPtr_t; | ||
299 | typedef steeringMethod::Hermite SM_t; | ||
300 | static const value_type projection_step; | ||
301 | static const char* _proj; | ||
302 | }; | ||
303 | const value_type traits_progressive::projection_step = 0.1; | ||
304 | const value_type traits_global ::projection_step = 0.1; | ||
305 | const value_type traits_hermite ::projection_step = 2; | ||
306 | const char* traits_progressive::_proj = "progressive"; | ||
307 | const char* traits_global ::_proj = "global"; | ||
308 | const char* traits_hermite ::_proj = "hermite"; | ||
309 | |||
310 | struct traits_global_circle : traits_global, traits_circle {}; | ||
311 | struct traits_global_parabola : traits_global, traits_parabola {}; | ||
312 | struct traits_progressive_circle : traits_progressive, traits_circle {}; | ||
313 | struct traits_progressive_parabola : traits_progressive, traits_parabola {}; | ||
314 | struct traits_hermite_circle : traits_hermite, traits_circle {}; | ||
315 | struct traits_hermite_parabola : traits_hermite, traits_parabola {}; | ||
316 | |||
317 | typedef boost::mpl::list<traits_global_circle, traits_progressive_circle, | ||
318 | traits_hermite_circle, traits_global_parabola, | ||
319 | traits_progressive_parabola, traits_hermite_parabola> | ||
320 | test_types; | ||
321 | |||
322 |
33/66✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 6 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 6 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 6 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 6 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 6 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 6 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 6 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 6 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 6 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 6 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 6 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 6 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 6 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 6 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 6 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 6 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 6 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 6 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 6 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 6 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 6 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 6 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 6 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 6 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 6 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 6 times.
✗ Branch 117 not taken.
|
24 | BOOST_AUTO_TEST_CASE_TEMPLATE(projectors, traits, test_types) { |
323 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | DevicePtr_t dev = createRobot(); |
324 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 6 times.
|
12 | BOOST_REQUIRE(dev); |
325 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
12 | ProblemPtr_t problem = Problem::create(dev); |
326 | |||
327 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
12 | ConstraintSetPtr_t c = createConstraints(dev); |
328 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
12 | DifferentiableFunctionPtr_t func = traits::func(dev); |
329 |
3/6✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
36 | c->configProjector()->add(Implicit::create( |
330 |
1/2✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
|
24 | func, ComparisonTypes_t(func->outputSpace()->nv(), EqualToZero))); |
331 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
12 | problem->steeringMethod(traits::SM_t::create(problem)); |
332 | 12 | problem->steeringMethod()->constraints(c); | |
333 | |||
334 |
2/2✓ Branch 3 taken 12 times.
✓ Branch 4 taken 6 times.
|
36 | for (int c = 0; c < 2; ++c) { |
335 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
|
24 | if (c == 0) |
336 |
2/4✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
|
24 | problem->setParameter("PathProjection/HessianBound", |
337 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
24 | Parameter((value_type)-1)); |
338 | else | ||
339 |
2/4✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
|
24 | problem->setParameter("PathProjection/HessianBound", |
340 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
24 | Parameter(traits::K)); |
341 | |||
342 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
24 | typename traits::ProjPtr_t projector = |
343 | traits::Proj_t::create(problem, traits::projection_step); | ||
344 | |||
345 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | std::cout << "========================================\n"; |
346 | |||
347 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
24 | Configuration_t q1(dev->configSize()); |
348 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
24 | Configuration_t q2(dev->configSize()); |
349 |
2/2✓ Branch 1 taken 96 times.
✓ Branch 2 taken 12 times.
|
216 | for (int i = 0; i < traits::NB_CONFS; ++i) { |
350 | // HPP_DEFINE_TIMECOUNTER(projector); | ||
351 |
3/6✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
|
192 | traits::make_conf(q1, q2, i); |
352 |
3/6✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
|
384 | PathPtr_t path = (*problem->steeringMethod())(q1, q2); |
353 | |||
354 | 192 | PathPtr_t projection; | |
355 | // Averaging the projection | ||
356 | bool success; | ||
357 |
2/2✓ Branch 0 taken 192 times.
✓ Branch 1 taken 96 times.
|
576 | for (int j = 0; j < 2; ++j) { |
358 | // HPP_START_TIMECOUNTER (projector); | ||
359 |
1/2✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
|
384 | success = projector->apply(path, projection); |
360 | // HPP_STOP_TIMECOUNTER (projector); | ||
361 | // HPP_DISPLAY_LAST_TIMECOUNTER (projector); | ||
362 | } | ||
363 |
4/8✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
|
192 | std::cout << traits::_proj << " " << traits::_func << ": projection of " |
364 |
5/10✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 96 times.
✗ Branch 14 not taken.
|
384 | << q1.transpose() << " -> " << q2.transpose() << " " |
365 |
5/8✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 41 times.
✓ Branch 4 taken 55 times.
✓ Branch 6 taken 96 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 96 times.
✗ Branch 10 not taken.
|
192 | << (success ? "succeeded." : "failed.") << std::endl; |
366 | // HPP_STREAM_TIMECOUNTER (std::cout, projector) << std::endl; | ||
367 | // displayPaths (path, projection, func); | ||
368 | } | ||
369 | } | ||
370 | 12 | } | |
371 |