00001 #include <math.h>
00002 #include <boost/bind.hpp>
00003
00004 #include "RRT.h"
00005 #include "PRM.h"
00006
00007 #include "TGT.h"
00008 #include "OmniWheel.h"
00009
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
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
00096 registerAlgorithm("RRT", AlgorithmCreate<RRT>, AlgorithmDelete<RRT>);
00097 registerAlgorithm("PRM", AlgorithmCreate<PRM>, AlgorithmDelete<PRM>);
00098
00099
00100 registerMobility("TurnGoTurn", MobilityCreate<TGT>, MobilityDelete<TGT>);
00101 registerMobility("OmniWheel", MobilityCreate<OmniWheel>, MobilityDelete<OmniWheel>);
00102
00103
00104
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
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
00204
00205 CORBA::Object_var poaObj = orb_ -> resolve_initial_references("RootPOA");
00206 PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
00207
00208
00209
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
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
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
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);
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;
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 }