diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index de1aa61918dc24956d02158c6a615a65105b512d..2842f5407fbe0d1a860b4cb568ddb32be1a65217 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -55,7 +55,7 @@ namespace hpp {
 
         /// Get the nearest neighbor in a given graph::Node and in a given
         /// ConnectedComponent.
-        RoadmapNodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
+        RoadmapNodePtr_t nearestNodeInState (const ConfigurationPtr_t& configuration,
             const ConnectedComponentPtr_t& connectedComponent,
             const graph::StatePtr_t& state,
             value_type& minDistance) const;
@@ -105,7 +105,7 @@ namespace hpp {
 	  weak_ = shPtr;
 	}
 
-        virtual void addEdge (const core::EdgePtr_t& edge);
+        virtual void impl_addEdge (const core::EdgePtr_t& edge);
 
       private:
         typedef graph::Histograms_t Histograms_t;
diff --git a/src/connected-component.cc b/src/connected-component.cc
index 289dc7a900dc87cb44c09ff75d51241a21f78591..ce7e0fd863658890a2d62fc48c81d09a8843ec5c 100644
--- a/src/connected-component.cc
+++ b/src/connected-component.cc
@@ -83,16 +83,9 @@ namespace hpp {
       core::ConnectedComponent::addNode(node);
       // Find right graph state in map and add roadmap node to corresponding vector
       const RoadmapNodePtr_t& n = static_cast <const RoadmapNodePtr_t> (node);
-      GraphStates_t::iterator mapIt = graphStateMap_.find(roadmap_->getState(n));
-      if (mapIt != graphStateMap_.end()) {
-        mapIt->second.push_back(n);
-      // if graph state not found, add new map element with one roadmap node
-      } else {
-	RoadmapNodes_t newRoadmapNodeVector;
-	newRoadmapNodeVector.push_back(n);
-	graphStateMap_.insert(std::pair<graph::StatePtr_t, RoadmapNodes_t>
-	  (roadmap_->getState(n), newRoadmapNodeVector));
-      }
+      RoadmapPtr_t roadmap = roadmap_.lock();
+      if (!roadmap) throw std::logic_error("The roadmap of this ConnectedComponent as been deleted.");
+      graphStateMap_[roadmap->getState(n)].push_back(n);
       assert (check ());
     }
 
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 3e13a38c066eb77f8d40586ee98731d3a1312f6e..383cf7da9d00a7eb50a5ecc739d0d804f5e68b53 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -184,7 +184,7 @@ namespace hpp {
         core::value_type distance;
         for (itState = graphStates.begin (); itState != graphStates.end (); ++itState) {
           HPP_START_TIMECOUNTER(nearestNeighbor);
-          RoadmapNodePtr_t near = roadmap_->nearestNode (q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent,*itcc), *itState, distance);
+          RoadmapNodePtr_t near = roadmap_->nearestNodeInState (q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent,*itcc), *itState, distance);
           HPP_STOP_TIMECOUNTER(nearestNeighbor);
           HPP_DISPLAY_LAST_TIMECOUNTER(nearestNeighbor);
           if (!near) continue;
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 175fa747e99f1e59baed17e58537cb87cb51b1bb..029dac53beccf64c2b9d50972c3589cb451cc568 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -91,7 +91,7 @@ namespace hpp {
       // graph->histograms() and this class will not know it.
     }
 
-    RoadmapNodePtr_t Roadmap::nearestNode (const ConfigurationPtr_t& configuration,
+    RoadmapNodePtr_t Roadmap::nearestNodeInState (const ConfigurationPtr_t& configuration,
         const ConnectedComponentPtr_t& connectedComponent,
         const graph::StatePtr_t& state,
         value_type& minDistance) const
@@ -159,9 +159,9 @@ namespace hpp {
       }
     }
 
-    void Roadmap::addEdge (const core::EdgePtr_t& edge)
+    void Roadmap::impl_addEdge (const core::EdgePtr_t& edge)
     {
-      Parent::addEdge(edge);
+      Parent::impl_addEdge(edge);
       const RoadmapNodePtr_t& f = static_cast <const RoadmapNodePtr_t> (edge->from());
       const RoadmapNodePtr_t& t = static_cast <const RoadmapNodePtr_t> (edge->to());
       if (f->graphState () == t->graphState ()) {