Directory: | ./ |
---|---|
File: | src/path-projector/recursive-hermite.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 91 | 132 | 68.9% |
Branches: | 115 | 283 | 40.6% |
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 | #include "hpp/core/path-projector/recursive-hermite.hh" | ||
30 | |||
31 | #include <hpp/core/config-projector.hh> | ||
32 | #include <hpp/core/interpolated-path.hh> | ||
33 | #include <hpp/core/path-vector.hh> | ||
34 | #include <hpp/core/path/hermite.hh> | ||
35 | #include <hpp/core/steering-method/hermite.hh> | ||
36 | #include <hpp/util/timer.hh> | ||
37 | #include <limits> | ||
38 | #include <queue> | ||
39 | #include <stack> | ||
40 | |||
41 | namespace hpp { | ||
42 | namespace core { | ||
43 | namespace pathProjector { | ||
44 | 4 | RecursiveHermitePtr_t RecursiveHermite::create( | |
45 | const DistancePtr_t& distance, const SteeringMethodPtr_t& steeringMethod, | ||
46 | value_type step) { | ||
47 | 4 | value_type beta = steeringMethod->problem() | |
48 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
8 | ->getParameter("PathProjection/RecursiveHermite/Beta") |
49 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | .floatValue(); |
50 | hppDout(info, "beta is " << beta); | ||
51 | return RecursiveHermitePtr_t( | ||
52 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
8 | new RecursiveHermite(distance, steeringMethod, step, beta)); |
53 | } | ||
54 | |||
55 | 4 | RecursiveHermitePtr_t RecursiveHermite::create(const ProblemConstPtr_t& problem, | |
56 | const value_type& step) { | ||
57 |
1/2✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
8 | return create(problem->distance(), problem->steeringMethod(), step); |
58 | } | ||
59 | |||
60 | 4 | RecursiveHermite::RecursiveHermite(const DistancePtr_t& distance, | |
61 | const SteeringMethodPtr_t& steeringMethod, | ||
62 | 4 | const value_type& M, const value_type& beta) | |
63 | 4 | : PathProjector(distance, steeringMethod, false), M_(M), beta_(beta) { | |
64 | // beta should be between 0.5 and 1. | ||
65 |
2/4✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
|
4 | if (beta_ < 0.5 || 1 < beta_) |
66 | ✗ | throw std::invalid_argument("Beta should be between 0.5 and 1"); | |
67 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
|
4 | if (!HPP_DYNAMIC_PTR_CAST(hpp::core::steeringMethod::Hermite, steeringMethod)) |
68 | ✗ | throw std::invalid_argument("Steering method should be of type Hermite"); | |
69 | 4 | } | |
70 | |||
71 | 64 | bool RecursiveHermite::impl_apply(const PathPtr_t& path, | |
72 | PathPtr_t& proj) const { | ||
73 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
|
64 | assert(path); |
74 | 64 | bool success = false; | |
75 | 64 | PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path); | |
76 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
64 | if (!pv) { |
77 |
5/12✓ Branch 3 taken 64 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 64 times.
✓ Branch 14 taken 64 times.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 64 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
64 | if (!path->constraints() || !path->constraints()->configProjector()) { |
78 | ✗ | proj = path; | |
79 | ✗ | success = true; | |
80 | } else { | ||
81 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
64 | success = project(path, proj); |
82 | } | ||
83 | } else { | ||
84 | PathVectorPtr_t res = | ||
85 | ✗ | PathVector::create(pv->outputSize(), pv->outputDerivativeSize()); | |
86 | ✗ | PathPtr_t part; | |
87 | ✗ | success = true; | |
88 | ✗ | for (size_t i = 0; i < pv->numberPaths(); i++) { | |
89 | ✗ | if (!apply(pv->pathAtRank(i), part)) { | |
90 | // We add the path only if part is not NULL and: | ||
91 | // - either its length is not zero, | ||
92 | // - or it's not the first one. | ||
93 | ✗ | if (part && (part->length() > 0 || i == 0)) { | |
94 | ✗ | res->appendPath(part); | |
95 | } | ||
96 | ✗ | success = false; | |
97 | ✗ | break; | |
98 | } | ||
99 | ✗ | res->appendPath(part); | |
100 | } | ||
101 | ✗ | proj = res; | |
102 | } | ||
103 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
|
64 | assert(proj); |
104 |
5/10✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 64 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 64 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 64 times.
|
64 | assert((proj->initial() - path->initial()).isZero()); |
105 |
11/20✓ Branch 0 taken 28 times.
✓ Branch 1 taken 36 times.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 28 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 28 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 28 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 28 times.
✓ Branch 20 taken 28 times.
✓ Branch 21 taken 36 times.
✓ Branch 23 taken 28 times.
✓ Branch 24 taken 36 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
|
64 | assert(!success || (proj->end() - path->end()).isZero()); |
106 | 64 | return success; | |
107 | 64 | } | |
108 | |||
109 | 64 | bool RecursiveHermite::project(const PathPtr_t& path, PathPtr_t& proj) const { | |
110 | 64 | ConstraintSetPtr_t constraints = path->constraints(); | |
111 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
|
64 | if (!constraints) { |
112 | ✗ | proj = path; | |
113 | ✗ | return true; | |
114 | } | ||
115 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | const Configuration_t q1 = path->initial(); |
116 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | const Configuration_t q2 = path->end(); |
117 |
3/6✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 64 times.
|
64 | if (!constraints->isSatisfied(q2)) return false; |
118 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | const ConfigProjectorPtr_t& cp = constraints->configProjector(); |
119 |
4/8✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 64 times.
|
64 | if (!cp || cp->dimension() == 0) { |
120 | ✗ | proj = path; | |
121 | ✗ | return true; | |
122 | } | ||
123 | |||
124 | 64 | steeringMethod_->constraints(constraints); | |
125 | |||
126 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | const value_type thr = 2 * cp->errorThreshold() / M_; |
127 | |||
128 | 64 | std::vector<HermitePtr_t> ps; | |
129 | 64 | HermitePtr_t p = HPP_DYNAMIC_PTR_CAST(Hermite, path); | |
130 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
|
64 | if (!p) { |
131 | ✗ | InterpolatedPathPtr_t ip = HPP_DYNAMIC_PTR_CAST(InterpolatedPath, path); | |
132 | ✗ | if (ip) { | |
133 | typedef InterpolatedPath::InterpolationPoints_t IPs_t; | ||
134 | ✗ | const IPs_t& ips = ip->interpolationPoints(); | |
135 | ✗ | ps.reserve(ips.size() - 1); | |
136 | ✗ | IPs_t::const_iterator _ip1 = ips.begin(); | |
137 | ✗ | std::advance(_ip1, 1); | |
138 | ✗ | for (IPs_t::const_iterator _ip0 = ips.begin(); _ip1 != ips.end(); | |
139 | ✗ | ++_ip0) { | |
140 | ✗ | ps.push_back( | |
141 | ✗ | HPP_DYNAMIC_PTR_CAST(Hermite, steer(_ip0->second, _ip1->second))); | |
142 | ✗ | ++_ip1; | |
143 | } | ||
144 | } else { | ||
145 | ✗ | p = HPP_DYNAMIC_PTR_CAST(Hermite, steer(path->initial(), path->end())); | |
146 | ✗ | ps.push_back(p); | |
147 | } | ||
148 | ✗ | } else { | |
149 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
64 | ps.push_back(p); |
150 | } | ||
151 | PathVectorPtr_t res = | ||
152 |
1/2✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
|
64 | PathVector::create(path->outputSize(), path->outputDerivativeSize()); |
153 | 64 | bool success = true; | |
154 |
2/2✓ Branch 1 taken 64 times.
✓ Branch 2 taken 28 times.
|
92 | for (std::size_t i = 0; i < ps.size(); ++i) { |
155 | 64 | p = ps[i]; | |
156 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | p->computeHermiteLength(); |
157 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 64 times.
|
64 | if (p->hermiteLength() < thr) { |
158 | ✗ | res->appendPath(p); | |
159 | ✗ | continue; | |
160 | } | ||
161 | PathVectorPtr_t r = | ||
162 |
1/2✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
|
64 | PathVector::create(path->outputSize(), path->outputDerivativeSize()); |
163 |
3/6✓ Branch 3 taken 64 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 64 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
|
64 | std::cout << p->hermiteLength() << " / " << thr << " : " |
164 |
3/6✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 64 times.
✗ Branch 12 not taken.
|
64 | << path->constraints()->name() << std::endl; |
165 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
64 | success = recurse(p, r, thr); |
166 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | res->concatenate(r); |
167 |
2/2✓ Branch 0 taken 36 times.
✓ Branch 1 taken 28 times.
|
64 | if (!success) break; |
168 |
2/2✓ Branch 1 taken 28 times.
✓ Branch 2 taken 36 times.
|
64 | } |
169 | #if HPP_ENABLE_BENCHMARK | ||
170 | value_type min = std::numeric_limits<value_type>::max(), max = 0, | ||
171 | totalLength = 0; | ||
172 | const size_t nbPaths = res->numberPaths(); | ||
173 | for (std::size_t i = 0; i < nbPaths; ++i) { | ||
174 | PathPtr_t curP = res->pathAtRank(i); | ||
175 | const value_type l = d(curP->initial(), curP->end()); | ||
176 | if (l < min) | ||
177 | min = l; | ||
178 | else if (l > max) | ||
179 | max = l; | ||
180 | totalLength += l; | ||
181 | } | ||
182 | hppBenchmark("Hermite path: " | ||
183 | << nbPaths << ", [ " << min << ", " | ||
184 | << (nbPaths == 0 ? 0 : totalLength / (value_type)nbPaths) << ", " | ||
185 | << max << "]"); | ||
186 | #endif | ||
187 |
2/2✓ Branch 0 taken 28 times.
✓ Branch 1 taken 36 times.
|
64 | if (success) { |
188 | 28 | proj = res; | |
189 | 28 | return true; | |
190 | } | ||
191 | 36 | const value_type tmin = path->timeRange().first; | |
192 |
1/3✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
36 | switch (res->numberPaths()) { |
193 | 36 | case 0: | |
194 |
2/4✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
|
36 | proj = path->extract(std::make_pair(tmin, tmin)); |
195 | 36 | break; | |
196 | ✗ | case 1: | |
197 | ✗ | proj = res->pathAtRank(0); | |
198 | ✗ | break; | |
199 | ✗ | default: | |
200 | ✗ | proj = res; | |
201 | ✗ | break; | |
202 | } | ||
203 | 36 | return false; | |
204 | 64 | } | |
205 | |||
206 | 1852600 | bool RecursiveHermite::recurse(const HermitePtr_t& path, PathVectorPtr_t& proj, | |
207 | const value_type& acceptThr) const { | ||
208 |
2/2✓ Branch 2 taken 926296 times.
✓ Branch 3 taken 926304 times.
|
1852600 | if (path->hermiteLength() < acceptThr) { |
209 | // TODO this does not work because it is not possible to remove | ||
210 | // constraints from a path. | ||
211 | // proj->appendPath (path->copy (ConstraintSetPtr_t())); | ||
212 |
1/2✓ Branch 3 taken 926296 times.
✗ Branch 4 not taken.
|
926296 | proj->appendPath(path); |
213 | 926296 | return true; | |
214 | } else { | ||
215 | 926304 | const value_type t = 0.5; // path->timeRange().first + path->length() / 2; | |
216 | bool success; | ||
217 |
1/2✓ Branch 2 taken 926304 times.
✗ Branch 3 not taken.
|
926304 | const Configuration_t q1(path->eval(t, success)); |
218 |
2/2✓ Branch 0 taken 36 times.
✓ Branch 1 taken 926268 times.
|
926304 | if (!success) { |
219 | hppDout(info, "RHP stopped because it could not project a configuration"); | ||
220 | 36 | return false; | |
221 | } | ||
222 |
1/2✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
|
926268 | const Configuration_t q0 = path->initial(); |
223 |
1/2✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
|
926268 | const Configuration_t q2 = path->end(); |
224 | // Velocities must be divided by two because each half is rescale | ||
225 | // from [0, 0.5] to [0, 1] | ||
226 |
3/6✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 926268 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 926268 times.
✗ Branch 9 not taken.
|
926268 | const vector_t vHalf = path->velocity(t) / 2; |
227 | |||
228 |
3/6✓ Branch 1 taken 926268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 926268 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 926268 times.
✗ Branch 8 not taken.
|
1852536 | HermitePtr_t left = HPP_DYNAMIC_PTR_CAST(Hermite, steer(q0, q1)); |
229 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 926268 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
926268 | if (!left) throw std::runtime_error("Not an path::Hermite"); |
230 |
4/8✓ Branch 3 taken 926268 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 926268 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 926268 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 926268 times.
✗ Branch 13 not taken.
|
926268 | left->v0(path->v0() / 2); |
231 |
2/4✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 926268 times.
✗ Branch 6 not taken.
|
926268 | left->v1(vHalf); |
232 |
1/2✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
|
926268 | left->computeHermiteLength(); |
233 | |||
234 |
3/6✓ Branch 1 taken 926268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 926268 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 926268 times.
✗ Branch 8 not taken.
|
1852536 | HermitePtr_t right = HPP_DYNAMIC_PTR_CAST(Hermite, steer(q1, q2)); |
235 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 926268 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
926268 | if (!right) throw std::runtime_error("Not an path::Hermite"); |
236 |
2/4✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 926268 times.
✗ Branch 6 not taken.
|
926268 | right->v0(vHalf); |
237 |
4/8✓ Branch 3 taken 926268 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 926268 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 926268 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 926268 times.
✗ Branch 13 not taken.
|
926268 | right->v1(path->v1() / 2); |
238 |
1/2✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
|
926268 | right->computeHermiteLength(); |
239 | |||
240 | 926268 | const value_type stopThr = beta_ * path->hermiteLength(); | |
241 | 926268 | bool lStop = (left->hermiteLength() > stopThr); | |
242 | 926268 | bool rStop = (right->hermiteLength() > stopThr); | |
243 |
2/4✓ Branch 0 taken 926268 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 926268 times.
|
926268 | bool stop = rStop || lStop; |
244 | // This is the inverse of the condition in the RSS paper. Is there a typo in | ||
245 | // the paper ? if (std::max (left->hermiteLength(), right->hermiteLength()) | ||
246 | // > beta * path->hermiteLength()) { | ||
247 | if (stop) { | ||
248 | hppDout(info, "RHP stopped: " << path->hermiteLength() << " * " << beta_ | ||
249 | << " -> " << left->hermiteLength() << " / " | ||
250 | << right->hermiteLength()); | ||
251 | } | ||
252 |
4/8✓ Branch 0 taken 926268 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 926268 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 926268 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 926268 times.
|
926268 | if (lStop || !recurse(left, proj, acceptThr)) return false; |
253 |
4/8✓ Branch 0 taken 926268 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 926268 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 926268 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 926268 times.
|
926268 | if (stop || !recurse(right, proj, acceptThr)) return false; |
254 | |||
255 | 926268 | return true; | |
256 | 926304 | } | |
257 | } | ||
258 | } // namespace pathProjector | ||
259 | } // namespace core | ||
260 | } // namespace hpp | ||
261 |