GCC Code Coverage Report

Directory: ./
File: src/path-planner/bi-rrt-star.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 8 317 2.5%
Branches: 15 704 2.1%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020 CNRS
3 // Authors: 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 <hpp/core/config-validations.hh>
31 #include <hpp/core/configuration-shooter.hh>
32 #include <hpp/core/edge.hh>
33 #include <hpp/core/path-planner/bi-rrt-star.hh>
34 #include <hpp/core/path-projector.hh>
35 #include <hpp/core/path-validation-report.hh>
36 #include <hpp/core/path-validation.hh>
37 #include <hpp/core/problem.hh>
38 #include <hpp/core/roadmap.hh>
39 #include <hpp/pinocchio/configuration.hh>
40 #include <hpp/pinocchio/liegroup-space.hh>
41 #include <queue>
42
43 namespace hpp {
44 namespace core {
45 namespace pathPlanner {
46 ✗ BiRrtStarPtr_t BiRrtStar::create(const ProblemConstPtr_t& problem) {
47 ✗ BiRrtStarPtr_t shPtr(new BiRrtStar(problem));
48 ✗ shPtr->init(shPtr);
49 ✗ return shPtr;
50 }
51
52 ✗ BiRrtStarPtr_t BiRrtStar::createWithRoadmap(const ProblemConstPtr_t& problem,
53 const RoadmapPtr_t& roadmap) {
54 ✗ BiRrtStarPtr_t shPtr(new BiRrtStar(problem, roadmap));
55 ✗ shPtr->init(shPtr);
56 ✗ return shPtr;
57 }
58
59 ✗ BiRrtStar::BiRrtStar(const ProblemConstPtr_t& problem)
60 : Parent_t(problem),
61 ✗ gamma_(1.),
62 ✗ extendMaxLength_(1.),
63 ✗ minimalPathLength_(1e-5),
64 ✗ toRoot_(2) {
65 ✗ maxIterations(100);
66 ✗ stopWhenProblemIsSolved(false);
67 }
68
69 ✗ BiRrtStar::BiRrtStar(const ProblemConstPtr_t& problem,
70 ✗ const RoadmapPtr_t& roadmap)
71 : Parent_t(problem, roadmap),
72 ✗ gamma_(1.),
73 ✗ extendMaxLength_(1.),
74 ✗ minimalPathLength_(1e-5),
75 ✗ toRoot_(2) {
76 ✗ maxIterations(100);
77 ✗ stopWhenProblemIsSolved(false);
78 }
79
80 ✗ void BiRrtStar::init(const BiRrtStarWkPtr_t& weak) {
81 ✗ Parent_t::init(weak);
82 ✗ weak_ = weak;
83 }
84
85 // ----------- Algorithm ---------------------------------------------- //
86
87 typedef std::pair<bool, PathPtr_t> ValidatedPath_t;
88
89 typedef std::map<NodePtr_t, EdgePtr_t> ParentMap_t;
90 ✗ value_type computeCost(const ParentMap_t& map, NodePtr_t n) {
91 typedef ParentMap_t::const_iterator It_t;
92 ✗ value_type c = 0;
93 ✗ for (It_t current = map.find(n); current->second;
94 ✗ current = map.find(current->second->from())) {
95 ✗ if (current == map.end())
96 ✗ throw std::logic_error(
97 "BiRRT*: This node has no parent. You cannot use BiRRT* from a "
98 ✗ "precomputed roadmap.");
99 ✗ c += current->second->path()->length();
100 }
101 ✗ return c;
102 }
103
104 /// \param map the parent map to update
105 /// \param n a roadmap node
106 /// \param e a roadmap edge that ends at \c n.
107 /// \note if \c e is NULL, then \c n is considered a root of the parent
108 /// map.
109 ✗ void setParent(ParentMap_t& map, NodePtr_t n, EdgePtr_t e, bool newNode) {
110 ✗ if (e) {
111 ✗ assert(e->to() == n);
112 ✗ if (map.find(e->from()) == map.end())
113 ✗ throw std::logic_error(
114 "BiRRT*: Could not find node from of edge in parent map. You cannot "
115 ✗ "use BiRRT* from a precomputed roadmap.");
116 }
117 ✗ if (newNode && map.count(n))
118 ✗ throw std::logic_error("BiRRT*: This node already exists in the roadmap.");
119 ✗ map[n] = e;
120 }
121
122 struct WeighedNode_t {
123 NodePtr_t node;
124 EdgePtr_t parent;
125 value_type cost;
126 ✗ bool operator<(const WeighedNode_t& other) const { return cost < other.cost; }
127 ✗ WeighedNode_t(NodePtr_t node, EdgePtr_t parent, value_type cost)
128 ✗ : node(node), parent(parent), cost(cost) {}
129 };
130 typedef std::priority_queue<WeighedNode_t> Queue_t;
131
132 ✗ ParentMap_t computeParentMap(NodePtr_t root) {
133 typedef std::map<NodePtr_t, WeighedNode_t> Visited_t;
134 typedef Visited_t::iterator ItV_t;
135 ✗ Visited_t visited;
136
137 ✗ Queue_t queue;
138 ✗ queue.push(WeighedNode_t(root, EdgePtr_t(), 0));
139
140 ✗ while (!queue.empty()) {
141 ✗ WeighedNode_t current(queue.top());
142 ✗ queue.pop();
143
144 std::pair<ItV_t, bool> res(
145 ✗ visited.insert(std::make_pair(current.node, current)));
146 ✗ bool addChildren(res.second);
147 ✗ if (!addChildren) {
148 // Not inserted because already visited. Check if path is better.
149 // Normally, it is not possible that the children of current are
150 // visited before all the best way of reaching current is found.
151 ✗ if (res.first->second.cost > current.cost) {
152 ✗ res.first->second.cost = current.cost;
153 ✗ res.first->second.parent = current.parent;
154 // Re-add to priority queue.
155 ✗ addChildren = true;
156 }
157 }
158 ✗ if (addChildren) {
159 ✗ const Edges_t& edges = current.node->outEdges();
160 ✗ for (Edges_t::const_iterator _edge = edges.begin(); _edge != edges.end();
161 ✗ ++_edge) {
162 ✗ EdgePtr_t edge(*_edge);
163 ✗ queue.push(WeighedNode_t(edge->to(), edge,
164 ✗ current.cost + edge->path()->length()));
165 }
166 }
167 }
168
169 ✗ ParentMap_t result;
170 ✗ for (ItV_t _v = visited.begin(); _v != visited.end(); ++_v)
171 ✗ result[_v->first] = _v->second.parent;
172 ✗ return result;
173 }
174
175 ✗ void BiRrtStar::startSolve() {
176 ✗ Parent_t::startSolve();
177
178 ✗ if (roadmap()->goalNodes().size() != 1)
179 ✗ throw std::invalid_argument("there should be only one goal node.");
180
181 ✗ extendMaxLength_ =
182 ✗ problem()->getParameter("BiRRT*/maxStepLength").floatValue();
183 ✗ if (extendMaxLength_ <= 0)
184 ✗ extendMaxLength_ = std::sqrt(problem()->robot()->numberDof());
185 ✗ gamma_ = problem()->getParameter("BiRRT*/gamma").floatValue();
186 ✗ minimalPathLength_ =
187 ✗ problem()->getParameter("BiRRT*/minimalPathLength").floatValue();
188
189 ✗ roots_[0] = roadmap()->initNode();
190 ✗ roots_[1] = roadmap()->goalNodes()[0];
191
192 ✗ toRoot_[0].clear();
193 ✗ toRoot_[1].clear();
194 ✗ setParent(toRoot_[0], roots_[0], EdgePtr_t(), true);
195 ✗ setParent(toRoot_[1], roots_[1], EdgePtr_t(), true);
196 }
197
198 ✗ void BiRrtStar::oneStep() {
199 ✗ Configuration_t q = sample();
200
201 ✗ if (roadmap()->connectedComponents().size() == 2) {
202 ✗ if (extend(roots_[0], toRoot_[0], q)) {
203 // in the unlikely event that extend connected the two graphs,
204 // then one of the connected component is not valid.
205 ✗ if (roots_[0]->connectedComponent() == roots_[1]->connectedComponent())
206 ✗ return;
207 ✗ connect(roots_[1], toRoot_[1], q);
208 }
209
210 ✗ std::swap(roots_[0], roots_[1]);
211 ✗ std::swap(toRoot_[0], toRoot_[1]);
212 } else {
213 ✗ if (toRoot_[1].find(roots_[0]) == toRoot_[1].end()) {
214 // Fill parent map
215 ✗ toRoot_[0] = computeParentMap(roots_[0]);
216 ✗ toRoot_[1] = computeParentMap(roots_[1]);
217 }
218
219 ✗ assert(toRoot_[0].size() == toRoot_[1].size());
220 ✗ assert(toRoot_[0].size() == roadmap()->nodes().size());
221 ✗ improve(q);
222 }
223 }
224
225 ✗ Configuration_t BiRrtStar::sample() {
226 ✗ Configuration_t q(problem()->robot()->configSize());
227 ✗ ConfigurationShooterPtr_t shooter = problem()->configurationShooter();
228
229 ✗ if (roadmap()->connectedComponents().size() == 1 &&
230 ✗ value_type(rand()) / INT_MAX > (value_type)0.2) {
231 // Compute best path and find one point
232 typedef ParentMap_t::const_iterator It_t;
233 ✗ int nedges = 0;
234 ✗ for (It_t current = toRoot_[0].find(roots_[1]); current->second;
235 ✗ current = toRoot_[0].find(current->second->from())) {
236 ✗ if (current == toRoot_[0].end()) {
237 ✗ shooter->shoot(q);
238 ✗ return q;
239 }
240 ✗ ++nedges;
241 }
242 ✗ if (nedges >= 2) {
243 ✗ int i = 1 + (rand() % (nedges - 1));
244 ✗ It_t edge1, edge0;
245 ✗ for (edge1 = toRoot_[0].find(roots_[1]); i != 1;
246 ✗ edge1 = toRoot_[0].find(edge1->second->from()))
247 ✗ --i;
248 ✗ edge0 = toRoot_[0].find(edge1->second->from());
249 ✗ if (edge0->second->to() != edge1->second->from())
250 ✗ throw std::logic_error("BiRRT*: wrong parent map.");
251
252 // qm = (q0 + q2) / 2
253 ✗ pinocchio::interpolate(problem()->robot(),
254 ✗ edge0->second->from()->configuration(),
255 ✗ edge1->second->to()->configuration(), 0.5, q);
256
257 // q = q1 + alpha * (qm - q1)
258 ✗ vector_t v(problem()->robot()->numberDof());
259 ✗ pinocchio::difference(problem()->robot(), q,
260 ✗ edge0->second->to()->configuration(), v);
261 ✗ v.normalize();
262
263 ✗ value_type l(extendMaxLength_ - value_type(rand()) * extendMaxLength_ /
264 ✗ (10 * (value_type)INT_MAX));
265 ✗ pinocchio::integrate(problem()->robot(),
266 ✗ edge0->second->to()->configuration(), l * v, q);
267 ✗ return q;
268 }
269 }
270
271 ✗ shooter->shoot(q);
272 ✗ return q;
273 }
274
275 ✗ bool validate(const ProblemConstPtr_t& problem, const PathPtr_t& path) {
276 ✗ PathPtr_t validPart;
277 ✗ PathValidationReportPtr_t report;
278 ✗ return problem->pathValidation()->validate(path, false, validPart, report);
279 }
280
281 ✗ bool BiRrtStar::buildPath(const Configuration_t& q0, const Configuration_t& q1,
282 value_type maxLength, bool validatePath,
283 PathPtr_t& result) {
284 ✗ result.reset();
285 ✗ bool allValid = true;
286 ✗ PathPtr_t path = problem()->steeringMethod()->steer(q0, q1);
287 ✗ if (!path) {
288 ✗ result = path;
289 ✗ return false;
290 }
291 ✗ if (problem()->pathProjector()) { // path projection
292 ✗ PathPtr_t projected;
293 ✗ allValid = problem()->pathProjector()->apply(path, projected);
294 ✗ if (!projected) {
295 ✗ result.reset();
296 ✗ return false;
297 }
298 ✗ path = projected;
299 }
300
301 ✗ if (maxLength > 0 && path->length() > maxLength) {
302 ✗ const interval_t& I = path->timeRange();
303 ✗ path = path->extract(I.first, I.first + maxLength);
304 }
305
306 ✗ if (!validatePath) {
307 ✗ result = path;
308 ✗ assert(!allValid || result->end() == q1);
309 ✗ return allValid;
310 }
311
312 ✗ PathPtr_t validPart;
313 ✗ PathValidationReportPtr_t report;
314 ✗ allValid &=
315 ✗ problem()->pathValidation()->validate(path, false, validPart, report);
316 ✗ result = validPart;
317 ✗ assert(!allValid || result->end() == q1);
318 ✗ return allValid;
319 }
320
321 ✗ bool BiRrtStar::extend(NodePtr_t target, ParentMap_t& parentMap,
322 Configuration_t& q) {
323 ✗ ConnectedComponentPtr_t cc(target->connectedComponent());
324
325 value_type dist;
326 ✗ NodePtr_t near = roadmap()->nearestNode(q, cc, dist);
327 ✗ if (dist < 1e-16) return false;
328
329 ✗ if (problem()->constraints()) {
330 // if the random configuration (alias q) is not successfully projected
331 // on the constraints, replace q by the middle configuration between
332 // q_near and q.
333 // The goal is to improve the success rate of the method for
334 // constraints that have a small basin of attraction
335 ✗ int i = 0;
336 ✗ while (!problem()->constraints()->apply(q) && i < 10) {
337 ✗ problem()->robot()->configSpace()->interpolate(near->configuration(), q,
338 .5, q);
339 ✗ ++i;
340 }
341 }
342
343 ✗ PathPtr_t path;
344 ✗ buildPath(near->configuration(), q, extendMaxLength_, true, path);
345 ✗ if (!path || path->length() < minimalPathLength_) return false;
346 ✗ q = path->end();
347 ✗ assert(path->end() == q);
348
349 ✗ value_type n((value_type)roadmap()->nodes().size());
350 ✗ NodeVector_t nearNodes = roadmap()->nodesWithinBall(
351 q, cc,
352 ✗ std::min(
353 ✗ gamma_ * std::pow(std::log(n) / n,
354 ✗ 1. / (value_type)problem()->robot()->numberDof()),
355 ✗ extendMaxLength_));
356
357 ✗ value_type cost_q(computeCost(parentMap, near) + path->length());
358 ✗ std::vector<ValidatedPath_t> paths;
359 ✗ paths.reserve(nearNodes.size());
360 ✗ for (NodeVector_t::const_iterator _near = nearNodes.begin();
361 ✗ _near != nearNodes.end(); ++_near) {
362 ✗ PathPtr_t near2new;
363 ✗ assert(!near2new);
364 ✗ if (*_near == near) {
365 ✗ assert(path->end() == q);
366 ✗ near2new = path;
367 ✗ assert(near2new->end() == q);
368 ✗ paths.push_back(ValidatedPath_t(true, near2new));
369 ✗ continue;
370 ✗ } else if (buildPath((*_near)->configuration(), q, -1, false, near2new)) {
371 ✗ paths.push_back(ValidatedPath_t(false, near2new));
372 ✗ assert(near2new->end() == q);
373 } else {
374 ✗ paths.push_back(ValidatedPath_t(false, PathPtr_t()));
375 ✗ continue;
376 }
377 ✗ assert(near2new->end() == q);
378
379 ✗ if (paths.size() > 0) {
380 ✗ value_type _cost_q = computeCost(parentMap, *_near) + near2new->length();
381 ✗ if (_cost_q < cost_q) {
382 ✗ paths.back().first = true;
383 // Run path validation
384 ✗ if (validate(problem(), near2new)) {
385 // Path is valid and shorter.
386 ✗ cost_q = _cost_q;
387 ✗ near = *_near;
388 ✗ path = near2new;
389 } else
390 ✗ paths.back().second.reset();
391 }
392 }
393 }
394
395 ✗ NodePtr_t nnew = roadmap()->addNode(q);
396 ✗ EdgePtr_t edge = roadmap()->addEdge(near, nnew, path);
397 ✗ roadmap()->addEdge(nnew, near, path->reverse());
398 ✗ assert(parentMap.find(near) != parentMap.end());
399 ✗ if (parentMap.count(nnew)) return false;
400 ✗ setParent(parentMap, nnew, edge, true);
401 ✗ assert(paths.size() == nearNodes.size());
402 ✗ for (std::size_t i = 0; i < nearNodes.size(); ++i) {
403 ✗ if (nearNodes[i] == near || !paths[i].second) continue;
404
405 ✗ value_type cost_q_near = cost_q + paths[i].second->length();
406 ✗ if (cost_q_near < computeCost(parentMap, nearNodes[i])) {
407 ✗ bool pathValid = paths[i].first;
408 ✗ if (!pathValid) // If path validation has not been run
409 ✗ pathValid = validate(problem(), paths[i].second);
410 ✗ if (pathValid) {
411 ✗ roadmap()->addEdge(nearNodes[i], nnew, paths[i].second);
412 edge =
413 ✗ roadmap()->addEdge(nnew, nearNodes[i], paths[i].second->reverse());
414 ✗ setParent(parentMap, nearNodes[i], edge, false);
415 }
416 }
417 }
418 ✗ return true;
419 }
420
421 ✗ bool BiRrtStar::connect(NodePtr_t b, ParentMap_t& parentMap,
422 const Configuration_t& q) {
423 ✗ Configuration_t qnew;
424 // while extend did not reach q
425 ✗ while (roadmap()->connectedComponents().size() == 2) {
426 ✗ qnew = q;
427 ✗ if (!extend(b, parentMap, qnew)) // extend failed
428 ✗ return false;
429 }
430 ✗ return true;
431 }
432
433 ✗ bool BiRrtStar::improve(const Configuration_t& q) {
434 value_type dist;
435 ✗ const NodePtr_t nearQ = roadmap()->nearestNode(q, dist);
436 ✗ if (dist < 1e-16) return false;
437
438 ✗ PathPtr_t nearQ_qnew;
439 ✗ buildPath(nearQ->configuration(), q, extendMaxLength_, true, nearQ_qnew);
440 ✗ if (!nearQ_qnew || nearQ_qnew->length() < minimalPathLength_) return false;
441
442 ✗ const Configuration_t qnew(nearQ_qnew->end());
443
444 ✗ const value_type n((value_type)roadmap()->nodes().size());
445 ✗ NodeVector_t nearNodes = roadmap()->nodesWithinBall(
446 ✗ qnew, roots_[0]->connectedComponent(),
447 ✗ std::min(
448 ✗ gamma_ * std::pow(std::log(n) / n,
449 ✗ 1. / (value_type)problem()->robot()->numberDof()),
450 ✗ extendMaxLength_));
451
452 ✗ const NodePtr_t nnew = roadmap()->addNode(qnew);
453
454 ✗ std::vector<ValidatedPath_t> paths;
455 ✗ paths.reserve(nearNodes.size());
456
457 ✗ for (int k = 0; k < 2; ++k) {
458 ✗ paths.clear();
459
460 ✗ NodePtr_t bestParent(nearQ);
461 ✗ PathPtr_t best_qnew(nearQ_qnew);
462 ✗ value_type cost_q(computeCost(toRoot_[k], nearQ) + nearQ_qnew->length());
463
464 ✗ for (NodeVector_t::const_iterator _near = nearNodes.begin();
465 ✗ _near != nearNodes.end(); ++_near) {
466 ✗ PathPtr_t near2new;
467 ✗ if (*_near == nearQ) {
468 ✗ near2new = nearQ_qnew;
469 ✗ assert(near2new->end() == qnew);
470 ✗ paths.push_back(ValidatedPath_t(true, near2new));
471 ✗ continue;
472 ✗ } else if (buildPath((*_near)->configuration(), qnew, -1, false,
473 near2new)) {
474 ✗ paths.push_back(ValidatedPath_t(false, near2new));
475 ✗ assert(near2new->end() == qnew);
476 } else {
477 ✗ paths.push_back(ValidatedPath_t(false, PathPtr_t()));
478 ✗ continue;
479 }
480 ✗ if (paths.size() > 0) {
481 value_type _cost_q =
482 ✗ computeCost(toRoot_[k], *_near) + near2new->length();
483 ✗ if (_cost_q < cost_q) {
484 ✗ paths.back().first = true;
485 // Run path validation
486 ✗ if (validate(problem(), near2new)) {
487 // Path is valid and shorter.
488 ✗ cost_q = _cost_q;
489 ✗ bestParent = *_near;
490 ✗ best_qnew = near2new;
491 } else
492 ✗ paths.back().second.reset();
493 }
494 }
495 }
496
497 ✗ EdgePtr_t edge = roadmap()->addEdge(bestParent, nnew, best_qnew);
498 ✗ roadmap()->addEdge(nnew, bestParent, best_qnew->reverse());
499 ✗ assert(toRoot_[k].find(bestParent) != toRoot_[k].end());
500 ✗ if (toRoot_[k].count(nnew)) continue;
501 ✗ setParent(toRoot_[k], nnew, edge, true);
502
503 ✗ assert(paths.size() == nearNodes.size());
504 ✗ for (std::size_t i = 0; i < nearNodes.size(); ++i) {
505 ✗ if (nearNodes[i] == bestParent || !paths[i].second) continue;
506
507 ✗ value_type cost_q_near = cost_q + paths[i].second->length();
508 ✗ if (cost_q_near < computeCost(toRoot_[k], nearNodes[i])) {
509 ✗ bool pathValid = paths[i].first;
510 ✗ if (!pathValid) // If path validation has not been run
511 ✗ pathValid = validate(problem(), paths[i].second);
512 ✗ if (pathValid) {
513 ✗ roadmap()->addEdge(nearNodes[i], nnew, paths[i].second);
514 ✗ edge = roadmap()->addEdge(nnew, nearNodes[i],
515 ✗ paths[i].second->reverse());
516 ✗ assert(toRoot_[k].find(nnew) != toRoot_[k].end());
517 ✗ setParent(toRoot_[k], nearNodes[i], edge, false);
518 }
519 }
520 }
521 }
522 ✗ return true;
523 }
524
525 // ----------- Declare parameters ------------------------------------- //
526
527 18 HPP_START_PARAMETER_DECLARATION(BiRrtStar)
528
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(
529 Parameter::FLOAT, "BiRRT*/maxStepLength",
530 "The maximum step length when extending. If negative, uses sqrt(dimension)",
531
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(-1.)));
532
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(Parameter::FLOAT, "BiRRT*/gamma",
533
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 "", Parameter(1.)));
534
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(
535 Parameter::FLOAT, "BiRRT*/minimalPathLength",
536 "The minimum length between 2 configurations in the roadmap.",
537
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(1e-4)));
538 18 HPP_END_PARAMETER_DECLARATION(BiRrtStar)
539 } // namespace pathPlanner
540 } // namespace core
541 } // namespace hpp
542