Directory: | ./ |
---|---|
File: | src/path-optimization/random-shortcut.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 4 | 89 | 4.5% |
Branches: | 5 | 136 | 3.7% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2014 CNRS | ||
3 | // Authors: Florent Lamiraux, Joseph Mirabel | ||
4 | // | ||
5 | |||
6 | // Redistribution and use in source and binary forms, with or without | ||
7 | // modification, are permitted provided that the following conditions are | ||
8 | // met: | ||
9 | // | ||
10 | // 1. Redistributions of source code must retain the above copyright | ||
11 | // notice, this list of conditions and the following disclaimer. | ||
12 | // | ||
13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
14 | // notice, this list of conditions and the following disclaimer in the | ||
15 | // documentation and/or other materials provided with the distribution. | ||
16 | // | ||
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
28 | // DAMAGE. | ||
29 | |||
30 | #include <cstdlib> | ||
31 | #include <deque> | ||
32 | #include <hpp/core/distance.hh> | ||
33 | #include <hpp/core/path-optimization/random-shortcut.hh> | ||
34 | #include <hpp/core/path-projector.hh> | ||
35 | #include <hpp/core/path-validation.hh> | ||
36 | #include <hpp/core/path-vector.hh> | ||
37 | #include <hpp/core/problem.hh> | ||
38 | #include <hpp/util/assertion.hh> | ||
39 | #include <hpp/util/debug.hh> | ||
40 | #include <limits> | ||
41 | |||
42 | namespace hpp { | ||
43 | namespace core { | ||
44 | namespace pathOptimization { | ||
45 | // Compute the length of a vector of paths assuming that each element | ||
46 | // is optimal for the given distance. | ||
47 | template <bool reEstimateLength = false> | ||
48 | struct _PathLength { | ||
49 | ✗ | static inline value_type run(const PathVectorPtr_t& path, | |
50 | const DistancePtr_t& distance) { | ||
51 | if (reEstimateLength) | ||
52 | return path->length(); | ||
53 | else { | ||
54 | ✗ | value_type result = 0; | |
55 | ✗ | for (std::size_t i = 0; i < path->numberPaths(); ++i) { | |
56 | ✗ | const PathPtr_t& element(path->pathAtRank(i)); | |
57 | ✗ | Configuration_t q1 = element->initial(); | |
58 | ✗ | Configuration_t q2 = element->end(); | |
59 | ✗ | result += (*distance)(q1, q2); | |
60 | } | ||
61 | ✗ | return result; | |
62 | } | ||
63 | } | ||
64 | }; | ||
65 | |||
66 | ✗ | RandomShortcutPtr_t RandomShortcut::create(const ProblemConstPtr_t& problem) { | |
67 | ✗ | RandomShortcut* ptr = new RandomShortcut(problem); | |
68 | ✗ | return RandomShortcutPtr_t(ptr); | |
69 | } | ||
70 | |||
71 | ✗ | RandomShortcut::RandomShortcut(const ProblemConstPtr_t& problem) | |
72 | ✗ | : PathOptimizer(problem) {} | |
73 | |||
74 | ✗ | PathVectorPtr_t RandomShortcut::optimize(const PathVectorPtr_t& path) { | |
75 | ✗ | monitorExecution(); | |
76 | |||
77 | using std::make_pair; | ||
78 | using std::numeric_limits; | ||
79 | ✗ | bool finished = false; | |
80 | value_type t[4]; | ||
81 | ✗ | Configuration_t q[4]; | |
82 | ✗ | q[0] = path->initial(); | |
83 | ✗ | q[1].resize(path->outputSize()), q[2].resize(path->outputSize()); | |
84 | ✗ | q[3] = path->end(); | |
85 | ✗ | PathVectorPtr_t tmpPath = path; | |
86 | |||
87 | // Maximal number of iterations without improvements | ||
88 | const std::size_t n = | ||
89 | ✗ | problem() | |
90 | ✗ | ->getParameter("PathOptimization/RandomShortcut/NumberOfLoops") | |
91 | ✗ | .intValue(); | |
92 | ✗ | std::size_t projectionError = n; | |
93 | ✗ | std::deque<value_type> length(n - 1, numeric_limits<value_type>::infinity()); | |
94 | ✗ | length.push_back(_PathLength<>::run(tmpPath, problem()->distance())); | |
95 | ✗ | PathVectorPtr_t result; | |
96 | |||
97 | ✗ | while (!shouldStop() && !finished && projectionError != 0) { | |
98 | ✗ | endIteration(); | |
99 | ✗ | t[0] = tmpPath->timeRange().first; | |
100 | ✗ | t[3] = tmpPath->timeRange().second; | |
101 | ✗ | bool error = !shootTimes(tmpPath, t[0], t[1], t[2], t[3]); | |
102 | ✗ | if (error) { | |
103 | ✗ | projectionError--; | |
104 | ✗ | continue; | |
105 | } | ||
106 | ✗ | for (int i = 1; i < 3; ++i) { | |
107 | ✗ | if (!tmpPath->eval(q[i], t[i])) { | |
108 | hppDout(error, | ||
109 | "Configuration at param " << t[i] << " could not be projected"); | ||
110 | ✗ | projectionError--; | |
111 | ✗ | error = true; | |
112 | ✗ | break; | |
113 | } | ||
114 | } | ||
115 | ✗ | if (error) continue; | |
116 | // Validate sub parts | ||
117 | bool valid[3]; | ||
118 | ✗ | PathPtr_t proj[3]; | |
119 | // Build and projects the path | ||
120 | ✗ | for (int i = 0; i < 3; ++i) proj[i] = steer(q[i], q[i + 1]); | |
121 | ✗ | if (!proj[0] && !proj[1] && !proj[2]) { | |
122 | hppDout(info, "Enable to create a valid path"); | ||
123 | ✗ | projectionError--; | |
124 | ✗ | continue; | |
125 | } | ||
126 | // validate the paths | ||
127 | ✗ | for (unsigned i = 0; i < 3; ++i) { | |
128 | ✗ | PathPtr_t validPart; | |
129 | ✗ | PathValidationReportPtr_t report; | |
130 | ✗ | if (!proj[i]) | |
131 | ✗ | valid[i] = false; | |
132 | else | ||
133 | ✗ | valid[i] = problem()->pathValidation()->validate(proj[i], false, | |
134 | validPart, report); | ||
135 | } | ||
136 | // Replace valid parts | ||
137 | result = | ||
138 | ✗ | PathVector::create(path->outputSize(), path->outputDerivativeSize()); | |
139 | try { | ||
140 | ✗ | for (int i = 0; i < 3; ++i) { | |
141 | ✗ | if (valid[i]) | |
142 | ✗ | result->appendPath(proj[i]); | |
143 | else | ||
144 | ✗ | result->concatenate( | |
145 | ✗ | tmpPath->extract(make_pair(t[i], t[i + 1]))->as<PathVector>()); | |
146 | } | ||
147 | ✗ | } catch (const projection_error& e) { | |
148 | hppDout(error, "Caught exception at with time " << t[1] << " and " << t[2] | ||
149 | << ": " << e.what()); | ||
150 | ✗ | projectionError--; | |
151 | ✗ | result = tmpPath; | |
152 | ✗ | continue; | |
153 | } | ||
154 | ✗ | value_type newLength = _PathLength<>::run(result, problem()->distance()); | |
155 | ✗ | if (length[n - 1] <= newLength) { | |
156 | hppDout(info, "the length would increase:" << length[n - 1] << " " | ||
157 | << newLength); | ||
158 | ✗ | result = tmpPath; | |
159 | ✗ | projectionError--; | |
160 | } else { | ||
161 | ✗ | length.push_back(newLength); | |
162 | ✗ | length.pop_front(); | |
163 | ✗ | finished = (length[0] - length[n - 1]) <= 1e-4 * length[n - 1]; | |
164 | hppDout(info, "length = " << length[n - 1]); | ||
165 | ✗ | tmpPath = result; | |
166 | ✗ | projectionError = n; | |
167 | } | ||
168 | } | ||
169 | ✗ | if (!result) return path; | |
170 | hppDout(info, "RandomShortcut:" << *result); | ||
171 | ✗ | for (std::size_t i = 0; i < result->numberPaths(); ++i) { | |
172 | ✗ | if (result->pathAtRank(i)->constraints()) | |
173 | hppDout(info, "At rank " << i << ", constraints are " | ||
174 | << *result->pathAtRank(i)->constraints()); | ||
175 | else | ||
176 | hppDout(info, "At rank " << i << ", no constraints"); | ||
177 | } | ||
178 | ✗ | return result; | |
179 | } | ||
180 | |||
181 | ✗ | bool RandomShortcut::shootTimes(const PathVectorPtr_t& /*current*/, | |
182 | const value_type& t0, value_type& t1, | ||
183 | value_type& t2, const value_type& t3) { | ||
184 | ✗ | value_type u2 = (t3 - t0) * rand() / RAND_MAX; | |
185 | ✗ | value_type u1 = (t3 - t0) * rand() / RAND_MAX; | |
186 | ✗ | if (u1 < u2) { | |
187 | ✗ | t1 = t0 + u1; | |
188 | ✗ | t2 = t0 + u2; | |
189 | } else { | ||
190 | ✗ | t1 = t0 + u2; | |
191 | ✗ | t2 = t0 + u1; | |
192 | } | ||
193 | ✗ | return true; | |
194 | } | ||
195 | |||
196 | // ----------- Declare parameters ------------------------------------- // | ||
197 | |||
198 | 18 | HPP_START_PARAMETER_DECLARATION(RandomShortcut) | |
199 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
200 | Parameter::INT, "PathOptimization/RandomShortcut/NumberOfLoops", | ||
201 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "Number of loops.", Parameter((size_type)5))); |
202 | 18 | HPP_END_PARAMETER_DECLARATION(RandomShortcut) | |
203 | } // namespace pathOptimization | ||
204 | } // namespace core | ||
205 | } // namespace hpp | ||
206 |