diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc index f50e2c905a1e90ff885a9de3c24850df06f62bd6..6aa09cfa3944585c2f2c485a65548e6079307a3e 100644 --- a/src/graph-path-validation.cc +++ b/src/graph-path-validation.cc @@ -119,8 +119,8 @@ namespace hpp { while (!possibleEdges.empty ()) { constraints = constraintGraph_->pathConstraint (possibleEdges.back()); constraints->offsetFromConfig(newPath (newTmin)); - assert (constraints->isSatisfied (newPath (newTmin))); - if (constraints->isSatisfied (newPath (newTmax))) { + if (constraints->isSatisfied (newPath (newTmin)) + && constraints->isSatisfied (newPath (newTmax))) { pathNoCollision->constraints (constraints); impl_validate (pathNoCollision, reverse, validPart); return false; diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc index 64ea88a79aa9891ee5ec147a1b0b0d8c290917c1..f085af9c5949475fd17f00bfdffcaf9dba812703 100644 --- a/src/graph-steering-method.cc +++ b/src/graph-steering-method.cc @@ -31,14 +31,18 @@ namespace hpp { PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const { value_type length = (*distance_) (q1,q2); - std::vector< graph::Edges_t > possibleEdges = - graph_->getEdge (graph_->getNode (q1), graph_->getNode (q2)); + std::vector< graph::Edges_t > possibleEdges; + try { + possibleEdges = graph_->getEdge (graph_->getNode (q1), graph_->getNode (q2)); + } catch (const std::logic_error& e) { + hppDout (error, e.what ()); + return PathPtr_t (); + } ConstraintSetPtr_t constraints; while (!possibleEdges.empty()) { constraints = graph_->pathConstraint (possibleEdges.back()); constraints->offsetFromConfig(q1); - assert (constraints->isSatisfied (q1)); - if (constraints->isSatisfied (q2)) { + if (constraints->isSatisfied (q1) && constraints->isSatisfied (q2)) { PathPtr_t path = core::StraightPath::create (robot_.lock(), q1, q2, length); path->constraints (constraints); return path;