/** * @function planPath * @brief Main function */ bool PathPlanner::planPath( int _robotId, const Eigen::VectorXi &_links, const Eigen::VectorXd &_start, const Eigen::VectorXd &_goal, bool _bidirectional, bool _connect, bool _greedy, bool _smooth, unsigned int _maxNodes ) { //world->mRobots[_robotId]->setQuickDofs( _start ); // Other quick way world->getRobot(_robotId)->setDofs( _start, _links ); if( world->checkCollision() ) return false; world->getRobot(_robotId)->setDofs( _goal, _links ); if( world->checkCollision() ) return false; bool result; if( _bidirectional ) { result = planBidirectionalRrt( _robotId, _links, _start, _goal, _connect, _greedy, _maxNodes ); } else { result = planSingleTreeRrt( _robotId, _links, _start, _goal, _connect, _greedy, _maxNodes ); } if( result && _smooth ) { smoothPath( _robotId, _links, path ); } return result; }
bool PathPlanner<R>::planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs, const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal, std::list<Eigen::VectorXd> &path) { Eigen::VectorXd savedConfiguration = robot->getPositions(dofs); // ==================================================================== // Check for collisions in the start and goal configurations // Sift through the possible start configurations and eliminate those that are in collision std::vector<Eigen::VectorXd> feasibleStart; for(unsigned int i = 0; i < start.size(); i++) { robot->setPositions(dofs, start[i]); if(!world->checkCollision()) feasibleStart.push_back(start[i]); } // Return false if there are no feasible start configurations if(feasibleStart.empty()) { printf("WARNING: PathPlanner: Feasible start points are empty!\n"); return false; } // Sift through the possible goal configurations and eliminate those that are in collision std::vector<Eigen::VectorXd> feasibleGoal; for(unsigned int i = 0; i < goal.size(); i++) { robot->setPositions(dofs, goal[i]); if(!world->checkCollision()) feasibleGoal.push_back(goal[i]); } // Return false if there are no feasible goal configurations if(feasibleGoal.empty()) { printf("WARNING: PathPlanner: Feasible goal points are empty!\n"); return false; } // ==================================================================== // Make the correct RRT algorithm for the given method // Direct the search towards single or bidirectional bool result = false; if(bidirectional) result = planBidirectionalRrt(robot, dofs, feasibleStart, feasibleGoal, path); else { if(feasibleGoal.size() > 1) fprintf(stderr, "WARNING: planPath is using ONLY the first goal!\n"); result = planSingleTreeRrt(robot, dofs, feasibleStart, feasibleGoal.front(), path); } // Restore previous robot configuration robot->setPositions(dofs, savedConfiguration); return result; }