Go to the documentation of this file.00001
00002
00003 #ifndef __PATH_PLANNER_H__
00004 #define __PATH_PLANNER_H__
00005
00006 #include <string>
00007 #include <map>
00008 #include <iostream>
00009 #include <sstream>
00010 #include <boost/function.hpp>
00011 #include "hrpUtil/TimeMeasure.h"
00012
00013 #include "exportdef.h"
00014 #include "Algorithm.h"
00015 #include "Configuration.h"
00016 #include "ConfigurationSpace.h"
00017 #include "Mobility.h"
00018 #include "Optimizer.h"
00019 #include "CollisionDetector.h"
00020 #include "hrpCollision/ColdetModelPair.h"
00021 #undef random
00022
00023 #include <hrpCorba/ORBwrap.h>
00024 #include <hrpCorba/ModelLoader.hh>
00025 #include <hrpCorba/CollisionDetector.hh>
00026 #include <hrpCorba/OnlineViewer.hh>
00027
00028 #include <hrpModel/World.h>
00029 #include <hrpModel/ConstraintForceSolver.h>
00030 #include <hrpUtil/TimeMeasure.h>
00031
00032 namespace PathEngine {
00033 class Algorithm;
00034 class Mobility;
00035 class PathPlanner;
00036
00037 typedef boost::function2<bool, PathPlanner *, const Configuration &> applyConfigFunc;
00038 typedef boost::shared_ptr<hrp::World<hrp::ConstraintForceSolver> > WorldPtr;
00044 class HRPPLANNER_API PathPlanner {
00045
00046 private:
00047 applyConfigFunc m_applyConfigFunc;
00048 typedef std::map<const std::string, std::pair<AlgorithmNewFunc, AlgorithmDeleteFunc> > AlgorithmFactory;
00049 typedef AlgorithmFactory::value_type AlgorithmFactoryValueType;
00053 AlgorithmFactory algorithmFactory_;
00054
00055 typedef std::map<std::string, std::pair<MobilityNewFunc, MobilityDeleteFunc> > MobilityFactory;
00056 typedef MobilityFactory::value_type MobilityFactoryValueType;
00060 MobilityFactory mobilityFactory_;
00061
00062 typedef std::map<std::string, std::pair<OptimizerNewFunc, OptimizerDeleteFunc> > OptimizerFactory;
00063 typedef OptimizerFactory::value_type OptimizerFactoryValueType;
00067 OptimizerFactory optimizerFactory_;
00068
00072 OpenHRP::ModelLoader_var modelLoader_;
00073
00077 OpenHRP::OnlineViewer_var onlineViewer_;
00078
00082 std::string algorithmName_;
00083
00087 Algorithm* algorithm_;
00088
00092 std::string mobilityName_;
00093
00097 Mobility* mobility_;
00098
00102 ConfigurationSpace cspace_;
00103
00107 hrp::BodyPtr model_;
00108
00112 bool debug_;
00113
00117 double dt_;
00118
00126 template<typename X, typename X_ptr>
00127 X_ptr checkCorbaServer(const std::string &n, CosNaming::NamingContext_var &cxt);
00128
00132 std::vector<Configuration> path_;
00133
00137 unsigned int countCollisionCheck_;
00138
00142 TimeMeasure timeCollisionCheck_, timeForwardKinematics_;
00143
00144 CORBA::ORB_var orb_;
00145
00146 WorldPtr world_;
00147
00148 OpenHRP::CollisionDetector_var collisionDetector_;
00149 OpenHRP::CharacterPositionSequence_var allCharacterPositions_;
00150
00151 void _setupCharacterData();
00152 void _updateCharacterPositions();
00153
00154 bool bboxMode_;
00155
00156 std::vector<hrp::ColdetModelPair> checkPairs_;
00157
00158 std::vector<hrp::Vector3> pointCloud_;
00159 double radius_;
00160
00161 std::pair<std::string, std::string> collidingPair_;
00162
00163 CollisionDetector *customCollisionDetector_;
00164
00165 bool defaultCheckCollision();
00166 public:
00171 WorldPtr world();
00172
00177 hrp::BodyPtr robot();
00178
00183 void setApplyConfigFunc(applyConfigFunc i_func);
00184
00189 bool setConfiguration(const Configuration &pos);
00190
00195 void getWorldState(OpenHRP::WorldState_out wstate);
00196
00202 void setPointCloud(const std::vector<hrp::Vector3>& i_cloud, double i_radius);
00203
00210 PathPlanner(unsigned int dim,
00211 WorldPtr world = WorldPtr(),
00212 bool isDebugMode = false);
00213
00217 ~PathPlanner();
00218
00226 void registerAlgorithm(const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc);
00227
00235 void registerMobility(const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc);
00236
00244 void registerOptimizer(const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc);
00245
00250 void initPlanner(const std::string &nameServer);
00251
00257 hrp::BodyPtr registerCharacter(const char* name,OpenHRP::BodyInfo_ptr cInfo);
00258
00264 hrp::BodyPtr registerCharacter(const char *name, hrp::BodyPtr i_body);
00265
00271 hrp::BodyPtr registerCharacterByURL(const char* name, const char* url);
00272
00279 void setCharacterPosition(const char* character,
00280 const OpenHRP::DblSequence& pos);
00281
00290 void registerIntersectionCheckPair(const char* char1,
00291 const char* name1,
00292 const char* char2,
00293 const char* name2,
00294 CORBA::Double tolerance);
00295
00296
00300 void initSimulation();
00301
00307 void getMobilityNames(std::vector<std::string> &mobilitys);
00308
00314 void getAlgorithmNames(std::vector<std::string> &algorithms);
00315
00321 void getOptimizerNames(std::vector<std::string> &optimizers);
00322
00330 bool getProperties(const std::string &algorithm,
00331 std::vector<std::string> &names,
00332 std::vector<std::string> &values);
00333
00338 void setRobotName(const std::string &model);
00339
00340
00345 void setAlgorithmName(const std::string &algorithm);
00346
00347
00353 bool setMobilityName(const std::string &mobility);
00354
00359 void setProperties(const std::map<std::string, std::string> &properties) {algorithm_->setProperties(properties);}
00364 void setStartConfiguration(const Configuration &pos) {algorithm_->setStartConfiguration(pos);}
00365
00370 void setGoalConfiguration(const Configuration &pos) {algorithm_->setGoalConfiguration(pos);}
00371
00376 bool calcPath();
00377
00381 void stopPlanning() {algorithm_->stopPlanning();}
00382
00387 RoadmapPtr getRoadmap() { return algorithm_->getRoadmap();}
00388
00393 std::vector<Configuration> getPath();
00394
00399 std::vector<Configuration>& getWayPoints();
00400
00405 Mobility* getMobility() {return mobility_;}
00406
00411 Algorithm* getAlgorithm() {return algorithm_;}
00412
00417 ConfigurationSpace* getConfigurationSpace() { return &cspace_; }
00418
00423 bool checkCollision();
00429 bool checkCollision(const Configuration &pos);
00430
00436 bool checkCollision(const std::vector<Configuration> &path);
00437
00442 void setDebug(bool debug) {debug_ = debug;}
00443
00449 bool optimize(const std::string& optimizer);
00450
00455 unsigned int countCollisionCheck() const { return timeCollisionCheck_.numCalls();}
00456
00461 double timeCollisionCheck() const;
00462
00463 double timeForwardKinematics() const;
00464
00465 void boundingBoxMode(bool mode) { bboxMode_ = mode; }
00466
00467 const std::pair<std::string, std::string> &collidingPair() { return collidingPair_; }
00468
00469 void setCollisionDetector(CollisionDetector *i_cd){
00470 customCollisionDetector_ = i_cd;
00471 }
00472 };
00473 };
00474 #endif // __PATH_PLANNER_H__