| 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 |