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;
    }
}
示例#2
0
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;
  }
}
示例#3
0
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;
}
示例#4
0
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;
}