| Directory: | ./ |
|---|---|
| File: | src/graph/edge.cc |
| Date: | 2025-03-07 11:10:46 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 90 | 405 | 22.2% |
| Branches: | 73 | 974 | 7.5% |
| 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/edge.hh" | ||
| 30 | |||
| 31 | #include <hpp/constraints/differentiable-function.hh> | ||
| 32 | #include <hpp/constraints/locked-joint.hh> | ||
| 33 | #include <hpp/constraints/solver/by-substitution.hh> | ||
| 34 | #include <hpp/core/obstacle-user.hh> | ||
| 35 | #include <hpp/core/path-validation.hh> | ||
| 36 | #include <hpp/core/path-vector.hh> | ||
| 37 | #include <hpp/pinocchio/configuration.hh> | ||
| 38 | #include <hpp/util/exception-factory.hh> | ||
| 39 | #include <hpp/util/pointer.hh> | ||
| 40 | #include <sstream> | ||
| 41 | |||
| 42 | #include "hpp/manipulation/constraint-set.hh" | ||
| 43 | #include "hpp/manipulation/device.hh" | ||
| 44 | #include "hpp/manipulation/graph/statistics.hh" | ||
| 45 | #include "hpp/manipulation/problem.hh" | ||
| 46 | #include "hpp/manipulation/steering-method/graph.hh" | ||
| 47 | |||
| 48 | namespace hpp { | ||
| 49 | namespace manipulation { | ||
| 50 | namespace graph { | ||
| 51 | 8 | Edge::Edge(const std::string& name) | |
| 52 | : GraphComponent(name), | ||
| 53 | 8 | isShort_(false), | |
| 54 | 8 | pathConstraints_(), | |
| 55 | 8 | targetConstraints_(), | |
| 56 | 8 | steeringMethod_(), | |
| 57 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | securityMargins_(), |
| 58 |
1/2✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
|
32 | pathValidation_() {} |
| 59 | |||
| 60 | ✗ | Edge::~Edge() {} | |
| 61 | |||
| 62 | 18 | StatePtr_t Edge::stateTo() const { return to_.lock(); } | |
| 63 | |||
| 64 | ✗ | StatePtr_t Edge::stateFrom() const { return from_.lock(); } | |
| 65 | |||
| 66 | ✗ | void Edge::relativeMotion(const RelativeMotion::matrix_type& m) { | |
| 67 | ✗ | if (!isInit_) | |
| 68 | ✗ | throw std::logic_error( | |
| 69 | "The graph must be initialized before changing the relative motion " | ||
| 70 | ✗ | "matrix."); | |
| 71 | shared_ptr<core::ObstacleUserInterface> oui = | ||
| 72 | ✗ | HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); | |
| 73 | ✗ | if (oui) oui->filterCollisionPairs(m); | |
| 74 | ✗ | relMotion_ = m; | |
| 75 | } | ||
| 76 | |||
| 77 | ✗ | bool Edge::direction(const core::PathPtr_t& path) const { | |
| 78 | ✗ | Configuration_t q0 = path->initial(), q1 = path->end(); | |
| 79 | ✗ | const bool src_contains_q0 = stateFrom()->contains(q0); | |
| 80 | ✗ | const bool dst_contains_q0 = stateTo()->contains(q0); | |
| 81 | ✗ | const bool src_contains_q1 = stateFrom()->contains(q1); | |
| 82 | ✗ | const bool dst_contains_q1 = stateTo()->contains(q1); | |
| 83 | // Karnaugh table: | ||
| 84 | // 1 = forward, 0 = reverse, ? = I don't know, * = 0 or 1 | ||
| 85 | // s0s1 \ d0d1 | 00 | 01 | 11 | 10 | ||
| 86 | // 00 | ? | ? | ? | ? | ||
| 87 | // 01 | ? | ? | 0 | 0 | ||
| 88 | // 11 | ? | 1 | * | 0 | ||
| 89 | // 10 | ? | 1 | 1 | 1 | ||
| 90 | // | ||
| 91 | /// true if reverse | ||
| 92 | ✗ | if ((!src_contains_q0 && !src_contains_q1) || | |
| 93 | ✗ | (!dst_contains_q0 && !dst_contains_q1) || | |
| 94 | ✗ | (!src_contains_q0 && !dst_contains_q0)) | |
| 95 | ✗ | HPP_THROW(std::runtime_error, | |
| 96 | "Edge " << name() << " does not seem to have generated path from" | ||
| 97 | << pinocchio::displayConfig(q0) << " to " | ||
| 98 | << pinocchio::displayConfig(q1)); | ||
| 99 | ✗ | return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1)); | |
| 100 | } | ||
| 101 | |||
| 102 | ✗ | void Edge::securityMarginForPair(const size_type& row, const size_type& col, | |
| 103 | const value_type& margin) { | ||
| 104 | ✗ | if ((row < 0) || (row >= securityMargins_.rows())) { | |
| 105 | ✗ | std::ostringstream os; | |
| 106 | ✗ | os << "Row index should be between 0 and " << securityMargins_.rows() + 1 | |
| 107 | ✗ | << ", got " << row << "."; | |
| 108 | ✗ | throw std::runtime_error(os.str().c_str()); | |
| 109 | } | ||
| 110 | ✗ | if ((col < 0) || (col >= securityMargins_.cols())) { | |
| 111 | ✗ | std::ostringstream os; | |
| 112 | ✗ | os << "Column index should be between 0 and " << securityMargins_.cols() + 1 | |
| 113 | ✗ | << ", got " << col << "."; | |
| 114 | ✗ | throw std::runtime_error(os.str().c_str()); | |
| 115 | } | ||
| 116 | ✗ | if (securityMargins_(row, col) != margin) { | |
| 117 | ✗ | securityMargins_(row, col) = margin; | |
| 118 | ✗ | securityMargins_(col, row) = margin; | |
| 119 | ✗ | invalidate(); | |
| 120 | } | ||
| 121 | } | ||
| 122 | |||
| 123 | ✗ | bool Edge::intersectionConstraint(const EdgePtr_t& other, | |
| 124 | ConfigProjectorPtr_t proj) const { | ||
| 125 | ✗ | GraphPtr_t g = graph_.lock(); | |
| 126 | |||
| 127 | ✗ | g->insertNumericalConstraints(proj); | |
| 128 | ✗ | insertNumericalConstraints(proj); | |
| 129 | ✗ | state()->insertNumericalConstraints(proj); | |
| 130 | |||
| 131 | ✗ | if (wkPtr_.lock() == other) // No intersection to be computed. | |
| 132 | ✗ | return false; | |
| 133 | |||
| 134 | ✗ | bool stateB_Eq_stateA = (state() == other->state()); | |
| 135 | |||
| 136 | ✗ | other->insertNumericalConstraints(proj); | |
| 137 | ✗ | if (!stateB_Eq_stateA) other->state()->insertNumericalConstraints(proj); | |
| 138 | ✗ | return true; | |
| 139 | } | ||
| 140 | |||
| 141 | 8 | EdgePtr_t Edge::create(const std::string& name, const GraphWkPtr_t& graph, | |
| 142 | const StateWkPtr_t& from, const StateWkPtr_t& to) { | ||
| 143 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | Edge* ptr = new Edge(name); |
| 144 | 8 | EdgePtr_t shPtr(ptr); | |
| 145 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | ptr->init(shPtr, graph, from, to); |
| 146 | 8 | return shPtr; | |
| 147 | } | ||
| 148 | |||
| 149 | 8 | void Edge::init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, | |
| 150 | const StateWkPtr_t& from, const StateWkPtr_t& to) { | ||
| 151 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | GraphComponent::init(weak); |
| 152 | 8 | parentGraph(graph); | |
| 153 | 8 | wkPtr_ = weak; | |
| 154 | 8 | from_ = from; | |
| 155 | 8 | to_ = to; | |
| 156 | 8 | state_ = to; | |
| 157 | // add 1 joint for the environment | ||
| 158 |
2/4✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
|
8 | size_type n(graph.lock()->robot()->nbJoints() + 1); |
| 159 | 8 | securityMargins_.resize(n, n); | |
| 160 | 8 | securityMargins_.setZero(); | |
| 161 | 8 | } | |
| 162 | |||
| 163 | 8 | void Edge::initialize() { | |
| 164 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
8 | if (!isInit_) { |
| 165 | 8 | targetConstraints_ = buildTargetConstraint(); | |
| 166 | 8 | pathConstraints_ = buildPathConstraint(); | |
| 167 | } | ||
| 168 | 8 | isInit_ = true; | |
| 169 | 8 | } | |
| 170 | |||
| 171 | ✗ | std::ostream& Edge::print(std::ostream& os) const { | |
| 172 | ✗ | os << "| | |-- "; | |
| 173 | ✗ | GraphComponent::print(os) << " --> " << to_.lock()->name(); | |
| 174 | ✗ | return os; | |
| 175 | } | ||
| 176 | |||
| 177 | ✗ | std::ostream& Edge::dotPrint(std::ostream& os, | |
| 178 | dot::DrawingAttributes da) const { | ||
| 179 | ✗ | da.insertWithQuote("label", name()); | |
| 180 | ✗ | da.insert("shape", "onormal"); | |
| 181 | ✗ | dot::Tooltip tp; | |
| 182 | ✗ | tp.addLine("Edge constains:"); | |
| 183 | ✗ | populateTooltip(tp); | |
| 184 | ✗ | da.insertWithQuote("tooltip", tp.toStr()); | |
| 185 | ✗ | da.insertWithQuote("labeltooltip", tp.toStr()); | |
| 186 | ✗ | os << stateFrom()->id() << " -> " << stateTo()->id() << " " << da << ";"; | |
| 187 | ✗ | return os; | |
| 188 | } | ||
| 189 | |||
| 190 | 4 | ConstraintSetPtr_t Edge::targetConstraint() const { | |
| 191 | 4 | throwIfNotInitialized(); | |
| 192 | ✗ | return targetConstraints_; | |
| 193 | } | ||
| 194 | |||
| 195 | // Merge constraints of several graph components into a config projector | ||
| 196 | // Replace constraints and complement by combination of both when | ||
| 197 | // necessary. | ||
| 198 | 16 | static void mergeConstraintsIntoConfigProjector( | |
| 199 | const ConfigProjectorPtr_t& proj, | ||
| 200 | const std::vector<GraphComponentPtr_t>& components, | ||
| 201 | const GraphPtr_t& graph) { | ||
| 202 | 16 | NumericalConstraints_t nc; | |
| 203 |
2/2✓ Branch 4 taken 48 times.
✓ Branch 5 taken 16 times.
|
64 | for (const auto& gc : components) |
| 204 |
2/4✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 48 times.
✗ Branch 9 not taken.
|
48 | nc.insert(nc.end(), gc->numericalConstraints().begin(), |
| 205 |
1/2✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
|
48 | gc->numericalConstraints().end()); |
| 206 | |||
| 207 | // Remove duplicate constraints | ||
| 208 | 16 | auto end = nc.end(); | |
| 209 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
|
16 | for (auto it = nc.begin(); it != end; ++it) |
| 210 | ✗ | end = std::remove(std::next(it), end, *it); | |
| 211 |
1/2✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
|
16 | nc.erase(end, nc.end()); |
| 212 | |||
| 213 | 16 | NumericalConstraints_t::iterator itnc1, itnc2; | |
| 214 | |||
| 215 | // Look for complement | ||
| 216 |
1/2✗ Branch 4 not taken.
✓ Branch 5 taken 16 times.
|
16 | for (itnc1 = nc.begin(); itnc1 != nc.end(); ++itnc1) { |
| 217 | ✗ | const auto& c1 = *itnc1; | |
| 218 | ✗ | itnc2 = std::next(itnc1); | |
| 219 | ✗ | constraints::ImplicitPtr_t combination; | |
| 220 | ✗ | itnc2 = std::find_if(std::next(itnc1), nc.end(), | |
| 221 | ✗ | [&c1, &combination, &graph](const auto& c2) -> bool { | |
| 222 | ✗ | assert(c1 != c2); | |
| 223 | ✗ | return graph->isComplement(c1, c2, combination) || | |
| 224 | ✗ | graph->isComplement(c2, c1, combination); | |
| 225 | }); | ||
| 226 | ✗ | if (itnc2 != nc.end()) { | |
| 227 | ✗ | assert(*itnc1 != *itnc2); | |
| 228 | ✗ | *itnc1 = combination; | |
| 229 | ✗ | nc.erase(itnc2); | |
| 230 | } | ||
| 231 | } | ||
| 232 | |||
| 233 |
1/4✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 16 times.
|
16 | for (const auto& _nc : nc) proj->add(_nc); |
| 234 | // Insert numerical costs | ||
| 235 | 16 | nc.clear(); | |
| 236 |
2/2✓ Branch 4 taken 48 times.
✓ Branch 5 taken 16 times.
|
64 | for (const auto& gc : components) |
| 237 |
2/4✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 48 times.
✗ Branch 9 not taken.
|
48 | nc.insert(nc.end(), gc->numericalCosts().begin(), |
| 238 |
1/2✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
|
48 | gc->numericalCosts().end()); |
| 239 |
1/4✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 16 times.
|
16 | for (const auto& _nc : nc) proj->add(_nc, 1); |
| 240 | 16 | } | |
| 241 | |||
| 242 | 8 | ConstraintSetPtr_t Edge::buildTargetConstraint() { | |
| 243 |
3/6✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
|
8 | std::string n = "(" + name() + ")"; |
| 244 | 8 | GraphPtr_t g = graph_.lock(); | |
| 245 | |||
| 246 |
3/6✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
|
8 | ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n); |
| 247 | |||
| 248 | ConfigProjectorPtr_t proj = ConfigProjector::create( | ||
| 249 |
5/10✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 8 times.
✗ Branch 18 not taken.
|
16 | g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); |
| 250 | 8 | proj->solver().solveLevelByLevel(this->solveLevelByLevel()); | |
| 251 | 8 | std::vector<GraphComponentPtr_t> components; | |
| 252 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | components.push_back(g); |
| 253 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | components.push_back(wkPtr_.lock()); |
| 254 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
8 | components.push_back(stateTo()); |
| 255 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 8 times.
|
8 | if (state() != stateTo()) { |
| 256 | ✗ | components.push_back(state()); | |
| 257 | } | ||
| 258 | // Copy constraints from | ||
| 259 | // - graph, | ||
| 260 | // - this edge, | ||
| 261 | // - the destination state, | ||
| 262 | // - the state in which the transition lies if different | ||
| 263 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | mergeConstraintsIntoConfigProjector(proj, components, parentGraph()); |
| 264 | |||
| 265 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | constraint->addConstraint(proj); |
| 266 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | constraint->edge(wkPtr_.lock()); |
| 267 | 16 | return constraint; | |
| 268 | 8 | } | |
| 269 | |||
| 270 | ✗ | ConstraintSetPtr_t Edge::pathConstraint() const { | |
| 271 | ✗ | throwIfNotInitialized(); | |
| 272 | ✗ | return pathConstraints_; | |
| 273 | } | ||
| 274 | |||
| 275 | 8 | ConstraintSetPtr_t Edge::buildPathConstraint() { | |
| 276 |
3/6✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
|
8 | std::string n = "(" + name() + ")"; |
| 277 | 8 | GraphPtr_t g = graph_.lock(); | |
| 278 | |||
| 279 |
3/6✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
|
8 | ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n); |
| 280 | |||
| 281 | ConfigProjectorPtr_t proj = ConfigProjector::create( | ||
| 282 |
5/10✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 8 times.
✗ Branch 18 not taken.
|
16 | g->robot(), "proj_" + n, .5 * g->errorThreshold(), g->maxIterations()); |
| 283 | 8 | proj->solver().solveLevelByLevel(this->solveLevelByLevel()); | |
| 284 | 8 | std::vector<GraphComponentPtr_t> components; | |
| 285 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | components.push_back(g); |
| 286 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | components.push_back(wkPtr_.lock()); |
| 287 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | components.push_back(state()); |
| 288 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | mergeConstraintsIntoConfigProjector(proj, components, parentGraph()); |
| 289 | |||
| 290 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | constraint->addConstraint(proj); |
| 291 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | constraint->edge(wkPtr_.lock()); |
| 292 | |||
| 293 | // Build steering method | ||
| 294 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | const ProblemPtr_t& problem(g->problem()); |
| 295 | steeringMethod_ = | ||
| 296 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
|
8 | problem->manipulationSteeringMethod()->innerSteeringMethod()->copy(); |
| 297 | 8 | steeringMethod_->constraints(constraint); | |
| 298 | // Build path validation and relative motion matrix | ||
| 299 | // TODO this path validation will not contain obstacles added after | ||
| 300 | // its creation. | ||
| 301 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | pathValidation_ = problem->pathValidationFactory(); |
| 302 | shared_ptr<core::ObstacleUserInterface> oui = | ||
| 303 | 8 | HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); | |
| 304 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (oui) { |
| 305 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
|
8 | relMotion_ = RelativeMotion::matrix(g->robot()); |
| 306 |
2/4✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
|
8 | RelativeMotion::fromConstraint(relMotion_, g->robot(), constraint); |
| 307 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | oui->filterCollisionPairs(relMotion_); |
| 308 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | oui->setSecurityMargins(securityMargins_); |
| 309 | } | ||
| 310 | 16 | return constraint; | |
| 311 | 8 | } | |
| 312 | |||
| 313 | ✗ | bool Edge::canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const { | |
| 314 | ✗ | ConstraintSetPtr_t constraints = pathConstraint(); | |
| 315 | ✗ | constraints->configProjector()->rightHandSideFromConfig(q1); | |
| 316 | ✗ | if (!constraints->isSatisfied(q1) || !constraints->isSatisfied(q2)) { | |
| 317 | ✗ | return false; | |
| 318 | } | ||
| 319 | ✗ | return true; | |
| 320 | } | ||
| 321 | |||
| 322 | ✗ | bool Edge::build(core::PathPtr_t& path, ConfigurationIn_t q1, | |
| 323 | ConfigurationIn_t q2) const { | ||
| 324 | using pinocchio::displayConfig; | ||
| 325 | ✗ | if (!steeringMethod_) { | |
| 326 | ✗ | std::ostringstream oss; | |
| 327 | ✗ | oss << "No steering method set in edge " << name() << "."; | |
| 328 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
| 329 | } | ||
| 330 | ✗ | ConstraintSetPtr_t constraints = pathConstraint(); | |
| 331 | ✗ | constraints->configProjector()->rightHandSideFromConfig(q1); | |
| 332 | ✗ | if (constraints->isSatisfied(q1)) { | |
| 333 | ✗ | if (constraints->isSatisfied(q2)) { | |
| 334 | ✗ | path = (*steeringMethod_)(q1, q2); | |
| 335 | ✗ | return (bool)path; | |
| 336 | } else { | ||
| 337 | hppDout(info, "q2 = " << displayConfig(q2) | ||
| 338 | << " does not satisfy the constraints of edge " | ||
| 339 | << name()); | ||
| 340 | hppDout(info, "q1 = " << displayConfig(q1)); | ||
| 341 | ✗ | return false; | |
| 342 | } | ||
| 343 | } else { | ||
| 344 | ✗ | value_type th(constraints->configProjector()->errorThreshold()); | |
| 345 | ✗ | if (!constraints->configProjector()->isSatisfied(q1, 2 * th)) { | |
| 346 | ✗ | std::ostringstream oss; | |
| 347 | ✗ | oss << "The initial configuration " << displayConfig(q1) | |
| 348 | << " does not satisfy the constraints of" | ||
| 349 | " edge " | ||
| 350 | ✗ | << name() << "." << std::endl; | |
| 351 | ✗ | oss << "The graph is probably malformed"; | |
| 352 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
| 353 | } | ||
| 354 | // q1 may slightly violate the edge constraint eventhough the graph | ||
| 355 | // is well constructed. | ||
| 356 | ✗ | return false; | |
| 357 | } | ||
| 358 | } | ||
| 359 | |||
| 360 | ✗ | bool Edge::generateTargetConfig(core::NodePtr_t nStart, | |
| 361 | ConfigurationOut_t q) const { | ||
| 362 | ✗ | return generateTargetConfig(nStart->configuration(), q); | |
| 363 | } | ||
| 364 | |||
| 365 | ✗ | bool Edge::generateTargetConfig(ConfigurationIn_t qStart, | |
| 366 | ConfigurationOut_t q) const { | ||
| 367 | ✗ | ConstraintSetPtr_t c = targetConstraint(); | |
| 368 | ✗ | ConfigProjectorPtr_t proj = c->configProjector(); | |
| 369 | ✗ | proj->rightHandSideFromConfig(qStart); | |
| 370 | ✗ | if (isShort_) q = qStart; | |
| 371 | ✗ | if (c->apply(q)) return true; | |
| 372 | ✗ | const ::hpp::statistics::SuccessStatistics& ss = proj->statistics(); | |
| 373 | ✗ | if (ss.nbFailure() > ss.nbSuccess()) { | |
| 374 | hppDout(warning, c->name() << " fails often.\n" << ss); | ||
| 375 | } else { | ||
| 376 | hppDout(warning, c->name() << " succeeds at rate " | ||
| 377 | << (value_type)(ss.nbSuccess()) / | ||
| 378 | (value_type)ss.numberOfObservations() | ||
| 379 | << "."); | ||
| 380 | } | ||
| 381 | ✗ | return false; | |
| 382 | } | ||
| 383 | |||
| 384 | ✗ | WaypointEdgePtr_t WaypointEdge::create(const std::string& name, | |
| 385 | const GraphWkPtr_t& graph, | ||
| 386 | const StateWkPtr_t& from, | ||
| 387 | const StateWkPtr_t& to) { | ||
| 388 | ✗ | WaypointEdge* ptr = new WaypointEdge(name); | |
| 389 | ✗ | WaypointEdgePtr_t shPtr(ptr); | |
| 390 | ✗ | ptr->init(shPtr, graph, from, to); | |
| 391 | ✗ | return shPtr; | |
| 392 | } | ||
| 393 | |||
| 394 | ✗ | void WaypointEdge::init(const WaypointEdgeWkPtr_t& weak, | |
| 395 | const GraphWkPtr_t& graph, const StateWkPtr_t& from, | ||
| 396 | const StateWkPtr_t& to) { | ||
| 397 | ✗ | Edge::init(weak, graph, from, to); | |
| 398 | ✗ | nbWaypoints(0); | |
| 399 | ✗ | wkPtr_ = weak; | |
| 400 | } | ||
| 401 | |||
| 402 | ✗ | void WaypointEdge::initialize() { | |
| 403 | ✗ | Edge::initialize(); | |
| 404 | // Set error threshold of internal edge to error threshold of | ||
| 405 | // waypoint edge divided by number of edges. | ||
| 406 | ✗ | assert(targetConstraint()->configProjector()); | |
| 407 | ✗ | value_type eps(targetConstraint()->configProjector()->errorThreshold() / | |
| 408 | ✗ | (value_type)edges_.size()); | |
| 409 | ✗ | for (Edges_t::iterator it(edges_.begin()); it != edges_.end(); ++it) { | |
| 410 | ✗ | (*it)->initialize(); | |
| 411 | ✗ | assert((*it)->targetConstraint()); | |
| 412 | ✗ | assert((*it)->targetConstraint()->configProjector()); | |
| 413 | ✗ | (*it)->targetConstraint()->configProjector()->errorThreshold(eps); | |
| 414 | } | ||
| 415 | } | ||
| 416 | |||
| 417 | ✗ | bool WaypointEdge::canConnect(ConfigurationIn_t q1, | |
| 418 | ConfigurationIn_t q2) const { | ||
| 419 | /// TODO: This is not correct | ||
| 420 | ✗ | for (std::size_t i = 0; i < edges_.size(); ++i) | |
| 421 | ✗ | if (!edges_[i]->canConnect(q1, q2)) return false; | |
| 422 | ✗ | return true; | |
| 423 | } | ||
| 424 | |||
| 425 | ✗ | bool WaypointEdge::build(core::PathPtr_t& path, ConfigurationIn_t q1, | |
| 426 | ConfigurationIn_t q2) const { | ||
| 427 | ✗ | core::PathPtr_t p; | |
| 428 | core::PathVectorPtr_t pv = | ||
| 429 | ✗ | core::PathVector::create(graph_.lock()->robot()->configSize(), | |
| 430 | ✗ | graph_.lock()->robot()->numberDof()); | |
| 431 | // Many times, this will be called rigth after | ||
| 432 | // WaypointEdge::generateTargetConfig so config_ already satisfies the | ||
| 433 | // constraints. | ||
| 434 | ✗ | size_type n = edges_.size(); | |
| 435 | ✗ | assert(configs_.cols() == n + 1); | |
| 436 | ✗ | bool useCache = lastSucceeded_ && configs_.col(0).isApprox(q1) && | |
| 437 | ✗ | configs_.col(n).isApprox(q2); | |
| 438 | ✗ | configs_.col(0) = q1; | |
| 439 | ✗ | configs_.col(n) = q2; | |
| 440 | |||
| 441 | ✗ | for (size_type i = 0; i < n; ++i) { | |
| 442 | ✗ | if (i < (n - 1) && !useCache) configs_.col(i + 1) = q2; | |
| 443 | ✗ | if (i < (n - 1) && !edges_[i]->generateTargetConfig(configs_.col(i), | |
| 444 | ✗ | configs_.col(i + 1))) { | |
| 445 | hppDout(info, "Waypoint edge " | ||
| 446 | << name() | ||
| 447 | << ": generateTargetConfig failed at waypoint " << i | ||
| 448 | << "." | ||
| 449 | << "\nUse cache: " << useCache); | ||
| 450 | ✗ | lastSucceeded_ = false; | |
| 451 | ✗ | return false; | |
| 452 | } | ||
| 453 | ✗ | assert(targetConstraint()); | |
| 454 | ✗ | assert(targetConstraint()->configProjector()); | |
| 455 | ✗ | value_type eps(targetConstraint()->configProjector()->errorThreshold()); | |
| 456 | ✗ | if ((configs_.col(i) - configs_.col(i + 1)).squaredNorm() > eps * eps) { | |
| 457 | ✗ | if (!edges_[i]->build(p, configs_.col(i), configs_.col(i + 1))) { | |
| 458 | hppDout(info, "Waypoint edge " | ||
| 459 | << name() << ": build failed at waypoint " << i << "." | ||
| 460 | << "\nUse cache: " << useCache); | ||
| 461 | ✗ | lastSucceeded_ = false; | |
| 462 | ✗ | return false; | |
| 463 | } | ||
| 464 | ✗ | pv->appendPath(p); | |
| 465 | } | ||
| 466 | } | ||
| 467 | |||
| 468 | ✗ | path = pv; | |
| 469 | ✗ | lastSucceeded_ = true; | |
| 470 | ✗ | return true; | |
| 471 | } | ||
| 472 | |||
| 473 | ✗ | bool WaypointEdge::generateTargetConfig(ConfigurationIn_t qStart, | |
| 474 | ConfigurationOut_t q) const { | ||
| 475 | ✗ | assert(configs_.cols() == size_type(edges_.size() + 1)); | |
| 476 | ✗ | configs_.col(0) = qStart; | |
| 477 | ✗ | for (std::size_t i = 0; i < edges_.size(); ++i) { | |
| 478 | ✗ | configs_.col(i + 1) = q; | |
| 479 | ✗ | if (!edges_[i]->generateTargetConfig(configs_.col(i), | |
| 480 | ✗ | configs_.col(i + 1))) { | |
| 481 | ✗ | q = configs_.col(i + 1); | |
| 482 | ✗ | lastSucceeded_ = false; | |
| 483 | ✗ | return false; | |
| 484 | } | ||
| 485 | } | ||
| 486 | ✗ | q = configs_.col(edges_.size()); | |
| 487 | ✗ | lastSucceeded_ = true; | |
| 488 | ✗ | return true; | |
| 489 | } | ||
| 490 | |||
| 491 | ✗ | void WaypointEdge::nbWaypoints(const size_type number) { | |
| 492 | ✗ | edges_.resize(number + 1); | |
| 493 | ✗ | states_.resize(number + 1); | |
| 494 | ✗ | states_.back() = stateTo(); | |
| 495 | ✗ | const size_type nbDof = graph_.lock()->robot()->configSize(); | |
| 496 | ✗ | configs_ = matrix_t(nbDof, number + 2); | |
| 497 | ✗ | invalidate(); | |
| 498 | } | ||
| 499 | |||
| 500 | ✗ | void WaypointEdge::setWaypoint(const std::size_t index, const EdgePtr_t wEdge, | |
| 501 | const StatePtr_t wTo) { | ||
| 502 | ✗ | assert(edges_.size() == states_.size()); | |
| 503 | ✗ | assert(index < edges_.size()); | |
| 504 | ✗ | if (index == states_.size() - 1) { | |
| 505 | ✗ | assert(!wTo || wTo == stateTo()); | |
| 506 | } else { | ||
| 507 | ✗ | states_[index] = wTo; | |
| 508 | } | ||
| 509 | ✗ | edges_[index] = wEdge; | |
| 510 | } | ||
| 511 | |||
| 512 | ✗ | const EdgePtr_t& WaypointEdge::waypoint(const std::size_t index) const { | |
| 513 | ✗ | assert(index < edges_.size()); | |
| 514 | ✗ | return edges_[index]; | |
| 515 | } | ||
| 516 | |||
| 517 | ✗ | std::ostream& WaypointEdge::print(std::ostream& os) const { | |
| 518 | ✗ | os << "| | |-- "; | |
| 519 | ✗ | GraphComponent::print(os) << " (waypoint) --> " << stateTo()->name(); | |
| 520 | ✗ | return os; | |
| 521 | } | ||
| 522 | |||
| 523 | ✗ | std::ostream& WaypointEdge::dotPrint(std::ostream& os, | |
| 524 | dot::DrawingAttributes da) const { | ||
| 525 | // First print the waypoint node, then the first edge. | ||
| 526 | ✗ | da["style"] = "dashed"; | |
| 527 | ✗ | for (std::size_t i = 0; i < states_.size() - 1; ++i) | |
| 528 | ✗ | states_[i]->dotPrint(os, da); | |
| 529 | |||
| 530 | ✗ | da["style"] = "solid"; | |
| 531 | ✗ | for (std::size_t i = 0; i < edges_.size(); ++i) | |
| 532 | ✗ | edges_[i]->dotPrint(os, da) << std::endl; | |
| 533 | |||
| 534 | ✗ | da["style"] = "dotted"; | |
| 535 | ✗ | da["dir"] = "both"; | |
| 536 | ✗ | da["arrowtail"] = "dot"; | |
| 537 | |||
| 538 | ✗ | return Edge::dotPrint(os, da); | |
| 539 | } | ||
| 540 | |||
| 541 | ✗ | std::ostream& LevelSetEdge::print(std::ostream& os) const { | |
| 542 | ✗ | os << "| | |-- "; | |
| 543 | ✗ | GraphComponent::print(os) << " (level set) --> " << stateTo()->name(); | |
| 544 | ✗ | return os; | |
| 545 | } | ||
| 546 | |||
| 547 | ✗ | std::ostream& LevelSetEdge::dotPrint(std::ostream& os, | |
| 548 | dot::DrawingAttributes da) const { | ||
| 549 | ✗ | da.insert("shape", "onormal"); | |
| 550 | ✗ | da.insert("style", "dashed"); | |
| 551 | ✗ | return Edge::dotPrint(os, da); | |
| 552 | } | ||
| 553 | |||
| 554 | ✗ | void LevelSetEdge::populateTooltip(dot::Tooltip& tp) const { | |
| 555 | ✗ | GraphComponent::populateTooltip(tp); | |
| 556 | ✗ | tp.addLine(""); | |
| 557 | ✗ | tp.addLine("Foliation condition constraints:"); | |
| 558 | ✗ | for (NumericalConstraints_t::const_iterator it = | |
| 559 | ✗ | condNumericalConstraints_.begin(); | |
| 560 | ✗ | it != condNumericalConstraints_.end(); ++it) { | |
| 561 | ✗ | tp.addLine("- " + (*it)->function().name()); | |
| 562 | } | ||
| 563 | ✗ | tp.addLine("Foliation parametrization constraints:"); | |
| 564 | ✗ | for (NumericalConstraints_t::const_iterator it = | |
| 565 | ✗ | paramNumericalConstraints_.begin(); | |
| 566 | ✗ | it != paramNumericalConstraints_.end(); ++it) { | |
| 567 | ✗ | tp.addLine("- " + (*it)->function().name()); | |
| 568 | } | ||
| 569 | } | ||
| 570 | |||
| 571 | ✗ | bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart, | |
| 572 | ConfigurationOut_t q) const { | ||
| 573 | // First, get an offset from the histogram | ||
| 574 | statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib = | ||
| 575 | ✗ | hist_->getDistrib(); | |
| 576 | ✗ | if (distrib.size() == 0) { | |
| 577 | hppDout(warning, "Edge " << name() << ": Distrib is empty"); | ||
| 578 | ✗ | return false; | |
| 579 | } | ||
| 580 | ✗ | const Configuration_t& qLeaf = distrib()->configuration(); | |
| 581 | |||
| 582 | ✗ | return generateTargetConfigOnLeaf(qStart, qLeaf, q); | |
| 583 | } | ||
| 584 | |||
| 585 | ✗ | bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart, | |
| 586 | ConfigurationOut_t q) const { | ||
| 587 | // First, get an offset from the histogram that is not in the same connected | ||
| 588 | // component. | ||
| 589 | statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib = | ||
| 590 | ✗ | hist_->getDistribOutOfConnectedComponent(nStart->connectedComponent()); | |
| 591 | ✗ | if (distrib.size() == 0) { | |
| 592 | hppDout(warning, "Edge " << name() << ": Distrib is empty"); | ||
| 593 | ✗ | return false; | |
| 594 | } | ||
| 595 | ✗ | const Configuration_t &qLeaf = distrib()->configuration(), | |
| 596 | ✗ | qStart = nStart->configuration(); | |
| 597 | |||
| 598 | ✗ | return generateTargetConfigOnLeaf(qStart, qLeaf, q); | |
| 599 | } | ||
| 600 | |||
| 601 | ✗ | bool LevelSetEdge::generateTargetConfigOnLeaf(ConfigurationIn_t qStart, | |
| 602 | ConfigurationIn_t qLeaf, | ||
| 603 | ConfigurationOut_t q) const { | ||
| 604 | // First, set the offset. | ||
| 605 | ✗ | ConstraintSetPtr_t cs = targetConstraint(); | |
| 606 | ✗ | const ConfigProjectorPtr_t cp = cs->configProjector(); | |
| 607 | ✗ | assert(cp); | |
| 608 | |||
| 609 | // Set right hand side of edge constraints with qStart | ||
| 610 | ✗ | cp->rightHandSideFromConfig(qStart); | |
| 611 | // Set right hand side of constraints parameterizing the target state | ||
| 612 | // foliation with qLeaf. | ||
| 613 | ✗ | for (NumericalConstraints_t::const_iterator it = | |
| 614 | ✗ | paramNumericalConstraints_.begin(); | |
| 615 | ✗ | it != paramNumericalConstraints_.end(); ++it) { | |
| 616 | ✗ | cp->rightHandSideFromConfig(*it, qLeaf); | |
| 617 | } | ||
| 618 | |||
| 619 | // Eventually, do the projection. | ||
| 620 | ✗ | if (isShort_) q = qStart; | |
| 621 | ✗ | if (cs->apply(q)) return true; | |
| 622 | ✗ | ::hpp::statistics::SuccessStatistics& ss = cp->statistics(); | |
| 623 | ✗ | if (ss.nbFailure() > ss.nbSuccess()) { | |
| 624 | hppDout(warning, cs->name() << " fails often." << std::endl << ss); | ||
| 625 | } else { | ||
| 626 | hppDout(warning, cs->name() << " succeeds at rate " | ||
| 627 | << (value_type)(ss.nbSuccess()) / | ||
| 628 | (value_type)ss.numberOfObservations() | ||
| 629 | << "."); | ||
| 630 | } | ||
| 631 | ✗ | return false; | |
| 632 | } | ||
| 633 | |||
| 634 | ✗ | void LevelSetEdge::init(const LevelSetEdgeWkPtr_t& weak, | |
| 635 | const GraphWkPtr_t& graph, const StateWkPtr_t& from, | ||
| 636 | const StateWkPtr_t& to) { | ||
| 637 | ✗ | Edge::init(weak, graph, from, to); | |
| 638 | ✗ | wkPtr_ = weak; | |
| 639 | } | ||
| 640 | |||
| 641 | ✗ | LevelSetEdgePtr_t LevelSetEdge::create(const std::string& name, | |
| 642 | const GraphWkPtr_t& graph, | ||
| 643 | const StateWkPtr_t& from, | ||
| 644 | const StateWkPtr_t& to) { | ||
| 645 | ✗ | LevelSetEdge* ptr = new LevelSetEdge(name); | |
| 646 | ✗ | LevelSetEdgePtr_t shPtr(ptr); | |
| 647 | ✗ | ptr->init(shPtr, graph, from, to); | |
| 648 | ✗ | return shPtr; | |
| 649 | } | ||
| 650 | |||
| 651 | ✗ | LeafHistogramPtr_t LevelSetEdge::histogram() const { return hist_; } | |
| 652 | |||
| 653 | ✗ | void LevelSetEdge::buildHistogram() { | |
| 654 | ✗ | Foliation f; | |
| 655 | |||
| 656 | /// Build the constraint set. | ||
| 657 | ✗ | std::string n = "(" + name() + ")"; | |
| 658 | ✗ | GraphPtr_t g = graph_.lock(); | |
| 659 | |||
| 660 | // The parametrizer | ||
| 661 | ✗ | ConstraintSetPtr_t param = ConstraintSet::create(g->robot(), "Set " + n); | |
| 662 | |||
| 663 | ConfigProjectorPtr_t proj = ConfigProjector::create( | ||
| 664 | ✗ | g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations()); | |
| 665 | |||
| 666 | ✗ | for (const auto& nc : paramNumericalConstraints_) proj->add(nc); | |
| 667 | |||
| 668 | ✗ | param->addConstraint(proj); | |
| 669 | ✗ | param->edge(wkPtr_.lock()); | |
| 670 | |||
| 671 | ✗ | f.parametrizer(param); | |
| 672 | |||
| 673 | // The codition | ||
| 674 | // TODO: We assumed that this part of the code can only be reached by | ||
| 675 | // configurations that are valid. | ||
| 676 | // It would be wiser to make sure configurations are valid, for instance | ||
| 677 | // by considering only configurations in the destination state of this | ||
| 678 | // edge. | ||
| 679 | ✗ | ConstraintSetPtr_t cond = ConstraintSet::create(g->robot(), "Set " + n); | |
| 680 | ✗ | proj = ConfigProjector::create(g->robot(), "projCond_" + n, | |
| 681 | ✗ | g->errorThreshold(), g->maxIterations()); | |
| 682 | |||
| 683 | ✗ | for (const auto& nc : condNumericalConstraints_) proj->add(nc); | |
| 684 | |||
| 685 | ✗ | f.condition(cond); | |
| 686 | ✗ | cond->addConstraint(proj); | |
| 687 | |||
| 688 | hppDout(info, "Build histogram of LevelSetEdge " << name() | ||
| 689 | << "\nParametrizer:\n" | ||
| 690 | << *param << "\nCondition:\n" | ||
| 691 | << *cond); | ||
| 692 | |||
| 693 | // TODO: If hist_ is not NULL, remove the previous Histogram. | ||
| 694 | // It should not be of any use and it slows down node insertion in the | ||
| 695 | // roadmap. | ||
| 696 | ✗ | hist_ = LeafHistogram::create(f); | |
| 697 | ✗ | g->insertHistogram(hist_); | |
| 698 | } | ||
| 699 | |||
| 700 | ✗ | void LevelSetEdge::initialize() { | |
| 701 | ✗ | if (!isInit_) { | |
| 702 | ✗ | Edge::initialize(); | |
| 703 | ✗ | buildHistogram(); | |
| 704 | } | ||
| 705 | } | ||
| 706 | |||
| 707 | ✗ | ConstraintSetPtr_t LevelSetEdge::buildTargetConstraint() { | |
| 708 | ✗ | std::string n = "(" + name() + ")"; | |
| 709 | ✗ | GraphPtr_t g = graph_.lock(); | |
| 710 | |||
| 711 | ✗ | ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n); | |
| 712 | |||
| 713 | ConfigProjectorPtr_t proj = ConfigProjector::create( | ||
| 714 | ✗ | g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); | |
| 715 | |||
| 716 | // Copy constraints from | ||
| 717 | // - graph, | ||
| 718 | // - param numerical constraints | ||
| 719 | // - this edge, | ||
| 720 | // - the destination state, | ||
| 721 | // - the state in which the transition lies if different | ||
| 722 | |||
| 723 | ✗ | g->insertNumericalConstraints(proj); | |
| 724 | ✗ | for (const auto& nc : paramNumericalConstraints_) proj->add(nc); | |
| 725 | |||
| 726 | ✗ | insertNumericalConstraints(proj); | |
| 727 | ✗ | stateTo()->insertNumericalConstraints(proj); | |
| 728 | ✗ | if (state() != stateTo()) { | |
| 729 | ✗ | state()->insertNumericalConstraints(proj); | |
| 730 | } | ||
| 731 | ✗ | constraint->addConstraint(proj); | |
| 732 | |||
| 733 | ✗ | constraint->edge(wkPtr_.lock()); | |
| 734 | ✗ | return constraint; | |
| 735 | } | ||
| 736 | |||
| 737 | ✗ | void LevelSetEdge::insertParamConstraint(const constraints::ImplicitPtr_t& nm) { | |
| 738 | ✗ | invalidate(); | |
| 739 | ✗ | paramNumericalConstraints_.push_back(nm); | |
| 740 | } | ||
| 741 | |||
| 742 | ✗ | const NumericalConstraints_t& LevelSetEdge::paramConstraints() const { | |
| 743 | ✗ | return paramNumericalConstraints_; | |
| 744 | } | ||
| 745 | |||
| 746 | ✗ | void LevelSetEdge::insertConditionConstraint( | |
| 747 | const constraints::ImplicitPtr_t& nm) { | ||
| 748 | ✗ | invalidate(); | |
| 749 | ✗ | condNumericalConstraints_.push_back(nm); | |
| 750 | } | ||
| 751 | |||
| 752 | ✗ | const NumericalConstraints_t& LevelSetEdge::conditionConstraints() const { | |
| 753 | ✗ | return condNumericalConstraints_; | |
| 754 | } | ||
| 755 | |||
| 756 | ✗ | LevelSetEdge::LevelSetEdge(const std::string& name) : Edge(name) {} | |
| 757 | |||
| 758 | ✗ | LevelSetEdge::~LevelSetEdge() {} | |
| 759 | } // namespace graph | ||
| 760 | } // namespace manipulation | ||
| 761 | } // namespace hpp | ||
| 762 |