working version of limb rrt

src/rbprmbuilder.impl.cc

@@ -653,8 +653,9 @@ std::cout << romDevice->currentConfiguration() << std::endl;
                 throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
             }
             //create helper
-            interpolation::LimbRRTHelper helper(fullBody_, problemSolver_->problem());
-            core::PathVectorPtr_t path = interpolation::interpolateStates(helper,lastStatesComputed_[s1],lastStatesComputed_[s2]);
+//            /interpolation::LimbRRTHelper helper(fullBody_, problemSolver_->problem());
+            core::PathVectorPtr_t path = interpolation::interpolateStates(fullBody_,problemSolver_->problem(),
+                                                                          lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2);
             problemSolver_->addPath(path);
             problemSolver_->robot()->setDimensionExtraConfigSpace(problemSolver_->robot()->extraConfigSpace().dimension()+1);
         }