diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index 81243621d274ffd9220e6c0348ebb6a6e9f7f320..aa28d4ac673c77f2c2f2c1326c09a1893a080d39 100644 --- a/src/manipulation-planner.cc +++ b/src/manipulation-planner.cc @@ -165,12 +165,7 @@ namespace hpp { bool success; ConfigurationPtr_t q_new (new Configuration_t ((*path) (t_final, success))); - if (!belongs (q_new, newNodes)) { - newNodes.push_back (roadmap ()->addNodeAndEdges - (near, q_new, path)); - } else { - delayedEdges.push_back (DelayedEdge_t (near, q_new, path)); - } + delayedEdges.push_back (DelayedEdge_t (near, q_new, path)); } } } @@ -185,10 +180,8 @@ namespace hpp { const core::PathPtr_t& validPath = itEdge-> get <2> (); core::NodePtr_t newNode = roadmap ()->addNode (q_new); roadmap ()->addEdge (near, newNode, validPath); - core::interval_t timeRange = validPath->timeRange (); - roadmap ()->addEdge (newNode, near, validPath->extract - (core::interval_t (timeRange.second , - timeRange.first))); + roadmap ()->addEdge (newNode, near, validPath->reverse()); + newNodes.push_back (newNode); } HPP_STOP_TIMECOUNTER(delayedEdges);