GCC Code Coverage Report

Directory: ./
File: src/diffusing-planner.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 125 160 78.1%
Branches: 172 392 43.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
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 <hpp/core/config-projector.hh>
31 #include <hpp/core/configuration-shooter.hh>
32 #include <hpp/core/diffusing-planner.hh>
33 #include <hpp/core/edge.hh>
34 #include <hpp/core/node.hh>
35 #include <hpp/core/path-projector.hh>
36 #include <hpp/core/path-validation.hh>
37 #include <hpp/core/path.hh>
38 #include <hpp/core/problem-target/goal-configurations.hh>
39 #include <hpp/core/problem.hh>
40 #include <hpp/core/roadmap.hh>
41 #include <hpp/core/steering-method.hh>
42 #include <hpp/pinocchio/configuration.hh>
43 #include <hpp/pinocchio/device.hh>
44 #include <hpp/util/debug.hh>
45 #include <hpp/util/timer.hh>
46 #include <iterator>
47 #include <pinocchio/math/quaternion.hpp>
48 #include <tuple>
49
50 namespace hpp {
51 namespace core {
52 namespace {
53 using pinocchio::displayConfig;
54
55 HPP_DEFINE_TIMECOUNTER(oneStep);
56 HPP_DEFINE_TIMECOUNTER(extend);
57 HPP_DEFINE_TIMECOUNTER(tryConnect);
58 HPP_DEFINE_TIMECOUNTER(validatePath);
59 HPP_DEFINE_TIMECOUNTER(delayedEdges);
60 } // namespace
61
62 7 DiffusingPlannerPtr_t DiffusingPlanner::createWithRoadmap(
63 const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap) {
64
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 DiffusingPlanner* ptr = new DiffusingPlanner(problem, roadmap);
65 7 return DiffusingPlannerPtr_t(ptr);
66 }
67
68 ✗ DiffusingPlannerPtr_t DiffusingPlanner::create(
69 const ProblemConstPtr_t& problem) {
70 ✗ DiffusingPlanner* ptr = new DiffusingPlanner(problem);
71 ✗ return DiffusingPlannerPtr_t(ptr);
72 }
73
74 ✗ DiffusingPlanner::DiffusingPlanner(const ProblemConstPtr_t& problem)
75 : PathPlanner(problem),
76 ✗ configurationShooter_(problem->configurationShooter()),
77 ✗ qProj_(problem->robot()->configSize()) {}
78
79 7 DiffusingPlanner::DiffusingPlanner(const ProblemConstPtr_t& problem,
80 7 const RoadmapPtr_t& roadmap)
81 : PathPlanner(problem, roadmap),
82 7 configurationShooter_(problem->configurationShooter()),
83
2/4
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 14 qProj_(problem->robot()->configSize()) {}
84
85 ✗ void DiffusingPlanner::init(const DiffusingPlannerWkPtr_t& weak) {
86 ✗ PathPlanner::init(weak);
87 ✗ weakPtr_ = weak;
88 }
89
90 31 bool belongs(ConfigurationIn_t q, const Nodes_t& nodes) {
91
2/2
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 25 times.
 34 for (Nodes_t::const_iterator itNode = nodes.begin(); itNode != nodes.end();
92 3 ++itNode) {
93
4/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✓ Branch 8 taken 3 times.
 9 if ((*itNode)->configuration() == q) return true;
94 }
95 25 return false;
96 }
97
98 48 PathPtr_t DiffusingPlanner::extend(const NodePtr_t& near,
99 ConfigurationIn_t target) {
100
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 const SteeringMethodPtr_t& sm(problem()->steeringMethod());
101 48 const ConstraintSetPtr_t& constraints(sm->constraints());
102
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 if (constraints) {
103
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 48 ConfigProjectorPtr_t configProjector(constraints->configProjector());
104
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 48 times.
 48 if (configProjector) {
105 ✗ assert(isNormalized(problem()->robot(), target,
106 PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
107 ✗ assert(isNormalized(problem()->robot(), near->configuration(),
108 PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
109 ✗ configProjector->projectOnKernel(near->configuration(), target, qProj_);
110 ✗ assert(isNormalized(problem()->robot(), qProj_,
111 PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
112 } else {
113
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 qProj_ = target;
114 }
115
3/6
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 48 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 48 times.
 48 if (!constraints->apply(qProj_)) {
116 ✗ return PathPtr_t();
117 }
118
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 } else {
119 ✗ qProj_ = target;
120 }
121
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 48 times.
 48 assert(!qProj_.hasNaN());
122 // Here, qProj_ is a configuration that satisfies the constraints
123 // or target if there are no constraints.
124
4/8
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 48 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 48 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 48 times.
✗ Branch 12 not taken.
 96 PathPtr_t path = (*sm)(near->configuration(), qProj_);
125
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 48 times.
 48 if (!path) {
126 ✗ return PathPtr_t();
127 }
128 value_type stepLength =
129
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 problem()
130
2/4
✓ Branch 3 taken 48 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 48 times.
✗ Branch 7 not taken.
 96 ->getParameter("DiffusingPlanner/extensionStepLength")
131
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 .floatValue();
132
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 48 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 48 times.
 48 if (stepLength > 0 && path->length() > stepLength) {
133 ✗ value_type t0 = path->timeRange().first;
134 ✗ path = path->extract(t0, t0 + stepLength);
135 }
136
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 PathProjectorPtr_t pp = problem()->pathProjector();
137
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 48 times.
 48 if (pp) {
138 ✗ PathPtr_t proj;
139 ✗ pp->apply(path, proj);
140 ✗ return proj;
141 }
142 48 return path;
143 48 }
144
145 7 void DiffusingPlanner::startSolve() {
146
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Parent_t::startSolve();
147
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problemTarget::GoalConfigurationsPtr_t gc(HPP_DYNAMIC_PTR_CAST(
148 7 problemTarget::GoalConfigurations, problem()->target()));
149
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!gc) {
150 ✗ throw std::logic_error(
151 "DiffusingPlanner only accepts goals defined "
152 ✗ "by goal configurations.");
153 }
154 7 }
155
156 /// This method performs one step of RRT extension as follows
157 /// 1. a random configuration "q_rand" is shot,
158 /// 2. for each connected component,
159 /// 2.1. the closest node "q_near" is chosen,
160 /// 2.2. "q_rand" is projected first on the tangent space of the
161 /// non-linear constraint at "q_near", this projection yields
162 /// "q_tmp", then "q_tmp" is projected on the non-linear constraint
163 /// manifold as "q_proj" (method extend)
164 /// 2.3. the steering method is called between "q_near" and "q_proj" that
165 /// returns "path",
166 /// 2.4. a valid connected part of "path", called "validPath" starting at
167 /// "q_near" is extracted, if "path" is valid (collision free),
168 /// the full "path" is returned, "q_new" is the end configuration of
169 /// "validPath",
170 /// 2.5 a new node containing "q_new" is added to the connected
171 /// component and a new edge is added between nodes containing
172 /// "q_near" and "q_new".
173 /// 3. Try to connect new nodes together using the steering method and
174 /// the current PathValidation instance.
175 ///
176 /// Note that edges are actually added to the roadmap after step 2 in order
177 /// to avoid iterating on the list of connected components while modifying
178 /// this list.
179
180 24 void DiffusingPlanner::oneStep() {
181 HPP_START_TIMECOUNTER(oneStep);
182
183 value_type stepRatio =
184
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 problem()
185
2/4
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
 48 ->getParameter("DiffusingPlanner/extensionStepRatio")
186
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 .floatValue();
187
188 typedef std::tuple<NodePtr_t, Configuration_t, PathPtr_t> DelayedEdge_t;
189 typedef std::vector<DelayedEdge_t> DelayedEdges_t;
190 24 DelayedEdges_t delayedEdges;
191
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 DevicePtr_t robot(problem()->robot());
192
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 PathValidationPtr_t pathValidation(problem()->pathValidation());
193 24 Nodes_t newNodes, nearestNeighbors;
194 24 PathPtr_t validPath, path;
195 // Pick a random node
196
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 Configuration_t q_rand;
197
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 configurationShooter_->shoot(q_rand);
198 //
199 // First extend each connected component toward q_rand
200 //
201 24 for (ConnectedComponents_t::const_iterator itcc =
202
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
 24 roadmap()->connectedComponents().begin();
203
4/6
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 48 times.
✓ Branch 11 taken 24 times.
 72 itcc != roadmap()->connectedComponents().end(); ++itcc) {
204 // Find nearest node in roadmap
205 value_type distance;
206
3/6
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 48 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 48 times.
✗ Branch 10 not taken.
 48 NodePtr_t near = roadmap()->nearestNode(q_rand, *itcc, distance);
207
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 nearestNeighbors.push_back(near);
208 HPP_START_TIMECOUNTER(extend);
209
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 48 path = extend(near, q_rand);
210 HPP_STOP_TIMECOUNTER(extend);
211
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 if (path) {
212 48 PathValidationReportPtr_t report;
213 HPP_START_TIMECOUNTER(validatePath);
214
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 48 bool pathValid = pathValidation->validate(path, false, validPath, report);
215 HPP_STOP_TIMECOUNTER(validatePath);
216 // Insert new path to q_near in roadmap
217 48 value_type t_final = validPath->timeRange().second;
218
2/2
✓ Branch 2 taken 35 times.
✓ Branch 3 taken 13 times.
 48 if (t_final != path->timeRange().first) {
219
3/6
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 35 if (!pathValid && stepRatio > 0 && stepRatio < 1.) {
220 ✗ value_type t0 = validPath->timeRange().first;
221 validPath =
222 ✗ validPath->extract(t0, t0 + validPath->length() * stepRatio);
223 }
224
1/2
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
 35 Configuration_t q_new(validPath->end());
225
10/14
✓ Branch 0 taken 31 times.
✓ Branch 1 taken 4 times.
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 25 times.
✓ Branch 9 taken 6 times.
✓ Branch 10 taken 31 times.
✓ Branch 11 taken 4 times.
✓ Branch 13 taken 29 times.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 35 if (!pathValid || !belongs(q_new, newNodes)) {
226 29 newNodes.push_back(
227
4/8
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 29 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 29 times.
✗ Branch 13 not taken.
 29 roadmap()->addNodeAndEdges(near, q_new, validPath));
228 } else {
229 // Store edges to add for later insertion.
230 // Adding edges while looping on connected components is indeed
231 // not recommended.
232
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 delayedEdges.push_back(DelayedEdge_t(near, q_new, validPath));
233 }
234 35 }
235 48 }
236 }
237 // Insert delayed edges
238 HPP_START_TIMECOUNTER(delayedEdges);
239
2/2
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 24 times.
 30 for (const auto& edge : delayedEdges) {
240 6 const NodePtr_t& near = std::get<0>(edge);
241
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 Configuration_t q_new = std::get<1>(edge);
242 6 const PathPtr_t& validPath = std::get<2>(edge);
243
3/6
✓ 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.
 6 NodePtr_t newNode = roadmap()->addNode(q_new);
244
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 roadmap()->addEdge(near, newNode, validPath);
245
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
 6 roadmap()->addEdge(newNode, near, validPath->reverse());
246 6 }
247 HPP_STOP_TIMECOUNTER(delayedEdges);
248
249 //
250 // Second, try to connect new nodes together
251 //
252 HPP_START_TIMECOUNTER(tryConnect);
253
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 const SteeringMethodPtr_t& sm(problem()->steeringMethod());
254
2/2
✓ Branch 5 taken 29 times.
✓ Branch 6 taken 24 times.
 53 for (Nodes_t::const_iterator itn1 = newNodes.begin(); itn1 != newNodes.end();
255 29 ++itn1) {
256 /// Try connecting to the other new nodes.
257
3/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 5 times.
✓ Branch 7 taken 29 times.
 34 for (Nodes_t::const_iterator itn2 = std::next(itn1); itn2 != newNodes.end();
258 5 ++itn2) {
259
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 Configuration_t q1((*itn1)->configuration());
260
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 Configuration_t q2((*itn2)->configuration());
261
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
 5 assert(q1 != q2);
262
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 path = (*sm)(q1, q2);
263
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 if (!path) continue;
264
265
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 PathProjectorPtr_t pp = problem()->pathProjector();
266
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 if (pp) {
267 ✗ PathPtr_t proj;
268 // If projection failed, continue
269 ✗ if (!pp->apply(path, proj)) continue;
270 ✗ path = proj;
271 }
272
273 5 PathValidationReportPtr_t report;
274 HPP_START_TIMECOUNTER(validatePath);
275
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 bool valid = pathValidation->validate(path, false, validPath, report);
276 HPP_STOP_TIMECOUNTER(validatePath);
277
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4 times.
 5 if (valid) {
278
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 roadmap()->addEdge(*itn1, *itn2, path);
279
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 roadmap()->addEdge(*itn2, *itn1, path->reverse());
280
6/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✓ Branch 8 taken 2 times.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 2 times.
 4 } else if (validPath && validPath->length() > 0) {
281 // A -> B
282
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Configuration_t cfg(validPath->end());
283
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 roadmap()->addNodeAndEdges(*itn1, cfg, validPath);
284 2 }
285
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 }
286 /// Try connecting this node to the list of nearest neighbors.
287
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
 29 const ConnectedComponentPtr_t& cc1 = (*itn1)->connectedComponent();
288
2/4
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
 29 Configuration_t q1((*itn1)->configuration());
289 29 for (Nodes_t::const_iterator itn2 = nearestNeighbors.begin();
290
2/2
✓ Branch 4 taken 58 times.
✓ Branch 5 taken 29 times.
 87 itn2 != nearestNeighbors.end(); ++itn2) {
291
3/4
✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 37 times.
✓ Branch 7 taken 21 times.
 58 if (cc1 == (*itn2)->connectedComponent()) continue;
292
2/4
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
 21 Configuration_t q2((*itn2)->configuration());
293
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
 21 assert(q1 != q2);
294
3/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 21 times.
✗ Branch 9 not taken.
 21 path = (*sm)(q1, q2);
295
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
 21 if (!path) continue;
296
297
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 PathProjectorPtr_t pp = problem()->pathProjector();
298
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
 21 if (pp) {
299 ✗ PathPtr_t proj;
300 // If projection failed, continue
301 ✗ if (!pp->apply(path, proj)) continue;
302 ✗ path = proj;
303 }
304
305 21 PathValidationReportPtr_t report;
306 HPP_START_TIMECOUNTER(validatePath);
307
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 bool valid = pathValidation->validate(path, false, validPath, report);
308 HPP_STOP_TIMECOUNTER(validatePath);
309
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
 21 if (valid) {
310 ✗ roadmap()->addEdge(*itn1, *itn2, path);
311 ✗ roadmap()->addEdge(*itn2, *itn1, path->reverse());
312
6/8
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 18 times.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 18 times.
✓ Branch 10 taken 3 times.
 21 } else if (validPath && validPath->length() > 0) {
313 // A -> B
314
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 Configuration_t cfg(validPath->end());
315
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 roadmap()->addNodeAndEdges(*itn1, cfg, validPath);
316 18 }
317
2/4
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
 21 }
318 29 }
319 HPP_STOP_TIMECOUNTER(tryConnect);
320
321 HPP_STOP_TIMECOUNTER(oneStep);
322
323 HPP_DISPLAY_TIMECOUNTER(oneStep);
324 HPP_DISPLAY_TIMECOUNTER(extend);
325 HPP_DISPLAY_TIMECOUNTER(validatePath);
326 HPP_DISPLAY_TIMECOUNTER(delayedEdges);
327 HPP_DISPLAY_TIMECOUNTER(tryConnect);
328 24 }
329
330 ✗ void DiffusingPlanner::configurationShooter(
331 const ConfigurationShooterPtr_t& shooter) {
332 ✗ configurationShooter_ = shooter;
333 }
334
335 18 HPP_START_PARAMETER_DECLARATION(DiffusingPlanner)
336
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(
337 Parameter::FLOAT, "DiffusingPlanner/extensionStepLength",
338 "Extension step length. "
339 "Not used if negative.",
340
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(-1.)));
341
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(
342 Parameter::FLOAT, "DiffusingPlanner/extensionStepRatio",
343 "When path from q_near to q_rand is in collision, keep only this "
344 "amount of the valid part. "
345 "Should be in ]0,1[. Not used if negative.",
346
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(-1.)));
347 18 HPP_END_PARAMETER_DECLARATION(DiffusingPlanner)
348 } // namespace core
349 } // namespace hpp
350