std::vector<Configuration> RandomShortcutOptimizer::optimize(const std::vector<Configuration> &path)
{
if (path.size() < 3) return path;
Mobility *mobility = planner_->getMobility();
int nSegment = path.size()-1;
int index1 = ((float)random())/RAND_MAX*nSegment;
int index2;
do {
index2 = ((float)random())/RAND_MAX*nSegment;
}while(index1 == index2);
int tmp;
if (index2 < index1) std::swap(index1, index2);
double ratio1 = ((double)random())/RAND_MAX;
double ratio2 = ((double)random())/RAND_MAX;
Configuration cfg1 = mobility->interpolate(path[index1], path[index1+1],
ratio1);
Configuration cfg2 = mobility->interpolate(path[index2], path[index2+1],
ratio2);
if (mobility->isReachable(cfg1, cfg2)){
std::vector<Configuration> optimized;
for (int i=0; i<=index1; i++) optimized.push_back(path[i]);
optimized.push_back(cfg1);
optimized.push_back(cfg2);
for (int i=index2+1; i<path.size(); i++) optimized.push_back(path[i]);
return optimized;
}else{
return path;
}
}
bool Algorithm::tryDirectConnection()
{
Mobility *mobility = planner_->getMobility();
if (mobility->isReachable(start_, goal_)){
path_.push_back(start_);
path_.push_back(goal_);
return true;
}else{
return false;
}
}
bool Algorithm::preparePlanning()
{
isRunning_ = true;
Mobility::interpolationDistance(atof(properties_["interpolation-distance"].c_str()));
#if 1
std::map<std::string, std::string>::iterator it;
it = properties_.begin();
std::cout << "properties:" << std::endl;
while (it != properties_.end()) {
std::cout << " " << it->first << " = " << it->second << std::endl;
it++;
}
std::cout << std::endl;
#endif
path_.clear();
std::cout << "start:" << start_ << std::endl;
std::cout << "goal:" << goal_ << std::endl;
// validity checks of start&goal configurations
if (!start_.isValid()){
std::cerr << "start configuration is invalid" << std::endl;
return false;
}
if (planner_->checkCollision(start_)){
std::cerr << "start configuration is not collision-free" << std::endl;
return false;
}
if (!goal_.isValid()){
std::cerr << "goal configuration is invalid" << std::endl;
return false;
}
if (planner_->checkCollision(goal_)){
std::cerr << "goal configuration is not collision-free" << std::endl;
return false;
}
#if 0
Mobility *mobility = planner_->getMobility();
if (!mobility->isReachable(start_, goal_, false)){
std::cerr << "goal is not reachable even if collision is not checked"
<< std::endl;
return false;
}
#endif
return true;
}
int RRT::extend(RoadmapPtr tree, Configuration& qRand, bool reverse) {
if (debug) std::cout << "RRT::extend("<< qRand << ", " << reverse << ")"
<< std::endl;
RoadmapNodePtr minNode;
double min;
tree->findNearestNode(qRand, minNode, min);
if (debug) std::cout << "nearest : pos = (" << minNode->position()
<< "), d = " << min << std::endl;
if (minNode != NULL) {
Mobility* mobility = planner_->getMobility();
if (min > eps_){
Configuration qRandOrg = qRand;
qRand = mobility->interpolate(minNode->position(), qRand, eps_/min);
if (debug) std::cout << "qRand = (" << qRand << ")" << std::endl;
if (mobility->distance(minNode->position(), qRand) > min){
std::cout << "distance didn't decrease" << std::endl;
std::cout << "qRandOrg : (" << qRandOrg << "), d = " << min
<< std::endl;
std::cout << "qRand : (" << qRand << "), d = "
<< mobility->distance(minNode->position(), qRand)
<< std::endl;
getchar();
}
}
if (reverse){
if (mobility->isReachable(qRand, minNode->position())){
RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
tree->addNode(newNode);
tree->addEdge(newNode, minNode);
if (min <= eps_) {
if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
return Reached;
}
else {
if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
return Advanced;
}
} else {
if (debug) std::cout << "trapped" << std::endl;
return Trapped;
}
}else{
if (mobility->isReachable(minNode->position(), qRand)){
RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
tree->addNode(newNode);
tree->addEdge(minNode, newNode);
if (min <= eps_) {
if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
return Reached;
}
else {
if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
return Advanced;
}
} else {
if (debug) std::cout << "trapped" << std::endl;
return Trapped;
}
}
}
return Trapped;
}