PathPlanner.h
Go to the documentation of this file.
00001 //-*- C++ -*-
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         //< point cloud created by vision or range sensor
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__


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