/**
* @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;
}
