| Directory: | ./ |
|---|---|
| File: | src/graph/state.cc |
| Date: | 2025-03-07 11:10:46 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 38 | 88 | 43.2% |
| Branches: | 30 | 158 | 19.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/graph/state.hh" | ||
| 30 | |||
| 31 | #include <hpp/constraints/differentiable-function.hh> | ||
| 32 | #include <hpp/constraints/solver/by-substitution.hh> | ||
| 33 | |||
| 34 | #include "hpp/manipulation/constraint-set.hh" | ||
| 35 | #include "hpp/manipulation/device.hh" | ||
| 36 | #include "hpp/manipulation/graph/edge.hh" | ||
| 37 | #include "hpp/manipulation/graph/graph.hh" | ||
| 38 | |||
| 39 | namespace hpp { | ||
| 40 | namespace manipulation { | ||
| 41 | namespace graph { | ||
| 42 | 4 | State::State(const std::string& name) | |
| 43 | 4 | : GraphComponent(name), configConstraints_(), isWaypoint_(false) {} | |
| 44 | |||
| 45 | ✗ | State::~State() {} | |
| 46 | |||
| 47 | 4 | StatePtr_t State::create(const std::string& name) { | |
| 48 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | State* state = new State(name); |
| 49 | 4 | StatePtr_t shPtr(state); | |
| 50 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | shPtr->init(shPtr); |
| 51 | 4 | return shPtr; | |
| 52 | } | ||
| 53 | |||
| 54 | 4 | void State::init(const StateWkPtr_t& weak) { | |
| 55 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | GraphComponent::init(weak); |
| 56 | 4 | wkPtr_ = weak; | |
| 57 | 4 | } | |
| 58 | |||
| 59 | 8 | EdgePtr_t State::linkTo(const std::string& name, const StatePtr_t& to, | |
| 60 | const size_type& w, EdgeFactory create) { | ||
| 61 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | EdgePtr_t newEdge = create(name, graph_, wkPtr_, to); |
| 62 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
8 | if (w >= 0) |
| 63 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | neighbors_.insert(newEdge, (Weight_t)w); |
| 64 | else | ||
| 65 | ✗ | hiddenNeighbors_.push_back(newEdge); | |
| 66 | 8 | return newEdge; | |
| 67 | } | ||
| 68 | |||
| 69 | 1 | bool State::contains(ConfigurationIn_t config) const { | |
| 70 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | return configConstraint()->isSatisfied(config); |
| 71 | } | ||
| 72 | |||
| 73 | ✗ | std::ostream& State::dotPrint(std::ostream& os, | |
| 74 | dot::DrawingAttributes da) const { | ||
| 75 | ✗ | da.insertWithQuote("label", name()); | |
| 76 | ✗ | da.insert("style", "filled"); | |
| 77 | ✗ | dot::Tooltip tp; | |
| 78 | ✗ | tp.addLine("State contains:"); | |
| 79 | ✗ | populateTooltip(tp); | |
| 80 | ✗ | da.insertWithQuote("tooltip", tp.toStr()); | |
| 81 | ✗ | os << id() << " " << da << ";" << std::endl; | |
| 82 | |||
| 83 | ✗ | dot::DrawingAttributes dac; | |
| 84 | ✗ | std::vector<double> p = neighbors_.probabilities(); | |
| 85 | ✗ | size_t i = 0; | |
| 86 | ✗ | for (Neighbors_t::const_iterator it = neighbors_.begin(); | |
| 87 | ✗ | it != neighbors_.end(); ++it) { | |
| 88 | ✗ | std::ostringstream oss; | |
| 89 | ✗ | oss << (p[i] * 3 + 0.5); | |
| 90 | ✗ | dac["penwidth"] = oss.str(); | |
| 91 | ✗ | i++; | |
| 92 | ✗ | it->second->dotPrint(os, dac) << std::endl; | |
| 93 | } | ||
| 94 | ✗ | return os; | |
| 95 | } | ||
| 96 | |||
| 97 | ✗ | void State::populateTooltip(dot::Tooltip& tp) const { | |
| 98 | ✗ | GraphComponent::populateTooltip(tp); | |
| 99 | ✗ | tp.addLine(""); | |
| 100 | ✗ | tp.addLine("Numerical constraints for paths are:"); | |
| 101 | ✗ | for (NumericalConstraints_t::const_iterator it = | |
| 102 | ✗ | numericalConstraintsForPath_.begin(); | |
| 103 | ✗ | it != numericalConstraintsForPath_.end(); ++it) { | |
| 104 | ✗ | tp.addLine("- " + (*it)->function().name()); | |
| 105 | } | ||
| 106 | } | ||
| 107 | |||
| 108 | ✗ | std::ostream& State::print(std::ostream& os) const { | |
| 109 | ✗ | os << "| |-- "; | |
| 110 | ✗ | GraphComponent::print(os) << std::endl; | |
| 111 | ✗ | for (Neighbors_t::const_iterator it = neighbors_.begin(); | |
| 112 | ✗ | it != neighbors_.end(); ++it) | |
| 113 | ✗ | os << *(it->second) << " - " << it->first << std::endl; | |
| 114 | ✗ | return os; | |
| 115 | } | ||
| 116 | |||
| 117 | 4 | void State::initialize() { | |
| 118 | 4 | isInit_ = true; | |
| 119 | |||
| 120 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | std::string n = "(" + name() + ")"; |
| 121 | 4 | GraphPtr_t g = graph_.lock(); | |
| 122 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | configConstraints_ = ConstraintSet::create(g->robot(), "Set " + n); |
| 123 | |||
| 124 | ConfigProjectorPtr_t proj = ConfigProjector::create( | ||
| 125 |
5/10✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
|
8 | g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations()); |
| 126 | 4 | proj->solver().solveLevelByLevel(this->solveLevelByLevel()); | |
| 127 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | g->insertNumericalConstraints(proj); |
| 128 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | insertNumericalConstraints(proj); |
| 129 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | configConstraints_->addConstraint(proj); |
| 130 | 4 | } | |
| 131 | |||
| 132 | ✗ | void State::updateWeight(const EdgePtr_t& e, const Weight_t& w) { | |
| 133 | ✗ | for (Neighbors_t::const_iterator it = neighbors_.begin(); | |
| 134 | ✗ | it != neighbors_.end(); ++it) { | |
| 135 | ✗ | if (it->second == e) { | |
| 136 | /// Update the weights | ||
| 137 | ✗ | neighbors_.insert(e, w); | |
| 138 | } | ||
| 139 | } | ||
| 140 | hppDout(error, "Edge not found"); | ||
| 141 | } | ||
| 142 | |||
| 143 | ✗ | Weight_t State::getWeight(const EdgePtr_t& e) { | |
| 144 | ✗ | for (Neighbors_t::const_iterator it = neighbors_.begin(); | |
| 145 | ✗ | it != neighbors_.end(); ++it) | |
| 146 | ✗ | if (it->second == e) return it->first; | |
| 147 | ✗ | for (std::vector<EdgePtr_t>::const_iterator it = hiddenNeighbors_.begin(); | |
| 148 | ✗ | it != hiddenNeighbors_.end(); ++it) | |
| 149 | ✗ | if (*it == e) return -1; | |
| 150 | hppDout(error, "Edge not found"); | ||
| 151 | ✗ | return 0; | |
| 152 | } | ||
| 153 | |||
| 154 | 1 | void State::addNumericalConstraint(const ImplicitPtr_t& numConstraint) { | |
| 155 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | ComparisonTypes_t comp(numConstraint->comparisonType()); |
| 156 |
1/2✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | for (constraints::ComparisonType c : comp) { |
| 157 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (c == constraints::Equality) { |
| 158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | throw std::logic_error( |
| 159 |
2/4✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | "Failed to insert constraint \"" + numConstraint->function().name() + |
| 160 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "\" as a state constraint since it contains a comparison " + |
| 161 | 2 | "of type Equality"); | |
| 162 | } | ||
| 163 | } | ||
| 164 | ✗ | GraphComponent::addNumericalConstraint(numConstraint); | |
| 165 | 1 | } | |
| 166 | |||
| 167 | } // namespace graph | ||
| 168 | } // namespace manipulation | ||
| 169 | } // namespace hpp | ||
| 170 |