PathPlanner.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <boost/bind.hpp>
00003 // planning algorithms
00004 #include "RRT.h"
00005 #include "PRM.h"
00006 // mobilities
00007 #include "TGT.h"
00008 #include "OmniWheel.h"
00009 // optimizers
00010 #include "ShortcutOptimizer.h"
00011 #include "RandomShortcutOptimizer.h"
00012 //
00013 #include "ConfigurationSpace.h"
00014 #include "PathPlanner.h"
00015 
00016 #include <hrpCorba/OpenHRPCommon.hh>
00017 #include <hrpModel/Body.h>
00018 #include <hrpModel/Link.h>
00019 #include <hrpModel/ModelLoaderUtil.h>
00020 
00021 using namespace PathEngine;
00022 using namespace hrp;
00023 
00024 //static const bool USE_INTERNAL_COLLISION_DETECTOR = false;
00025 static const bool USE_INTERNAL_COLLISION_DETECTOR = true;
00026 
00027 // ----------------------------------------------
00028 // ネームサーバからオブジェクトを取得
00029 // ----------------------------------------------
00030 template<typename X, typename X_ptr>
00031 X_ptr PathPlanner::checkCorbaServer(const std::string &n, CosNaming::NamingContext_var &cxt)
00032 {
00033     CosNaming::Name ncName;
00034     ncName.length(1);
00035     ncName[0].id = CORBA::string_dup(n.c_str());
00036     ncName[0].kind = CORBA::string_dup("");
00037     X_ptr srv = NULL;
00038     try {
00039         srv = X::_narrow(cxt->resolve(ncName));
00040     } catch(const CosNaming::NamingContext::NotFound &exc) {
00041         std::cerr << n << " not found: ";
00042         switch(exc.why) {
00043         case CosNaming::NamingContext::missing_node:
00044             std::cerr << "Missing Node" << std::endl;
00045         case CosNaming::NamingContext::not_context:
00046             std::cerr << "Not Context" << std::endl;
00047             break;
00048         case CosNaming::NamingContext::not_object:
00049             std::cerr << "Not Object" << std::endl;
00050             break;
00051         }
00052         return (X_ptr)NULL;
00053     } catch(CosNaming::NamingContext::CannotProceed &exc) {
00054         std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00055     } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00056         std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00057     }
00058     return srv;
00059 }
00060 
00061 bool setConfigurationToBaseXYTheta(PathPlanner *planner, const Configuration& cfg)
00062 {
00063     Link *baseLink = planner->robot()->rootLink();
00064     // 目標値設定  
00065     baseLink->p(0) = cfg.value(0);
00066     baseLink->p(1) = cfg.value(1);
00067     Matrix33 R;
00068 
00069     double c = cos(cfg.value(2)), s = sin(cfg.value(2));
00070     R(0,0) = c; R(0,1) = -s; R(0,2) = 0;
00071     R(1,0) = s; R(1,1) =  c; R(1,2) = 0;
00072     R(2,0) = 0; R(2,1) =  0; R(2,2) = 1;
00073     baseLink->setSegmentAttitude(R);
00074     planner->robot()->calcForwardKinematics();
00075 
00076     return true;
00077 }
00078 
00079 // ----------------------------------------------
00080 // コンストラクタ
00081 // ----------------------------------------------
00082 PathPlanner::PathPlanner(unsigned int dim, 
00083                          boost::shared_ptr<hrp::World<hrp::ConstraintForceSolver> > world,
00084                          bool isDebugMode) 
00085     : m_applyConfigFunc(setConfigurationToBaseXYTheta), algorithm_(NULL), mobility_(NULL), cspace_(dim), debug_(isDebugMode), world_(world), customCollisionDetector_(NULL)
00086 {
00087     if (isDebugMode) {
00088         std::cerr << "PathPlanner::PathPlanner() : debug mode" << std::endl;
00089     }
00090 
00091     if (!world_){
00092         world_ = WorldPtr(new hrp::World<hrp::ConstraintForceSolver>);
00093     }
00094 
00095     // planning algorithms
00096     registerAlgorithm("RRT", AlgorithmCreate<RRT>, AlgorithmDelete<RRT>);
00097     registerAlgorithm("PRM", AlgorithmCreate<PRM>, AlgorithmDelete<PRM>);
00098 
00099     // mobilities
00100     registerMobility("TurnGoTurn", MobilityCreate<TGT>, MobilityDelete<TGT>);
00101     registerMobility("OmniWheel", MobilityCreate<OmniWheel>, MobilityDelete<OmniWheel>);
00102     //registerMobility("ArcAndOdv", MobilityCreate<ArcAndOdv>, MobilityDelete<ArcAndOdv>);
00103 
00104     // optimizers
00105     registerOptimizer("Shortcut", 
00106                       OptimizerCreate<ShortcutOptimizer>,
00107                       OptimizerDelete<ShortcutOptimizer>);
00108     registerOptimizer("RandomShortcut", 
00109                       OptimizerCreate<RandomShortcutOptimizer>,
00110                       OptimizerDelete<RandomShortcutOptimizer>);
00111                     
00112     allCharacterPositions_ = new OpenHRP::CharacterPositionSequence;
00113 
00114     //bboxMode_ = true;
00115     bboxMode_ = false;
00116 
00117     dt_ = 0.01;
00118 }
00119 
00120 // ----------------------------------------------
00121 // デストラクタ
00122 // ----------------------------------------------
00123 PathPlanner::~PathPlanner() {
00124     if (debug_) {
00125         std::cerr << "PathPlanner::~PathPlanner()" << std::endl;
00126     }
00127     if (algorithm_ != NULL) {
00128         algorithmFactory_[algorithmName_].second(algorithm_);
00129     }
00130 
00131     if (mobility_ != NULL) {
00132         mobilityFactory_[mobilityName_].second(mobility_);
00133     }
00134 }
00135 
00136 void PathPlanner::getOptimizerNames(std::vector<std::string> &optimizers) {
00137     optimizers.clear();
00138     OptimizerFactory::iterator it = optimizerFactory_.begin();
00139     while (it != optimizerFactory_.end()) {
00140         optimizers.push_back((*it).first);
00141         it++;
00142     }
00143 }
00144 
00145 void PathPlanner::getMobilityNames(std::vector<std::string> &mobilities) {
00146     mobilities.clear();
00147     MobilityFactory::iterator it = mobilityFactory_.begin();
00148     while (it != mobilityFactory_.end()) {
00149         mobilities.push_back((*it).first);
00150         it++;
00151     }
00152 }
00153 
00154 // ----------------------------------------------
00155 // アルゴリズム一覧取得
00156 // ----------------------------------------------
00157 void PathPlanner::getAlgorithmNames(std::vector<std::string> &algorithms) {
00158     algorithms.clear();
00159     AlgorithmFactory::iterator it = algorithmFactory_.begin();
00160     while (it != algorithmFactory_.end()) {
00161         algorithms.push_back((*it).first);
00162         it++;
00163     }
00164 }
00165 
00166 // ----------------------------------------------
00167 // プロパティ一覧取得
00168 // ----------------------------------------------
00169 bool PathPlanner::getProperties(const std::string &algorithm,
00170                                 std::vector<std::string> &names,
00171                                 std::vector<std::string> &values)
00172 {
00173     if (debug_) {
00174         std::cerr << "PathPlanner::getPropertyNames(" << algorithm << ")" << std::endl;
00175     }
00176     if (algorithmFactory_.count(algorithm) > 0) {
00177         Algorithm* algorithmInst = algorithmFactory_[algorithm].first(this);
00178         algorithmInst->getProperties(names, values);
00179         algorithmFactory_[algorithm].second(algorithmInst);
00180     }
00181     else {
00182         std::cerr << "algorithm not found" << std::endl;
00183         return false;
00184     }
00185     return true;
00186 }
00187 
00188 // ----------------------------------------------
00189 // 初期化
00190 // ----------------------------------------------
00191 void PathPlanner::initPlanner(const std::string &nameServer) {
00192 
00193     if (debug_) {
00194         std::cerr << "PathPlanner::initPlanner(" << nameServer << ")" << std::endl;
00195     }
00196 
00197     try {
00198         int ac = 0;
00199         char* av[1];
00200         av[0] = NULL;
00201         orb_ = CORBA::ORB_init(ac, av);
00202         //
00203         // Resolve Root POA
00204         //
00205         CORBA::Object_var poaObj = orb_ -> resolve_initial_references("RootPOA");
00206         PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
00207 
00208         //
00209         // Get a reference to the POA manager
00210         //
00211         PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
00212 
00213         std::ostringstream stream;
00214         stream << "corbaloc:iiop:" << nameServer << "/NameService";
00215 
00216         CosNaming::NamingContext_var cxT;
00217         CORBA::Object_var       nS = orb_->string_to_object(stream.str().c_str());
00218         cxT = CosNaming::NamingContext::_narrow(nS);
00219 
00220         if (is_nil(cxT)) {
00221             std::cerr << "name serivce not found" << std::endl;
00222         }else{
00223             std::cerr << "NameService OK." << std::endl;
00224         }
00225 
00226         if (!USE_INTERNAL_COLLISION_DETECTOR){
00227             std::cerr << "CollisonDetectorFactory ";
00228             OpenHRP::CollisionDetectorFactory_var cdFactory =
00229                 checkCorbaServer <OpenHRP::CollisionDetectorFactory,
00230                 OpenHRP::CollisionDetectorFactory_var> ("CollisionDetectorFactory", cxT);
00231             if (CORBA::is_nil(cdFactory)) {
00232                 std::cerr << "not found" << std::endl;
00233             }else{
00234                 std::cerr << "OK." << std::endl;
00235             }
00236             try{
00237                 collisionDetector_ = cdFactory->create();
00238             }catch(...){
00239                 std::cerr << "failed to create CollisionDetector" << std::endl;
00240             }
00241         }
00242 
00243         std::cerr << "ModelLoader ";
00244         modelLoader_ =
00245             checkCorbaServer <OpenHRP::ModelLoader,
00246             OpenHRP::ModelLoader_var> ("ModelLoader", cxT);
00247 
00248         if (CORBA::is_nil(modelLoader_)) {
00249             std::cerr << "not found" << std::endl;
00250         }else{
00251             std::cerr << "OK." << std::endl;
00252         }
00253 
00254         if (debug_) {
00255             onlineViewer_ =
00256                 checkCorbaServer <OpenHRP::OnlineViewer,
00257                 OpenHRP::OnlineViewer_var> ("OnlineViewer", cxT);
00258 
00259             if (CORBA::is_nil(onlineViewer_)) {
00260                 std::cerr << "OnlineViewer not found" << std::endl;
00261             }
00262         }
00263     
00264     } catch (CORBA::SystemException& ex) {
00265         std::cerr << ex._rep_id() << std::endl;
00266         return;
00267     }
00268 
00269     world_->clearBodies();
00270     checkPairs_.clear();
00271 
00272 }
00273 
00274 void PathPlanner::setAlgorithmName(const std::string &algorithmName)
00275 {
00276     // アルゴリズム名とインスタンスをセット
00277     algorithmName_ = algorithmName;
00278     if (algorithmFactory_.count(algorithmName) > 0) {
00279         if (algorithm_ != NULL) {
00280             algorithmFactory_[algorithmName_].second(algorithm_);
00281         }
00282         algorithm_ = algorithmFactory_[algorithmName_].first(this);
00283     } else {
00284         std::cerr << "algorithm(" << algorithmName << ") not found" 
00285                   << std::endl;
00286     }
00287 }
00288 
00289 // ----------------------------------------------
00290 // キャラクタをURLから登録
00291 // ----------------------------------------------
00292 BodyPtr PathPlanner::registerCharacterByURL(const char* name, const char* url){
00293     if (debug_) {
00294         std::cerr << "PathPlanner::registerCharacterByURL(" << name << ", " << url << ")" << std::endl;
00295     }
00296 
00297     if (CORBA::is_nil(modelLoader_)) {
00298         std::cerr << "nil reference to servers" << std::endl;
00299         return BodyPtr();
00300     }
00301 
00302     OpenHRP::BodyInfo_ptr cInfo = modelLoader_->getBodyInfo(url);
00303     BodyPtr body = registerCharacter(name, cInfo);
00304 
00305     if (debug_) {
00306         if (CORBA::is_nil(onlineViewer_)) {
00307             std::cerr << "nil reference to OnlineViewer" << std::endl;
00308         }
00309         onlineViewer_->load(name, url);
00310     }
00311     return body;
00312 }
00313 
00314 // ----------------------------------------------
00315 // 位置の設定
00316 // ----------------------------------------------
00317 void PathPlanner::setCharacterPosition(const char* character,
00318                                        const OpenHRP::DblSequence& pos)
00319 {
00320     if (debug_){
00321         std::cerr << "PathPlanner::setCharacterPosition(" << character << ", "
00322                   << pos[0] << ", " << pos[1] << ", " << pos[2] << ")" 
00323                   << std::endl;
00324     }
00325     BodyPtr body = world_->body(character);
00326     if (!body) std::cerr << "PathPlanner::setCharacterPosition() : character("
00327                          << character << ") not found" << std::endl;
00328     Link* l = body->rootLink();
00329     l->p(0) = pos[0];
00330     l->p(1) = pos[1];
00331     l->p(2) = pos[2];
00332     Matrix33 R;
00333     getMatrix33FromRowMajorArray(R, pos.get_buffer(), 3);
00334     l->setSegmentAttitude(R);
00335 }
00336 
00337 void computeBoundingBox(BodyPtr body, double min[3], double max[3])
00338 {
00339     bool firsttime = true;
00340     Vector3 v, p;
00341     Link *l, *root=body->rootLink();
00342     Matrix33 Rt = root->R.transpose();
00343     float x, y, z;
00344     for (unsigned int i=0; i<body->numLinks(); i++){
00345         l = body->link(i);
00346         for (int j=0; j<l->coldetModel->getNumVertices(); j++){
00347             l->coldetModel->getVertex(j, x, y, z);
00348             v[0] = x; v[1] = y; v[2] = z;
00349             p = Rt*(l->R*v+l->p-root->p);
00350             if (firsttime){
00351                 for (int k=0; k<3; k++) min[k] = max[k] = p[k];
00352                 firsttime = false;
00353             }else{
00354                 for (int k=0; k<3; k++){
00355                     if (p[k] < min[k]) min[k] = p[k];
00356                     if (p[k] > max[k]) max[k] = p[k];
00357                 }
00358             }
00359         }
00360     }
00361     std::cerr << "bounding box of " << body->name() << ": ("
00362               << min[0] << ", " << min[1] << ", " << min[2] << ") - ("
00363               << max[0] << ", " << max[1] << ", " << max[2] << ")" 
00364               << std::endl;
00365 }
00366 
00367 BodyPtr createBoundingBoxBody(BodyPtr body)
00368 {
00369     double min[3], max[3];
00370     computeBoundingBox(body, min, max);
00371 
00372     ColdetModelPtr coldetModel(new ColdetModel());
00373     coldetModel->setNumVertices(8);
00374     coldetModel->setNumTriangles(12);
00375     coldetModel->setVertex(0, max[0], max[1], max[2]);
00376     coldetModel->setVertex(1, min[0], max[1], max[2]);
00377     coldetModel->setVertex(2, min[0], min[1], max[2]);
00378     coldetModel->setVertex(3, max[0], min[1], max[2]);
00379     coldetModel->setVertex(4, max[0], max[1], min[2]);
00380     coldetModel->setVertex(5, min[0], max[1], min[2]);
00381     coldetModel->setVertex(6, min[0], min[1], min[2]);
00382     coldetModel->setVertex(7, max[0], min[1], min[2]);
00383     coldetModel->setTriangle(0, 0, 1, 2);
00384     coldetModel->setTriangle(1, 0, 2, 3);
00385     coldetModel->setTriangle(2, 0, 3, 7);
00386     coldetModel->setTriangle(3, 0, 7, 4);
00387     coldetModel->setTriangle(4, 0, 4, 5);
00388     coldetModel->setTriangle(5, 0, 5, 1);
00389     coldetModel->setTriangle(6, 3, 2, 6);
00390     coldetModel->setTriangle(7, 3, 6, 7);
00391     coldetModel->setTriangle(8, 1, 5, 6);
00392     coldetModel->setTriangle(9, 1, 6, 2);
00393     coldetModel->setTriangle(10, 4, 7, 6);
00394     coldetModel->setTriangle(11, 4, 6, 5);
00395     coldetModel->build();
00396 
00397     Link *root = new Link();
00398     root->R = Matrix33::Identity();
00399     root->Rs = Matrix33::Identity();
00400     root->coldetModel = coldetModel;
00401 
00402     BodyPtr bboxBody = BodyPtr(new Body());
00403     bboxBody->setRootLink(root);
00404 
00405     return bboxBody;
00406 }
00407 
00408 // ----------------------------------------------
00409 // キャラクタをBodyInfoから登録
00410 // ----------------------------------------------
00411 BodyPtr PathPlanner::registerCharacter(const char* name, OpenHRP::BodyInfo_ptr cInfo) {
00412     if (debug_) {
00413         std::cerr << "PathPlanner::registerCharacter(" << name << ", " << cInfo << ")" << std::endl;
00414     }
00415 
00416     BodyPtr body(new Body());
00417 
00418     if(loadBodyFromBodyInfo(body, cInfo, USE_INTERNAL_COLLISION_DETECTOR)){
00419         body->setName(name);
00420         if(debug_){
00421             //std::cerr << "Loaded Model:\n" << *body << std::endl;
00422         }
00423 
00424         if (bboxMode_ && USE_INTERNAL_COLLISION_DETECTOR){
00425             body = createBoundingBoxBody(body);
00426             body->setName(name);
00427         }
00428 
00429         if(!USE_INTERNAL_COLLISION_DETECTOR){
00430             collisionDetector_->registerCharacter(name, cInfo);
00431         }
00432         world_->addBody(body);
00433     }
00434     return body;
00435 }
00436 
00437 BodyPtr PathPlanner::registerCharacter(const char *name, BodyPtr i_body)
00438 {
00439     i_body->setName(name);
00440     world_->addBody(i_body);
00441     return i_body;
00442 }
00443 
00444 
00445 
00446 
00447 // ----------------------------------------------
00448 // ロボット名を設定
00449 // ----------------------------------------------
00450 void PathPlanner::setRobotName(const std::string &name) {
00451     if (debug_) {
00452         std::cerr << "PathPlanner::setRobotName(" << name << ")" << std::endl;
00453     }
00454 
00455     model_ = world_->body(name);
00456     if (!model_) {
00457         std::cerr << "PathPlanner::setRobotName() : robot(" << name << ") not found" << std::endl;
00458         return;
00459     }
00460 }
00461 
00462 // ----------------------------------------------
00463 // ロボット名を設定
00464 // ----------------------------------------------
00465 bool PathPlanner::setMobilityName(const std::string &mobility)
00466 {
00467     if (debug_) {
00468         std::cerr << "PathPlanner::setMobilityName(" << mobility << ")" << std::endl;
00469     }
00470 
00471     mobilityName_ = mobility;
00472     if (mobilityFactory_.count(mobilityName_) > 0) {
00473         if (mobility_ != NULL) {
00474             mobilityFactory_[mobilityName_].second(mobility_);
00475         }
00476         mobility_ = mobilityFactory_[mobilityName_].first(this);
00477     }
00478     else {
00479         std::cerr << "PathPlanner::setMobilityName() : mobility(" << mobility << ") not found" << std::endl;
00480         return false;
00481     }
00482     return true;
00483 }
00484 
00485 
00486 
00487 // ----------------------------------------------
00488 // 衝突チェックペアを登録
00489 // ----------------------------------------------
00490 void PathPlanner::registerIntersectionCheckPair(const char* charName1, 
00491                                                 const char* linkName1, 
00492                                                 const char* charName2,
00493                                                 const char* linkName2,
00494                                                 CORBA::Double tolerance) {
00495     if (debug_){
00496         std::cerr << "PathPlanner::registerIntersectionCheckPair("
00497                   << charName1 << ", " << linkName1 << ", " << charName2 
00498                   << ", " << linkName2
00499                   << ", " << tolerance << ")" << std::endl;
00500     }
00501     int bodyIndex1 = world_->bodyIndex(charName1);
00502     int bodyIndex2 = world_->bodyIndex(charName2);
00503 
00504     if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
00505 
00506         BodyPtr body1 = world_->body(bodyIndex1);
00507         BodyPtr body2 = world_->body(bodyIndex2);
00508 
00509         std::string emptyString = "";
00510         std::vector<Link*> links1;
00511         if(emptyString == linkName1){
00512             const LinkTraverse& traverse = body1->linkTraverse();
00513             links1.resize(traverse.numLinks());
00514             std::copy(traverse.begin(), traverse.end(), links1.begin());
00515         } else {
00516             links1.push_back(body1->link(linkName1));
00517         }
00518 
00519         std::vector<Link*> links2;
00520         if(emptyString == linkName2){
00521             const LinkTraverse& traverse = body2->linkTraverse();
00522             links2.resize(traverse.numLinks());
00523             std::copy(traverse.begin(), traverse.end(), links2.begin());
00524         } else {
00525             links2.push_back(body2->link(linkName2));
00526         }
00527 
00528         for(size_t i=0; i < links1.size(); ++i){
00529             for(size_t j=0; j < links2.size(); ++j){
00530                 Link* link1 = links1[i];
00531                 Link* link2 = links2[j];
00532 
00533                 if(link1 && link2 && link1 != link2){
00534                     if(USE_INTERNAL_COLLISION_DETECTOR){
00535                         checkPairs_.push_back(ColdetModelPair(link1->coldetModel,
00536                                                               link2->coldetModel,
00537                                                               tolerance));
00538                     }else{
00539                         OpenHRP::LinkPair_var linkPair = new OpenHRP::LinkPair();
00540                         linkPair->charName1  = CORBA::string_dup(charName1);
00541                         linkPair->linkName1 = CORBA::string_dup(link1->name.c_str());
00542                         linkPair->charName2  = CORBA::string_dup(charName2);
00543                         linkPair->linkName2 = CORBA::string_dup(link2->name.c_str());
00544                         linkPair->tolerance = tolerance;
00545                         collisionDetector_->addCollisionPair(linkPair);
00546                     }
00547                 }
00548             }
00549         }
00550     }
00551 }
00552 // ----------------------------------------------
00553 // 初期化
00554 // ----------------------------------------------
00555 void PathPlanner::initSimulation () {
00556     world_->setTimeStep(0.002);
00557     world_->setCurrentTime(0.0);
00558     world_->setRungeKuttaMethod();
00559     world_->enableSensors(true);
00560   
00561     int n = world_->numBodies();
00562     for(int i=0; i < n; ++i){
00563         world_->body(i)->initializeConfiguration();
00564     }
00565   
00566     _setupCharacterData();
00567   
00568     Vector3 g;
00569     g[0] = g[1] = 0.0; g[2] = 9.8;
00570     world_->setGravityAcceleration(g);
00571   
00572     world_->initialize();
00573 }
00574 
00575 bool PathPlanner::setConfiguration(const Configuration &pos)
00576 {
00577     return m_applyConfigFunc(this, pos);
00578 }
00579 
00580 bool PathPlanner::checkCollision (const Configuration &pos) {
00581 #if 0
00582     if (debug_) {
00583         std::cerr << "checkCollision(" << pos << ")" << std::endl;
00584     }
00585 #endif
00586     if (!setConfiguration(pos)) return true;
00587 
00588     // 干渉チェック
00589     bool ret = checkCollision();
00590 
00591     if (debug_) {
00592         // 結果を得る
00593         OpenHRP::WorldState_var state;
00594         getWorldState(state);
00595 
00596         static double nowTime = 0;
00597         state->time = nowTime;
00598         nowTime += dt_;
00599 
00600         onlineViewer_->update(state);
00601     }
00602     return ret;
00603 }
00604 
00605 void PathPlanner::getWorldState(OpenHRP::WorldState_out wstate)
00606 {
00607     if(debug_){
00608         std::cerr << "DynamicsSimulator_impl::getWorldState()\n";
00609     }
00610 
00611     _updateCharacterPositions();
00612 
00613     wstate = new OpenHRP::WorldState;
00614 
00615     wstate->time = world_->currentTime();
00616     wstate->characterPositions = allCharacterPositions_;
00617 
00618     if(debug_){
00619         std::cerr << "getWorldState - exit" << std::endl;
00620     }
00621 }
00622 
00623 bool PathPlanner::checkCollision()
00624 {
00625     if (customCollisionDetector_){
00626         timeForwardKinematics_.begin();
00627         customCollisionDetector_->updatePositions();
00628         timeForwardKinematics_.end();
00629         timeCollisionCheck_.begin();
00630         bool ret = customCollisionDetector_->checkCollision();
00631         timeCollisionCheck_.end();
00632         if (ret) collidingPair_ = customCollisionDetector_->collidingPair();
00633         return ret;
00634     }else{
00635         return defaultCheckCollision();
00636     }
00637 }
00638 
00639 bool PathPlanner::defaultCheckCollision()
00640 {
00641     timeForwardKinematics_.begin();
00642 
00643     if (USE_INTERNAL_COLLISION_DETECTOR){
00644         Link *l;
00645             for (unsigned int j=0; j<model_->numLinks(); j++){
00646                 l = model_->link(j);
00647                 l->coldetModel->setPosition(l->attitude(), l->p);
00648             }
00649     }else{
00650         _updateCharacterPositions();
00651     }
00652     timeForwardKinematics_.end();
00653     timeCollisionCheck_.begin();
00654     if(USE_INTERNAL_COLLISION_DETECTOR){
00655         for (unsigned int i=0; i<checkPairs_.size(); i++){
00656             if (checkPairs_[i].tolerance() == 0){
00657                 if (checkPairs_[i].checkCollision()){
00658                     collidingPair_.first  = checkPairs_[i].model(0)->name();
00659                     collidingPair_.second = checkPairs_[i].model(1)->name();
00660                     timeCollisionCheck_.end();
00661                     return true;
00662                 }
00663             } else{
00664                 if (checkPairs_[i].detectIntersection()) {
00665                     collidingPair_.first  = checkPairs_[i].model(0)->name();
00666                     collidingPair_.second = checkPairs_[i].model(1)->name();
00667                     timeCollisionCheck_.end();
00668                     return true;
00669                 }
00670             } 
00671         } 
00672         if (pointCloud_.size() > 0){
00673             for (unsigned int i=0; i<model_->numLinks(); i++){
00674                 Link *l = model_->link(i);
00675                 if (l->coldetModel->checkCollisionWithPointCloud(pointCloud_,
00676                                                                  radius_)){
00677                     timeCollisionCheck_.end();
00678                     return true;
00679                 }
00680             }
00681         }
00682         timeCollisionCheck_.end();
00683         return false;
00684     }else{
00685         OpenHRP::LinkPairSequence_var pairs = new OpenHRP::LinkPairSequence;
00686         collisionDetector_->queryIntersectionForDefinedPairs(false, allCharacterPositions_.in(), pairs);
00687         
00688         timeCollisionCheck_.end();
00689         return pairs->length() > 0;
00690     }
00691 }
00692 
00693 // ----------------------------------------------
00694 // アルゴリズム登録
00695 // ----------------------------------------------
00696 void PathPlanner::registerAlgorithm(const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc) {
00697     algorithmFactory_.insert(AlgorithmFactoryValueType(algorithmName, std::make_pair(newFunc, deleteFunc)));
00698 }
00699 
00700 void PathPlanner::registerMobility(const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc) {
00701     mobilityFactory_.insert(MobilityFactoryValueType(mobilityName, std::make_pair(newFunc, deleteFunc)));
00702 }
00703 void PathPlanner::registerOptimizer(const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc) {
00704     optimizerFactory_.insert(OptimizerFactoryValueType(optimizerName, std::make_pair(newFunc, deleteFunc)));
00705 }
00706 bool PathPlanner::checkCollision(const std::vector<Configuration> &path) {
00707     unsigned int checked = 1;
00708     unsigned int div = 2;
00709 
00710     std::vector<bool> isVisited;
00711     for (unsigned int i=0; i<path.size()-1; i++) {
00712         isVisited.push_back(false);
00713     }
00714     isVisited.push_back(true); // the last Configuration is not checked
00715 
00716  
00717     while (checked < (path.size()-1) || path.size()/div > 0) {
00718         int step = path.size()/div;
00719         for (unsigned int i=step; i<path.size(); i+=step) {
00720             if (!isVisited[i]) {
00721                 checked++;
00722                 if (checkCollision(path[i])) {
00723                     return true;
00724                 }
00725                 isVisited[i] = true;
00726             }
00727         }
00728         div++;
00729     }
00730     if (checked != path.size()-1) {
00731         std::cerr << "checkCollision() : there are unchecked configurations."
00732                   << " path.size() = " << path.size() << ", checked = " 
00733                   << checked << std::endl;
00734     }
00735     return false; // the first Configuration is not checked
00736 }
00737 bool PathPlanner::calcPath()
00738 {
00739     path_.clear();
00740     BodyPtr body;
00741     Link *l;
00742     for (unsigned int i=0; i<world_->numBodies(); i++){
00743         body = world_->body(i);
00744         body->calcForwardKinematics();
00745         for (unsigned int j=0; j<body->numLinks(); j++){
00746             l = body->link(j);
00747             l->coldetModel->setPosition(l->attitude(), l->p);
00748         }
00749     }
00750     std::cerr << "The number of collision check pairs = " << checkPairs_.size() << std::endl;
00751     if (!algorithm_->preparePlanning()){
00752         std::cerr << "preparePlanning() failed" << std::endl;
00753         return false;
00754     }
00755 
00756     if (algorithm_->tryDirectConnection()){
00757         path_ = algorithm_->getPath();
00758         std::cerr << "connected directly" << std::endl;
00759         return true;
00760     }
00761     std::cerr << "failed direct connection" << std::endl;
00762 
00763     if (algorithm_->calcPath()){
00764         path_ = algorithm_->getPath();
00765         return true;
00766     }
00767 
00768     return false;
00769 }
00770 
00771 bool PathPlanner::optimize(const std::string& optimizer)
00772 {
00773     if (optimizerFactory_.count(optimizer) > 0) {
00774         Optimizer *opt = optimizerFactory_[optimizer].first(this);
00775         path_ = opt->optimize(path_);
00776         optimizerFactory_[optimizer].second(opt);
00777         return true;
00778     }else{
00779         return false;
00780     }
00781 }
00782 
00783 std::vector<Configuration>& PathPlanner::getWayPoints()
00784 {
00785     return path_;
00786 }
00787 std::vector<Configuration> PathPlanner::getPath()
00788 {
00789     std::vector<Configuration> finalPath;
00790     if (path_.size() == 0) return finalPath;
00791 
00792     for (unsigned int i=0; i<path_.size()-1; i++) {
00793         std::vector<Configuration> localPath;
00794         mobility_->getPath(path_[i], path_[i+1],localPath);
00795         finalPath.insert(finalPath.end(), localPath.begin(), localPath.end()-1);
00796     }
00797     finalPath.push_back(path_[path_.size()-1]);
00798 
00799     return finalPath;
00800 }  
00801 
00802 void PathPlanner::_setupCharacterData()
00803 {
00804     if(debug_){
00805         std::cerr << "PathPlanner::_setupCharacterData()\n";
00806     }
00807 
00808     int n = world_->numBodies();
00809     allCharacterPositions_->length(n);
00810 
00811     for(int i=0; i < n; ++i){
00812         BodyPtr body = world_->body(i);
00813 
00814         int numLinks = body->numLinks();
00815         OpenHRP::CharacterPosition& characterPosition = allCharacterPositions_[i];
00816         characterPosition.characterName = CORBA::string_dup(body->name().c_str());
00817         OpenHRP::LinkPositionSequence& linkPositions = characterPosition.linkPositions;
00818         linkPositions.length(numLinks);
00819 
00820         if(debug_){
00821             std::cerr << "character[" << i << "], nlinks = " << numLinks << "\n";
00822         }
00823     }
00824 
00825     if(debug_){
00826         std::cerr << "_setupCharacterData() - exit" << std::endl;;
00827     }
00828 }
00829 
00830 
00831 void PathPlanner::_updateCharacterPositions()
00832 {
00833     if(debug_){
00834         std::cerr << "PathPlanner::_updateCharacterPositions()\n";
00835     }
00836 
00837     int n = world_->numBodies();
00838 
00839     {   
00840 #pragma omp parallel for num_threads(3)
00841         for(int i=0; i < n; ++i){
00842             BodyPtr body = world_->body(i);
00843             int numLinks = body->numLinks();
00844                         
00845             OpenHRP::CharacterPosition& characterPosition = allCharacterPositions_[i];
00846                         
00847             if(debug_){
00848                 std::cerr << "character[" << i << "], nlinks = " << numLinks << "\n";
00849             }
00850                         
00851             for(int j=0; j < numLinks; ++j) {
00852                 Link* link = body->link(j);
00853                 OpenHRP::LinkPosition& linkPosition = characterPosition.linkPositions[j];
00854                 setVector3(link->p, linkPosition.p);
00855                 setMatrix33ToRowMajorArray(link->segmentAttitude(), linkPosition.R);
00856             }
00857         }
00858     }
00859 
00860     if(debug_){
00861         std::cerr << "_updateCharacterData() - exit" << std::endl;
00862     }
00863 }
00864 
00865 
00866 double PathPlanner::timeCollisionCheck() const
00867 {
00868     return timeCollisionCheck_.totalTime();
00869 }
00870 
00871 double PathPlanner::timeForwardKinematics() const
00872 {
00873     return timeForwardKinematics_.totalTime();
00874 }
00875 
00876 void PathPlanner::setApplyConfigFunc(applyConfigFunc i_func)
00877 {
00878     m_applyConfigFunc = i_func;
00879 }
00880 
00881 BodyPtr PathPlanner::robot()
00882 {
00883     return model_;
00884 }
00885 
00886 WorldPtr PathPlanner::world()
00887 {
00888     return world_;
00889 }
00890 
00891 void PathPlanner::setPointCloud(const std::vector<Vector3>& i_cloud, 
00892                                 double i_radius)
00893 {
00894     pointCloud_ = i_cloud;
00895     radius_ = i_radius;
00896 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56