From 006fbaca1d503f5499ce96db2b958fa3c55a990f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 3 Sep 2015 11:41:40 +0200
Subject: [PATCH] Modification of the manipulation RRT
* For each random configuration, the extension step is done for every
* connected component and every node of the graph of constraints.
---
src/manipulation-planner.cc | 44 +++++++++++++++++++++----------------
1 file changed, 25 insertions(+), 19 deletions(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index a4d0b00..06819ed 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -66,6 +66,9 @@ namespace hpp {
{
DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem ().robot ());
HPP_ASSERT(robot);
+ const graph::Nodes_t& graphNodes = problem_.constraintGraph ()
+ ->nodeSelector ()->getNodes ();
+ graph::Nodes_t::const_iterator itNode;
core::Nodes_t newNodes;
core::PathPtr_t path;
@@ -78,25 +81,28 @@ namespace hpp {
itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
// Find the nearest neighbor.
core::value_type distance;
- core::NodePtr_t near = roadmap ()->nearestNode (q_rand, *itcc, distance);
-
- bool pathIsValid = extend (near, q_rand, path);
- // Insert new path to q_near in roadmap
- if (pathIsValid) {
- value_type t_final = path->timeRange ().second;
- if (t_final != path->timeRange ().first) {
- ConfigurationPtr_t q_new (new Configuration_t
- ((*path) (t_final)));
- if (!belongs (q_new, newNodes)) {
- newNodes.push_back (roadmap ()->addNodeAndEdges
- (near, q_new, path));
- } else {
- core::NodePtr_t newNode = roadmap ()->addNode (q_new);
- roadmap ()->addEdge (near, newNode, path);
- core::interval_t timeRange = path->timeRange ();
- roadmap ()->addEdge (newNode, near, path->extract
- (core::interval_t (timeRange.second ,
- timeRange.first)));
+ for (itNode = graphNodes.begin (); itNode != graphNodes.end (); ++itNode) {
+ core::NodePtr_t near = roadmap_->nearestNode (q_rand, *itcc, *itNode, distance);
+ if (!near) continue;
+
+ bool pathIsValid = extend (near, q_rand, path);
+ // Insert new path to q_near in roadmap
+ if (pathIsValid) {
+ value_type t_final = path->timeRange ().second;
+ if (t_final != path->timeRange ().first) {
+ ConfigurationPtr_t q_new (new Configuration_t
+ ((*path) (t_final)));
+ if (!belongs (q_new, newNodes)) {
+ newNodes.push_back (roadmap ()->addNodeAndEdges
+ (near, q_new, path));
+ } else {
+ core::NodePtr_t newNode = roadmap ()->addNode (q_new);
+ roadmap ()->addEdge (near, newNode, path);
+ core::interval_t timeRange = path->timeRange ();
+ roadmap ()->addEdge (newNode, near, path->extract
+ (core::interval_t (timeRange.second ,
+ timeRange.first)));
+ }
}
}
}
--
GitLab