コード例 #1
0
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
ファイル: RRT.cpp プロジェクト: rkoyama1623/openhrp3
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;
}