GCC Code Coverage Report

Directory: ./
File: src/manipulation-planner.cc
Date: 2025-05-07 11:07:45
Exec Total Coverage
Lines: 4 244 1.6%
Branches: 5 450 1.1%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
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/manipulation/manipulation-planner.hh"
30
31 #include <hpp/core/configuration-shooter.hh>
32 #include <hpp/core/connected-component.hh>
33 #include <hpp/core/nearest-neighbor.hh>
34 #include <hpp/core/path-projector.hh>
35 #include <hpp/core/path-validation.hh>
36 #include <hpp/core/projection-error.hh>
37 #include <hpp/core/roadmap.hh>
38 #include <hpp/pinocchio/configuration.hh>
39 #include <hpp/util/assertion.hh>
40 #include <hpp/util/pointer.hh>
41 #include <hpp/util/timer.hh>
42 #include <iterator>
43 #include <tuple>
44
45 #include "hpp/manipulation/connected-component.hh"
46 #include "hpp/manipulation/device.hh"
47 #include "hpp/manipulation/graph-path-validation.hh"
48 #include "hpp/manipulation/graph/edge.hh"
49 #include "hpp/manipulation/graph/state-selector.hh"
50 #include "hpp/manipulation/graph/statistics.hh"
51 #include "hpp/manipulation/problem.hh"
52 #include "hpp/manipulation/roadmap-node.hh"
53 #include "hpp/manipulation/roadmap.hh"
54
55 namespace hpp {
56 namespace manipulation {
57 namespace {
58 HPP_DEFINE_TIMECOUNTER(oneStep);
59 HPP_DEFINE_TIMECOUNTER(extend);
60 HPP_DEFINE_TIMECOUNTER(tryConnect);
61 HPP_DEFINE_TIMECOUNTER(nearestNeighbor);
62 HPP_DEFINE_TIMECOUNTER(delayedEdges);
63 HPP_DEFINE_TIMECOUNTER(tryConnectNewNodes);
64 HPP_DEFINE_TIMECOUNTER(tryConnectToRoadmap);
65 /// extend steps
66 HPP_DEFINE_TIMECOUNTER(chooseEdge);
67 HPP_DEFINE_TIMECOUNTER(generateTargetConfig);
68 HPP_DEFINE_TIMECOUNTER(buildPath);
69 HPP_DEFINE_TIMECOUNTER(projectPath);
70 HPP_DEFINE_TIMECOUNTER(validatePath);
71
72 ✗ graph::StatePtr_t getState(const graph::GraphPtr_t graph,
73 const core::NodePtr_t& node) {
74 ✗ RoadmapNodePtr_t mnode(dynamic_cast<RoadmapNode*>(node));
75 ✗ if (mnode != NULL)
76 ✗ return mnode->graphState();
77 else
78 ✗ return graph->getState(node->configuration());
79 }
80
81 ✗ core::PathPtr_t connect(const Configuration_t& q1, const Configuration_t& q2,
82 const graph::StatePtr_t& s1,
83 const graph::StatePtr_t& s2,
84 const graph::GraphPtr_t& graph,
85 const PathProjectorPtr_t& pathProjector,
86 const PathValidationPtr_t& pathValidation) {
87 ✗ assert(graph && s1 && s2);
88 ✗ graph::Edges_t possibleEdges = graph->getEdges(s1, s2);
89
90 ✗ core::PathPtr_t path, tmpPath;
91
92 ✗ graph::EdgePtr_t edge;
93 ✗ for (std::size_t i = 0; i < possibleEdges.size(); ++i) {
94 ✗ edge = possibleEdges[i];
95 ✗ if (edge->build(path, q1, q2)) break;
96 }
97 ✗ if (!path) return path;
98 ✗ if (pathProjector) {
99 ✗ if (!pathProjector->apply(path, tmpPath)) return core::PathPtr_t();
100 ✗ path = tmpPath;
101 }
102
103 ✗ PathValidationReportPtr_t report;
104 ✗ if (pathValidation->validate(path, false, tmpPath, report)) return path;
105 ✗ return core::PathPtr_t();
106 }
107 } // namespace
108
109 const std::vector<ManipulationPlanner::Reason> ManipulationPlanner::reasons_ = {
110 SuccessBin::createReason(
111 "--Path could not be fully projected"), // PATH_PROJECTION_SHORTER = 0,
112 SuccessBin::createReason(
113 "--Path could not be fully validated"), // PATH_VALIDATION_SHORTER = 1,
114 SuccessBin::createReason(
115 "--Reached destination node"), // REACHED_DESTINATION_NODE = 2,
116 SuccessBin::createReason("Failure"), // FAILURE = 3,
117 SuccessBin::createReason(
118 "--Projection of configuration on edge leaf"), // PROJECTION = 4,
119 SuccessBin::createReason("--SteeringMethod"), // STEERING_METHOD = 5,
120 SuccessBin::createReason(
121 "--Path validation returned length 0"), // PATH_VALIDATION_ZERO = 6,
122 SuccessBin::createReason(
123 "--Path could not be projected at all"), // PATH_PROJECTION_ZERO = 7
124 };
125
126 ✗ ManipulationPlannerPtr_t ManipulationPlanner::create(
127 const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap) {
128 ManipulationPlanner* ptr;
129 ✗ core::RoadmapPtr_t r2 = roadmap;
130 ✗ ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST(const Problem, problem);
131 ✗ RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST(Roadmap, r2);
132 ✗ if (!r)
133 ✗ throw std::invalid_argument(
134 ✗ "The roadmap must be of type hpp::manipulation::Roadmap.");
135 ✗ if (!p)
136 ✗ throw std::invalid_argument(
137 ✗ "The problem must be of type hpp::manipulation::Problem.");
138
139 ✗ ptr = new ManipulationPlanner(p, r);
140 ✗ ManipulationPlannerPtr_t shPtr(ptr);
141 ✗ ptr->init(shPtr);
142 ✗ return shPtr;
143 }
144
145 ✗ ManipulationPlanner::ErrorFreqs_t ManipulationPlanner::getEdgeStat(
146 const graph::EdgePtr_t& edge) const {
147 ✗ const std::size_t& id = edge->id();
148 ✗ ErrorFreqs_t ret;
149 ✗ if (indexPerEdgeStatistics_.size() <= id || indexPerEdgeStatistics_[id] < 0) {
150 ✗ for (std::size_t i = 0; i < reasons_.size(); ++i) ret.push_back(0);
151 } else {
152 const SuccessStatistics& ss =
153 ✗ perEdgeStatistics_[indexPerEdgeStatistics_[id]];
154 ✗ ret.push_back(ss.nbSuccess());
155 ✗ for (std::size_t i = 0; i < reasons_.size(); ++i)
156 ✗ ret.push_back(ss.nbFailure(reasons_[i]));
157 }
158 ✗ return ret;
159 }
160
161 ✗ StringList_t ManipulationPlanner::errorList() {
162 ✗ StringList_t ret;
163 ✗ ret.push_back("Success");
164 ✗ for (std::size_t i = 0; i < reasons_.size(); ++i)
165 ✗ ret.push_back(reasons_[i].what);
166 ✗ return ret;
167 }
168
169 ✗ void ManipulationPlanner::oneStep() {
170 HPP_START_TIMECOUNTER(oneStep);
171
172 ✗ DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem()->robot());
173 HPP_ASSERT(robot);
174 const graph::States_t& graphStates =
175 ✗ problem_->constraintGraph()->stateSelector()->getStates();
176 ✗ graph::States_t::const_iterator itState;
177 ✗ core::Nodes_t newNodes;
178 ✗ core::PathPtr_t path;
179
180 typedef std::tuple<core::NodePtr_t, Configuration_t, core::PathPtr_t>
181 DelayedEdge_t;
182 typedef std::vector<DelayedEdge_t> DelayedEdges_t;
183 ✗ DelayedEdges_t delayedEdges;
184
185 // Pick a random node
186 ✗ Configuration_t q_rand = shooter_->shoot();
187
188 // Extend each connected component
189 ✗ for (core::ConnectedComponents_t::const_iterator itcc =
190 ✗ roadmap()->connectedComponents().begin();
191 ✗ itcc != roadmap()->connectedComponents().end(); ++itcc) {
192 // Find the nearest neighbor.
193 core::value_type distance;
194 ✗ for (itState = graphStates.begin(); itState != graphStates.end();
195 ✗ ++itState) {
196 HPP_START_TIMECOUNTER(nearestNeighbor);
197 ✗ RoadmapNodePtr_t near = roadmap_->nearestNodeInState(
198 ✗ q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent, *itcc), *itState,
199 ✗ distance);
200 HPP_STOP_TIMECOUNTER(nearestNeighbor);
201 HPP_DISPLAY_LAST_TIMECOUNTER(nearestNeighbor);
202 ✗ if (!near) continue;
203
204 HPP_START_TIMECOUNTER(extend);
205 ✗ bool pathIsValid = extend(near, q_rand, path);
206 HPP_STOP_TIMECOUNTER(extend);
207 HPP_DISPLAY_LAST_TIMECOUNTER(extend);
208 // Insert new path to q_near in roadmap
209 ✗ if (pathIsValid) {
210 ✗ value_type t_final = path->timeRange().second;
211 ✗ if (t_final != path->timeRange().first) {
212 bool success;
213 ✗ Configuration_t q_new(path->eval(t_final, success));
214 ✗ assert(success);
215 ✗ assert(!path->constraints() ||
216 path->constraints()->isSatisfied(q_new));
217 ✗ assert(problem_->constraintGraph()->getState(q_new));
218 ✗ delayedEdges.push_back(DelayedEdge_t(near, q_new, path));
219 }
220 }
221 }
222 }
223
224 HPP_START_TIMECOUNTER(delayedEdges);
225 // Insert delayed edges
226 ✗ for (const auto& edge : delayedEdges) {
227 ✗ const core::NodePtr_t& near = std::get<0>(edge);
228 ✗ Configuration_t q_new = std::get<1>(edge);
229 ✗ const core::PathPtr_t& validPath = std::get<2>(edge);
230 ✗ core::NodePtr_t newNode = roadmap()->addNode(q_new);
231 ✗ roadmap()->addEdge(near, newNode, validPath);
232 ✗ roadmap()->addEdge(newNode, near, validPath->reverse());
233 ✗ newNodes.push_back(newNode);
234 }
235 HPP_STOP_TIMECOUNTER(delayedEdges);
236
237 // Try to connect the new nodes together
238 HPP_START_TIMECOUNTER(tryConnectNewNodes);
239 ✗ const std::size_t nbConn = tryConnectNewNodes(newNodes);
240 HPP_STOP_TIMECOUNTER(tryConnectNewNodes);
241 HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectNewNodes);
242 ✗ if (nbConn == 0) {
243 HPP_START_TIMECOUNTER(tryConnectToRoadmap);
244 ✗ tryConnectToRoadmap(newNodes);
245 HPP_STOP_TIMECOUNTER(tryConnectToRoadmap);
246 HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectToRoadmap);
247 }
248 HPP_STOP_TIMECOUNTER(oneStep);
249 HPP_DISPLAY_LAST_TIMECOUNTER(oneStep);
250 HPP_DISPLAY_TIMECOUNTER(oneStep);
251 HPP_DISPLAY_TIMECOUNTER(extend);
252 HPP_DISPLAY_TIMECOUNTER(tryConnect);
253 HPP_DISPLAY_TIMECOUNTER(tryConnectNewNodes);
254 HPP_DISPLAY_TIMECOUNTER(tryConnectToRoadmap);
255 HPP_DISPLAY_TIMECOUNTER(nearestNeighbor);
256 HPP_DISPLAY_TIMECOUNTER(delayedEdges);
257 HPP_DISPLAY_TIMECOUNTER(chooseEdge);
258 HPP_DISPLAY_TIMECOUNTER(generateTargetConfig);
259 HPP_DISPLAY_TIMECOUNTER(buildPath);
260 HPP_DISPLAY_TIMECOUNTER(projectPath);
261 HPP_DISPLAY_TIMECOUNTER(validatePath);
262 }
263
264 ✗ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
265 ConfigurationIn_t q_rand,
266 core::PathPtr_t& validPath) {
267 ✗ graph::GraphPtr_t graph = problem_->constraintGraph();
268 ✗ PathProjectorPtr_t pathProjector = problem_->pathProjector();
269 ✗ pinocchio::DevicePtr_t robot(problem_->robot());
270 ✗ value_type eps(graph->errorThreshold());
271 // Select next node in the constraint graph.
272 ✗ const Configuration_t q_near = n_near->configuration();
273 HPP_START_TIMECOUNTER(chooseEdge);
274 ✗ graph::EdgePtr_t edge = graph->chooseEdge(n_near);
275 HPP_STOP_TIMECOUNTER(chooseEdge);
276 ✗ if (!edge) {
277 ✗ return false;
278 }
279 ✗ qProj_ = q_rand;
280 HPP_START_TIMECOUNTER(generateTargetConfig);
281 ✗ SuccessStatistics& es = edgeStat(edge);
282 ✗ if (!edge->generateTargetConfig(n_near, qProj_)) {
283 HPP_STOP_TIMECOUNTER(generateTargetConfig);
284 ✗ es.addFailure(reasons_[FAILURE]);
285 ✗ es.addFailure(reasons_[PROJECTION]);
286 ✗ return false;
287 }
288 ✗ if (pinocchio::isApprox(robot, qProj_, q_near, eps)) {
289 ✗ es.addFailure(reasons_[FAILURE]);
290 ✗ es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
291 ✗ return false;
292 }
293 HPP_STOP_TIMECOUNTER(generateTargetConfig);
294 ✗ core::PathPtr_t path;
295 HPP_START_TIMECOUNTER(buildPath);
296 ✗ if (!edge->build(path, q_near, qProj_)) {
297 HPP_STOP_TIMECOUNTER(buildPath);
298 ✗ es.addFailure(reasons_[FAILURE]);
299 ✗ es.addFailure(reasons_[STEERING_METHOD]);
300 ✗ return false;
301 }
302 HPP_STOP_TIMECOUNTER(buildPath);
303 ✗ core::PathPtr_t projPath;
304 ✗ bool projShorter = false;
305 ✗ if (pathProjector) {
306 HPP_START_TIMECOUNTER(projectPath);
307 ✗ projShorter = !pathProjector->apply(path, projPath);
308 ✗ if (projShorter) {
309 ✗ if (!projPath || projPath->length() == 0) {
310 hppDout(info, "");
311 HPP_STOP_TIMECOUNTER(projectPath);
312 ✗ es.addFailure(reasons_[FAILURE]);
313 ✗ es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
314 ✗ return false;
315 }
316 }
317 HPP_STOP_TIMECOUNTER(projectPath);
318 } else
319 ✗ projPath = path;
320 ✗ PathValidationPtr_t pathValidation(problem_->pathValidation());
321 ✗ PathValidationReportPtr_t report;
322 ✗ core::PathPtr_t fullValidPath;
323 HPP_START_TIMECOUNTER(validatePath);
324 ✗ bool fullyValid = false;
325 try {
326 fullyValid =
327 ✗ pathValidation->validate(projPath, false, fullValidPath, report);
328 ✗ } catch (const core::projection_error& e) {
329 hppDout(error, e.what());
330 ✗ es.addFailure(reasons_[FAILURE]);
331 ✗ es.addFailure(reasons_[PATH_VALIDATION_ZERO]);
332 ✗ return false;
333 }
334 HPP_STOP_TIMECOUNTER(validatePath);
335 ✗ if (fullValidPath->length() == 0) {
336 ✗ es.addFailure(reasons_[FAILURE]);
337 ✗ es.addFailure(reasons_[PATH_VALIDATION_ZERO]);
338 ✗ validPath = fullValidPath;
339 ✗ return false;
340 } else {
341 ✗ if (extendStep_ == 1 || fullyValid) {
342 ✗ validPath = fullValidPath;
343 } else {
344 ✗ const value_type& length = fullValidPath->length();
345 ✗ const value_type& t_init = fullValidPath->timeRange().first;
346 try {
347 ✗ validPath = fullValidPath->extract(
348 ✗ core::interval_t(t_init, t_init + length * extendStep_));
349 ✗ } catch (const core::projection_error& e) {
350 hppDout(error, e.what());
351 ✗ es.addSuccess();
352 ✗ es.addFailure(reasons_[PATH_PROJECTION_SHORTER]);
353 ✗ return false;
354 }
355 }
356 hppDout(info, "Extension:" << std::endl << es);
357 }
358 ✗ if (!projShorter && fullyValid) {
359 ✗ es.addSuccess();
360 ✗ es.addFailure(reasons_[REACHED_DESTINATION_NODE]);
361 } else {
362 ✗ es.addSuccess();
363 ✗ if (projShorter) {
364 ✗ es.addFailure(reasons_[PATH_PROJECTION_SHORTER]);
365 } else {
366 ✗ es.addFailure(reasons_[PATH_VALIDATION_SHORTER]);
367 }
368 }
369 ✗ return true;
370 }
371
372 ✗ ManipulationPlanner::SuccessStatistics& ManipulationPlanner::edgeStat(
373 const graph::EdgePtr_t& edge) {
374 ✗ const std::size_t& id = edge->id();
375 ✗ if (indexPerEdgeStatistics_.size() <= id) {
376 ✗ indexPerEdgeStatistics_.resize(id + 1, -1);
377 }
378 ✗ if (indexPerEdgeStatistics_[id] < 0) {
379 ✗ indexPerEdgeStatistics_[id] = (int)perEdgeStatistics_.size();
380 ✗ perEdgeStatistics_.push_back(SuccessStatistics(edge->name(), 2));
381 }
382 ✗ return perEdgeStatistics_[indexPerEdgeStatistics_[id]];
383 }
384
385 ✗ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
386 const core::Nodes_t nodes) {
387 ✗ PathProjectorPtr_t pathProjector(problem()->pathProjector());
388 ✗ core::PathPtr_t path;
389 ✗ graph::GraphPtr_t graph = problem_->constraintGraph();
390 ✗ graph::Edges_t possibleEdges;
391
392 ✗ bool connectSucceed = false;
393 ✗ std::size_t nbConnection = 0;
394 ✗ const std::size_t K = 7;
395 value_type distance;
396 ✗ for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
397 ✗ ++itn1) {
398 ✗ const Configuration_t& q1((*itn1)->configuration());
399 ✗ graph::StatePtr_t s1 = getState(graph, *itn1);
400 ✗ connectSucceed = false;
401 ✗ for (core::ConnectedComponents_t::const_iterator itcc =
402 ✗ roadmap()->connectedComponents().begin();
403 ✗ itcc != roadmap()->connectedComponents().end(); ++itcc) {
404 ✗ if (*itcc == (*itn1)->connectedComponent()) continue;
405 core::Nodes_t knearest =
406 ✗ roadmap()->nearestNeighbor()->KnearestSearch(q1, *itcc, K, distance);
407 ✗ for (core::Nodes_t::const_iterator itn2 = knearest.begin();
408 ✗ itn2 != knearest.end(); ++itn2) {
409 ✗ bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
410 ✗ bool _2to1 = (*itn1)->isInNeighbor(*itn2);
411 ✗ assert(!_1to2 || !_2to1);
412
413 ✗ const Configuration_t& q2((*itn2)->configuration());
414 ✗ graph::StatePtr_t s2 = getState(graph, *itn2);
415 ✗ assert(q1 != q2);
416
417 ✗ path = connect(q1, q2, s1, s2, graph, pathProjector,
418 ✗ problem_->pathValidation());
419
420 ✗ if (path) {
421 ✗ nbConnection++;
422 ✗ if (!_1to2) roadmap()->addEdge(*itn1, *itn2, path);
423 ✗ if (!_2to1) {
424 ✗ core::interval_t timeRange = path->timeRange();
425 ✗ roadmap()->addEdge(*itn2, *itn1,
426 ✗ path->extract(core::interval_t(
427 timeRange.second, timeRange.first)));
428 }
429 ✗ connectSucceed = true;
430 ✗ break;
431 }
432 }
433 ✗ if (connectSucceed) break;
434 }
435 }
436 ✗ return nbConnection;
437 }
438
439 ✗ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
440 const core::Nodes_t nodes) {
441 ✗ PathProjectorPtr_t pathProjector(problem()->pathProjector());
442 ✗ core::PathPtr_t path;
443 ✗ graph::GraphPtr_t graph = problem_->constraintGraph();
444 ✗ std::size_t nbConnection = 0;
445 ✗ for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
446 ✗ ++itn1) {
447 ✗ const Configuration_t& q1((*itn1)->configuration());
448 ✗ graph::StatePtr_t s1 = getState(graph, *itn1);
449
450 ✗ for (core::Nodes_t::const_iterator itn2 = std::next(itn1);
451 ✗ itn2 != nodes.end(); ++itn2) {
452 ✗ if ((*itn1)->connectedComponent() == (*itn2)->connectedComponent())
453 ✗ continue;
454 ✗ bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
455 ✗ bool _2to1 = (*itn1)->isInNeighbor(*itn2);
456 ✗ assert(!_1to2 || !_2to1);
457 ✗ const Configuration_t& q2((*itn2)->configuration());
458 ✗ graph::StatePtr_t s2 = getState(graph, *itn2);
459 ✗ assert(q1 != q2);
460
461 ✗ path = connect(q1, q2, s1, s2, graph, pathProjector,
462 ✗ problem_->pathValidation());
463 ✗ if (path) {
464 ✗ nbConnection++;
465 ✗ if (!_1to2) roadmap()->addEdge(*itn1, *itn2, path);
466 ✗ if (!_2to1) {
467 ✗ core::interval_t timeRange = path->timeRange();
468 ✗ roadmap()->addEdge(*itn2, *itn1,
469 ✗ path->extract(core::interval_t(timeRange.second,
470 timeRange.first)));
471 }
472 }
473 }
474 }
475 ✗ return nbConnection;
476 }
477
478 ✗ ManipulationPlanner::ManipulationPlanner(const ProblemConstPtr_t& problem,
479 ✗ const RoadmapPtr_t& roadmap)
480 : core::PathPlanner(problem, roadmap),
481 ✗ shooter_(problem->configurationShooter()),
482 ✗ problem_(problem),
483 ✗ roadmap_(roadmap),
484 ✗ extendStep_(
485 ✗ problem->getParameter("ManipulationPlanner/extendStep").floatValue()),
486 ✗ qProj_(problem->robot()->configSize()) {}
487
488 ✗ void ManipulationPlanner::init(const ManipulationPlannerWkPtr_t& weak) {
489 ✗ core::PathPlanner::init(weak);
490 ✗ weakPtr_ = weak;
491 }
492
493 using core::Parameter;
494 using core::ParameterDescription;
495
496 1 HPP_START_PARAMETER_DECLARATION(ManipulationPlanner)
497
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 core::Problem::declareParameter(ParameterDescription(
498 Parameter::FLOAT, "ManipulationPlanner/extendStep",
499
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 "Step of the RRT extension", Parameter((value_type)1)));
500 1 HPP_END_PARAMETER_DECLARATION(ManipulationPlanner)
501 } // namespace manipulation
502 } // namespace hpp
503