Algorithm.cpp
Go to the documentation of this file.
00001 #include "Mobility.h"
00002 #include "PathPlanner.h"
00003 #include "Algorithm.h"
00004 #include "Roadmap.h"
00005 #include "ConfigurationSpace.h"
00006 
00007 using namespace PathEngine;
00008 
00009 Algorithm::Algorithm(PathPlanner* planner) 
00010   : start_(planner->getConfigurationSpace()->size()),
00011     goal_ (planner->getConfigurationSpace()->size()),
00012     isRunning_(false), planner_(planner), verbose_(false)
00013 {
00014   properties_["interpolation-distance"] = "0.1"; 
00015   roadmap_ = RoadmapPtr(new Roadmap(planner_));
00016 }
00017 
00018 Algorithm::~Algorithm()
00019 {
00020 }
00021 
00022 void Algorithm::setProperty(const std::string& key, const std::string& value)
00023 {
00024     properties_[key] = value;
00025 }
00026 
00027 void Algorithm::setProperties(const std::map<std::string, std::string> &properties) 
00028 {
00029   std::map<std::string, std::string>::const_iterator it;
00030   it = properties.begin();
00031   while (it != properties.end()) {
00032     properties_[it->first] = it->second;
00033     it++;
00034   }
00035 }
00036 
00037 void Algorithm::getProperties(std::vector<std::string> &names,
00038                               std::vector<std::string> &values) {
00039   names.clear();
00040   values.clear();
00041 
00042   std::map<std::string, std::string>::iterator it;
00043   it = properties_.begin();
00044   while (it != properties_.end()) {
00045     names.push_back((*it).first);
00046     values.push_back((*it).second);
00047     it++;
00048   }
00049 }
00050 
00051 bool Algorithm::tryDirectConnection()
00052 {
00053   Mobility *mobility = planner_->getMobility();
00054 
00055   if (mobility->isReachable(start_, goal_)){
00056     path_.push_back(start_);
00057     path_.push_back(goal_);
00058     return true;
00059   }else{
00060     return false;
00061   }
00062 }
00063 
00064 bool Algorithm::preparePlanning()
00065 {
00066   isRunning_ = true;
00067 
00068   Mobility::interpolationDistance(atof(properties_["interpolation-distance"].c_str()));
00069 #if 1
00070   std::map<std::string, std::string>::iterator it;
00071   it = properties_.begin();
00072   std::cout << "properties:" << std::endl;
00073   while (it != properties_.end()) {
00074       std::cout << "  " << it->first << " = " << it->second << std::endl;
00075       it++;
00076   }
00077   std::cout << std::endl;
00078 #endif
00079 
00080   path_.clear();
00081 
00082   std::cout << "start:" << start_ << std::endl;
00083   std::cout << "goal:" << goal_ << std::endl;
00084 
00085   // validity checks of start&goal configurations
00086   ConfigurationSpace *cspace = planner_->getConfigurationSpace();
00087   if (!cspace->isValid(start_)){
00088       std::cerr << "start configuration is invalid" << std::endl;
00089       return false;
00090   }
00091   if (!ignoreCollisionAtStart_ && planner_->checkCollision(start_)){
00092       const std::pair<std::string, std::string> &pair
00093           = planner_->collidingPair();
00094       std::cerr << "start configuration is not collision-free("
00095                 << pair.first << "<->" << pair.second << ")" << std::endl;
00096       return false;
00097   }
00098   if (!cspace->isValid(goal_)){
00099       std::cerr << "goal configuration is invalid" << std::endl;
00100       return false;
00101   }
00102   if (!ignoreCollisionAtGoal_ && planner_->checkCollision(goal_)){
00103       const std::pair<std::string, std::string> &pair
00104           = planner_->collidingPair();
00105       std::cerr << "goal configuration is not collision-free("
00106                 << pair.first << "<->" << pair.second << ")" << std::endl;
00107       return false;
00108   }
00109 #if 0
00110   Mobility *mobility = planner_->getMobility();
00111   if (!mobility->isReachable(start_, goal_, false)){
00112       std::cerr << "goal is not reachable even if collision is not checked"
00113                 << std::endl;
00114       return false;
00115   }
00116 #endif
00117 
00118   return true;
00119 }


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:52