| Directory: | ./ |
|---|---|
| File: | src/path-optimization/random-shortcut.cc |
| Date: | 2025-03-10 11:18:21 |
| 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 |