Tree(std::shared_ptr<StateSpace<T>> stateSpace, bool reverse = false) { _stateSpace = stateSpace; _reverse = reverse; // default values setStepSize(0.1); setMaxIterations(1000); setGoalBias(0); setWaypointBias(0); setGoalMaxDist(0.1); }
NodeTree* rrtgrow(const std::vector<float> &start,const std::vector<float> &goal,float goalbias,std::vector<float> upper,std::vector<float> lower,OpenRAVE::EnvironmentBasePtr env, OpenRAVE::RobotBasePtr robot) { RRTNode* startNode = new RRTNode(start); RRTNode* goalNode = new RRTNode(goal); setgoalConfig(goal); setGoalBias(goalbias); NodeTree* initPath = new NodeTree(startNode); NodeTree* path= new NodeTree(); NodeTree* finalPath=new NodeTree(); RRTNode* nearestNode = NULL; // initPath->setgoalflag(false); goalflag = false; std::vector<float>::const_iterator it; std::cout<<std::endl<<"Given:"<<std::endl<<"Goal:"<<goal[0]<<","<<goal[1]<<","<<goal[2]<<","<<goal[3]<<","<<goal[4]<<","<<goal[5]<<","<<goal[6]<<std::endl; std::cout<<std::endl<<"Start:"<<start[0]<<","<<start[1]<<","<<start[2]<<","<<start[3]<<","<<start[4]<<","<<start[5]<<","<<start[6]<<std::endl; for (int i=0; i<10000;++i){ std::cout<<std::endl<<"RRT-iteration"<<i<<std::endl; RRTNode* sampledNode = initPath->getRamdomSample(upper,lower,goalNode); if(!checkifCollision(sampledNode->getConfig(),env,robot)){ nearestNode = initPath->nearestNeighbour(sampledNode); std::vector<float> nn(nearestNode->getConfig().begin(),nearestNode->getConfig().end()); std::cout<<"Nearest found:["<<nn[0]<<","<<nn[1]<<","<<nn[2]<<","<<nn[3]<<","<<nn[4]<<","<<nn[5]<<","<<nn[6]<<"]"<<std::endl; // std::cout<<"Nearest Node found..."<<std::endl; path = initPath->connectNodes(sampledNode, nearestNode, goalNode,initPath->stepSize(),env,robot); } else continue; if(path->sizeNodes()==1){ std::cout<<"restart subpath.."<<std::endl; continue; } else if(getNearestDistance(path->lastNode()->getConfig(),goal)==0){ // std::cout<<"found found..."<<std::endl; initPath->addNodeTree(path); std::cout<<"...Final..."<<std::endl; for(it=path->lastNode()->getConfig().begin(); it!=path->lastNode()->getConfig().end(); ++it){ std::cout<<"final Node:"<<(*it)<<std::endl; } break; } else{ initPath->addNodeTree(path); std::cout<<"Intermediate Tree..."<<std::endl; // finalPath = initPath->getPath(initPath->sizeNodes()-1); } } std::cout<<" Path found..."<<std::endl; finalPath = initPath->getPath(initPath->sizeNodes()-1); std::cout<<" initPath Size..."<<initPath->sizeNodes()<<std::endl; std::cout<<" FinalPath Size..."<<finalPath->sizeNodes()<<std::endl; return finalPath; }