Mobility.cpp
Go to the documentation of this file.
00001 #include "PathPlanner.h"
00002 #include "Mobility.h"
00003 
00004 using namespace PathEngine;
00005 
00006 double Mobility::interpolationDistance_ = 0.1;
00007 
00008 bool Mobility::isReachable(Configuration& from, Configuration& to,
00009                            bool checkCollision) const
00010 {
00011     std::vector<Configuration> path;
00012     if (!getPath(from, to, path)) return false;
00013 #if 0
00014     std::cout << "isReachable(" << from << ", " << to << ")" << std::endl;
00015     for (unsigned int  i = 0; i<path.size(); i++){
00016         std::cout << i << ":" << path[i] << std::endl;
00017     }
00018 #endif
00019     if (checkCollision){
00020         return !planner_->checkCollision(path);
00021     }else{
00022         return true;
00023     }
00024 }
00025 
00026 bool Mobility::getPath(Configuration &from, Configuration &to,
00027                        std::vector<Configuration>& o_path) const
00028 {
00029     o_path.push_back(from);
00030     
00031     unsigned int n = (unsigned int)(distance(from, to)/interpolationDistance())+1;
00032     Configuration pos(planner_->getConfigurationSpace()->size());
00033     for (unsigned int i=1; i<n; i++){
00034         pos = interpolate(from, to, ((double)i)/n);
00035         o_path.push_back(pos);
00036     }
00037     o_path.push_back(to);
00038       
00039     return true;
00040 }
00041 
00042 
00043 


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