Directory: | ./ |
---|---|
File: | src/path-planner/k-prm-star.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 4 | 119 | 3.4% |
Branches: | 5 | 219 | 2.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2018 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 <cmath> | ||
31 | #include <hpp/core/config-validations.hh> | ||
32 | #include <hpp/core/configuration-shooter.hh> | ||
33 | #include <hpp/core/path-planner/k-prm-star.hh> | ||
34 | #include <hpp/core/path-planning-failed.hh> | ||
35 | #include <hpp/core/path-projector.hh> | ||
36 | #include <hpp/core/path-validation-report.hh> | ||
37 | #include <hpp/core/path-validation.hh> | ||
38 | #include <hpp/core/problem.hh> | ||
39 | #include <hpp/core/roadmap.hh> | ||
40 | |||
41 | namespace hpp { | ||
42 | namespace core { | ||
43 | namespace pathPlanner { | ||
44 | |||
45 | const double kPrmStar::kPRM = 2 * exp(1); | ||
46 | |||
47 | ✗ | kPrmStarPtr_t kPrmStar::create(const ProblemConstPtr_t& problem) { | |
48 | ✗ | kPrmStarPtr_t shPtr(new kPrmStar(problem)); | |
49 | ✗ | shPtr->init(shPtr); | |
50 | ✗ | return shPtr; | |
51 | } | ||
52 | |||
53 | ✗ | kPrmStarPtr_t kPrmStar::createWithRoadmap(const ProblemConstPtr_t& problem, | |
54 | const RoadmapPtr_t& roadmap) { | ||
55 | ✗ | kPrmStarPtr_t shPtr(new kPrmStar(problem, roadmap)); | |
56 | ✗ | shPtr->init(shPtr); | |
57 | ✗ | return shPtr; | |
58 | } | ||
59 | |||
60 | ✗ | void kPrmStar::startSolve() { | |
61 | ✗ | Parent_t::startSolve(); | |
62 | ✗ | numberNodes_ = problem()->getParameter("kPRM*/numberOfNodes").intValue(); | |
63 | ✗ | if (numberNodes_ == 0) { | |
64 | ✗ | std::ostringstream oss; | |
65 | ✗ | oss << "kPrmStar: Number nodes should be positive, got " << numberNodes_; | |
66 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
67 | } | ||
68 | ✗ | numberNeighbors_ = | |
69 | ✗ | (size_type)floor((kPRM * log((value_type)numberNodes_)) + .5); | |
70 | ✗ | if (roadmap()->nodes().size() >= numberNodes_) { | |
71 | ✗ | state_ = CONNECT_INIT_GOAL; | |
72 | } else { | ||
73 | ✗ | state_ = BUILD_ROADMAP; | |
74 | } | ||
75 | } | ||
76 | |||
77 | ✗ | void kPrmStar::tryConnectInitAndGoals() {} | |
78 | |||
79 | ✗ | void kPrmStar::oneStep() { | |
80 | ✗ | std::ostringstream oss; | |
81 | // Shoot valid random configurations | ||
82 | ✗ | switch (state_) { | |
83 | ✗ | case BUILD_ROADMAP: | |
84 | ✗ | generateRandomConfig(); | |
85 | ✗ | break; | |
86 | ✗ | case LINK_NODES: | |
87 | ✗ | linkNodes(); | |
88 | ✗ | break; | |
89 | ✗ | case CONNECT_INIT_GOAL: | |
90 | ✗ | connectInitAndGoal(); | |
91 | ✗ | state_ = FAILURE; | |
92 | ✗ | break; | |
93 | ✗ | case FAILURE: | |
94 | ✗ | oss << "kPRM* failed to solve problem with " << numberNodes_ << " nodes."; | |
95 | ✗ | throw path_planning_failed(oss.str().c_str()); | |
96 | } | ||
97 | } | ||
98 | |||
99 | ✗ | void kPrmStar::generateRandomConfig() { | |
100 | // shoot a valid random configuration | ||
101 | ✗ | Configuration_t qrand; | |
102 | // Report of configuration validation: unused here | ||
103 | ✗ | ValidationReportPtr_t validationReport; | |
104 | // Configuration validation methods associated to the problem | ||
105 | ✗ | ConfigValidationsPtr_t configValidations(problem()->configValidations()); | |
106 | // Get the constraints the robot is subject to | ||
107 | ✗ | ConstraintSetPtr_t constraints(problem()->constraints()); | |
108 | // Get the problem shooter | ||
109 | ✗ | ConfigurationShooterPtr_t shooter = problem()->configurationShooter(); | |
110 | // Get roadmap | ||
111 | ✗ | RoadmapPtr_t r(roadmap()); | |
112 | ✗ | if (r->nodes().size() < numberNodes_) { | |
113 | ✗ | size_type nbTry = 0; | |
114 | ✗ | bool valid(false); | |
115 | // After 10000 trials throw if no valid configuration has been found. | ||
116 | do { | ||
117 | ✗ | shooter->shoot(qrand); | |
118 | ✗ | valid = (!constraints || constraints->apply(qrand)); | |
119 | ✗ | if (valid) valid = configValidations->validate(qrand, validationReport); | |
120 | ✗ | nbTry++; | |
121 | ✗ | } while (!valid && nbTry < 10000); | |
122 | ✗ | if (!valid) { | |
123 | ✗ | throw path_planning_failed( | |
124 | ✗ | "Failed to generate free configuration after 10000 trials."); | |
125 | } | ||
126 | ✗ | r->addNode(qrand); | |
127 | } else { | ||
128 | ✗ | state_ = LINK_NODES; | |
129 | ✗ | linkingNodeIt_ = r->nodes().begin(); | |
130 | ✗ | neighbors_ = roadmap()->nearestNodes((*linkingNodeIt_)->configuration(), | |
131 | ✗ | numberNeighbors_); | |
132 | ✗ | itNeighbor_ = neighbors_.begin(); | |
133 | } | ||
134 | } | ||
135 | |||
136 | ✗ | void kPrmStar::linkNodes() { | |
137 | // Get roadmap | ||
138 | ✗ | RoadmapPtr_t r(roadmap()); | |
139 | ✗ | if (linkingNodeIt_ != r->nodes().end()) { | |
140 | ✗ | if (connectNodeToClosestNeighbors(*linkingNodeIt_)) { | |
141 | ✗ | ++linkingNodeIt_; | |
142 | ✗ | if (linkingNodeIt_ != r->nodes().end()) { | |
143 | ✗ | neighbors_ = roadmap()->nearestNodes((*linkingNodeIt_)->configuration(), | |
144 | ✗ | numberNeighbors_); | |
145 | // Connect current node with closest neighbors | ||
146 | ✗ | itNeighbor_ = neighbors_.begin(); | |
147 | } | ||
148 | } else { | ||
149 | ✗ | ++itNeighbor_; | |
150 | } | ||
151 | } else { | ||
152 | ✗ | state_ = CONNECT_INIT_GOAL; | |
153 | } | ||
154 | } | ||
155 | |||
156 | ✗ | bool kPrmStar::connectNodeToClosestNeighbors(const NodePtr_t& node) { | |
157 | // Retrieve the path validation algorithm associated to the problem | ||
158 | ✗ | PathValidationPtr_t pathValidation(problem()->pathValidation()); | |
159 | // Retrieve the steering method | ||
160 | ✗ | SteeringMethodPtr_t sm(problem()->steeringMethod()); | |
161 | // Retrieve the constraints the robot is subject to | ||
162 | ✗ | ConstraintSetPtr_t constraints(problem()->constraints()); | |
163 | // Retrieve path projector | ||
164 | ✗ | PathProjectorPtr_t pathProjector(problem()->pathProjector()); | |
165 | |||
166 | ✗ | if (itNeighbor_ != neighbors_.end()) { | |
167 | // Connect only nodes that are not already connected | ||
168 | ✗ | if (!(*itNeighbor_)->isOutNeighbor(node) && (node != *itNeighbor_)) { | |
169 | PathPtr_t p( | ||
170 | ✗ | (*sm)(node->configuration(), (*itNeighbor_)->configuration())); | |
171 | ✗ | PathValidationReportPtr_t report; | |
172 | ✗ | PathPtr_t validPart, projected; | |
173 | ✗ | if (p) { | |
174 | bool success; | ||
175 | ✗ | if (pathProjector) { | |
176 | ✗ | success = pathProjector->apply(p, projected); | |
177 | } else { | ||
178 | ✗ | projected = p; | |
179 | ✗ | success = true; | |
180 | } | ||
181 | ✗ | if (success) { | |
182 | ✗ | if (pathValidation->validate(projected, false, validPart, report)) { | |
183 | ✗ | roadmap()->addEdges(node, *itNeighbor_, projected); | |
184 | } | ||
185 | } | ||
186 | } | ||
187 | } | ||
188 | ✗ | return false; | |
189 | } else { | ||
190 | // itNeighbor_ reached the end | ||
191 | ✗ | return true; | |
192 | } | ||
193 | } | ||
194 | |||
195 | ✗ | void kPrmStar::connectInitAndGoal() { | |
196 | ✗ | NodePtr_t initNode(roadmap()->initNode()); | |
197 | ✗ | if (initNode->outEdges().empty()) { | |
198 | neighbors_ = | ||
199 | ✗ | roadmap()->nearestNodes(initNode->configuration(), numberNeighbors_); | |
200 | // Connect current node with closest neighbors | ||
201 | ✗ | for (itNeighbor_ = neighbors_.begin(); itNeighbor_ != neighbors_.end(); | |
202 | ✗ | ++itNeighbor_) { | |
203 | ✗ | connectNodeToClosestNeighbors(initNode); | |
204 | } | ||
205 | } | ||
206 | ✗ | for (NodeVector_t::const_iterator itn(roadmap()->goalNodes().begin()); | |
207 | ✗ | itn != roadmap()->goalNodes().end(); ++itn) { | |
208 | neighbors_ = | ||
209 | ✗ | roadmap()->nearestNodes((*itn)->configuration(), numberNeighbors_); | |
210 | ✗ | if ((*itn)->inEdges().empty()) { | |
211 | ✗ | for (itNeighbor_ = neighbors_.begin(); itNeighbor_ != neighbors_.end(); | |
212 | ✗ | ++itNeighbor_) { | |
213 | ✗ | connectNodeToClosestNeighbors(*itn); | |
214 | } | ||
215 | } | ||
216 | } | ||
217 | } | ||
218 | |||
219 | ✗ | kPrmStar::STATE kPrmStar::getComputationState() const { return state_; } | |
220 | |||
221 | ✗ | kPrmStar::kPrmStar(const ProblemConstPtr_t& problem) | |
222 | ✗ | : Parent_t(problem), state_(BUILD_ROADMAP) {} | |
223 | |||
224 | ✗ | kPrmStar::kPrmStar(const ProblemConstPtr_t& problem, | |
225 | ✗ | const RoadmapPtr_t& roadmap) | |
226 | ✗ | : Parent_t(problem, roadmap), state_(BUILD_ROADMAP) {} | |
227 | |||
228 | ✗ | void kPrmStar::init(const kPrmStarWkPtr_t& weak) { | |
229 | ✗ | Parent_t::init(weak); | |
230 | ✗ | weak_ = weak; | |
231 | } | ||
232 | |||
233 | // ----------- Declare parameters ------------------------------------- // | ||
234 | |||
235 | 18 | HPP_START_PARAMETER_DECLARATION(kPrmStar) | |
236 |
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( |
237 | Parameter::INT, "kPRM*/numberOfNodes", | ||
238 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "The desired number of nodes in the roadmap.", Parameter((size_type)100))); |
239 | 18 | HPP_END_PARAMETER_DECLARATION(kPrmStar) | |
240 | } // namespace pathPlanner | ||
241 | } // namespace core | ||
242 | } // namespace hpp | ||
243 |