Directory: | ./ |
---|---|
File: | src/diffusing-planner.cc |
Date: | 2024-08-10 11:29:48 |
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 |