GCC Code Coverage Report

Directory: ./
File: src/path-projector/recursive-hermite.cc
Date: 2025-03-10 11:18:21
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