| Directory: | ./ |
|---|---|
| File: | src/steering-method/graph.cc |
| Date: | 2025-03-07 11:10:46 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 13 | 41 | 31.7% |
| Branches: | 5 | 50 | 10.0% |
| 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/steering-method/graph.hh" | ||
| 30 | |||
| 31 | #include <hpp/core/steering-method/straight.hh> | ||
| 32 | #include <hpp/core/straight-path.hh> | ||
| 33 | #include <hpp/manipulation/graph/edge.hh> | ||
| 34 | #include <hpp/manipulation/graph/graph.hh> | ||
| 35 | #include <hpp/manipulation/problem.hh> | ||
| 36 | #include <hpp/util/pointer.hh> | ||
| 37 | |||
| 38 | namespace hpp { | ||
| 39 | namespace manipulation { | ||
| 40 | 4 | SteeringMethod::SteeringMethod(const ProblemConstPtr_t& problem) | |
| 41 | : core::SteeringMethod(problem), | ||
| 42 | 4 | problem_(problem), | |
| 43 |
1/2✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
4 | steeringMethod_(core::steeringMethod::Straight::create(problem)) {} |
| 44 | |||
| 45 | ✗ | SteeringMethod::SteeringMethod(const SteeringMethod& other) | |
| 46 | : core::SteeringMethod(other), | ||
| 47 | ✗ | problem_(other.problem_), | |
| 48 | ✗ | steeringMethod_(other.steeringMethod_) {} | |
| 49 | |||
| 50 | namespace steeringMethod { | ||
| 51 | |||
| 52 | 4 | GraphPtr_t Graph::create(const core::ProblemConstPtr_t& problem) { | |
| 53 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
|
4 | assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem)); |
| 54 | 4 | ProblemConstPtr_t p = HPP_STATIC_PTR_CAST(const Problem, problem); | |
| 55 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | Graph* ptr = new Graph(p); |
| 56 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | GraphPtr_t shPtr(ptr); |
| 57 | 4 | ptr->init(shPtr); | |
| 58 | 8 | return shPtr; | |
| 59 | 4 | } | |
| 60 | |||
| 61 | ✗ | GraphPtr_t Graph::createCopy(const GraphPtr_t& other) { | |
| 62 | ✗ | Graph* ptr = new Graph(*other); | |
| 63 | ✗ | GraphPtr_t shPtr(ptr); | |
| 64 | ✗ | ptr->init(shPtr); | |
| 65 | ✗ | return shPtr; | |
| 66 | } | ||
| 67 | |||
| 68 | 4 | Graph::Graph(const ProblemConstPtr_t& problem) | |
| 69 | 4 | : SteeringMethod(problem), weak_() {} | |
| 70 | |||
| 71 | ✗ | Graph::Graph(const Graph& other) : SteeringMethod(other) {} | |
| 72 | |||
| 73 | ✗ | PathPtr_t Graph::impl_compute(ConfigurationIn_t q1, | |
| 74 | ConfigurationIn_t q2) const { | ||
| 75 | ✗ | graph::Edges_t possibleEdges; | |
| 76 | // If q1 and q2 are the same, call the problem steering method between | ||
| 77 | // them | ||
| 78 | ✗ | if (q1 == q2) { | |
| 79 | core::SteeringMethodPtr_t sm( | ||
| 80 | ✗ | problem_->manipulationSteeringMethod()->innerSteeringMethod()); | |
| 81 | ✗ | return (*sm)(q1, q2); | |
| 82 | } | ||
| 83 | ✗ | if (!problem_->constraintGraph()) | |
| 84 | ✗ | throw std::invalid_argument( | |
| 85 | ✗ | "The constraint graph should be set to use the steeringMethod::Graph"); | |
| 86 | ✗ | const graph::Graph& graph = *(problem_->constraintGraph()); | |
| 87 | try { | ||
| 88 | ✗ | possibleEdges = graph.getEdges(graph.getState(q1), graph.getState(q2)); | |
| 89 | ✗ | } catch (const std::logic_error& e) { | |
| 90 | hppDout(error, e.what()); | ||
| 91 | ✗ | return PathPtr_t(); | |
| 92 | } | ||
| 93 | ✗ | PathPtr_t path; | |
| 94 | ✗ | if (possibleEdges.empty()) { | |
| 95 | hppDout(info, "No edge found."); | ||
| 96 | } | ||
| 97 | ✗ | while (!possibleEdges.empty()) { | |
| 98 | ✗ | if (possibleEdges.back()->build(path, q1, q2)) { | |
| 99 | ✗ | return path; | |
| 100 | } | ||
| 101 | ✗ | possibleEdges.pop_back(); | |
| 102 | } | ||
| 103 | ✗ | return PathPtr_t(); | |
| 104 | } | ||
| 105 | } // namespace steeringMethod | ||
| 106 | } // namespace manipulation | ||
| 107 | } // namespace hpp | ||
| 108 |