hrplib/hrpPlanner/PathPlanner.h
Go to the documentation of this file.
1 //-*- C++ -*-
2 
3 #ifndef __PATH_PLANNER_H__
4 #define __PATH_PLANNER_H__
5 
6 #include <string>
7 #include <map>
8 #include <iostream>
9 #include <sstream>
10 #include <boost/function.hpp>
11 #include "hrpUtil/TimeMeasure.h"
12 
13 #include "exportdef.h"
14 #include "Algorithm.h"
15 #include "Configuration.h"
16 #include "ConfigurationSpace.h"
17 #include "Mobility.h"
18 #include "Optimizer.h"
19 #include "CollisionDetector.h"
21 #undef random
22 
23 #include <hrpCorba/ORBwrap.h>
24 #include <hrpCorba/ModelLoader.hh>
25 #include <hrpCorba/CollisionDetector.hh>
26 #include <hrpCorba/OnlineViewer.hh>
27 
28 #include <hrpModel/World.h>
30 #include <hrpUtil/TimeMeasure.h>
31 
32 namespace PathEngine {
33  class Algorithm;
34  class Mobility;
35  class PathPlanner;
36 
37  typedef boost::function2<bool, PathPlanner *, const Configuration &> applyConfigFunc;
38  typedef boost::shared_ptr<hrp::World<hrp::ConstraintForceSolver> > WorldPtr;
45 
46  private:
47  applyConfigFunc m_applyConfigFunc;
48  typedef std::map<const std::string, std::pair<AlgorithmNewFunc, AlgorithmDeleteFunc> > AlgorithmFactory;
49  typedef AlgorithmFactory::value_type AlgorithmFactoryValueType;
53  AlgorithmFactory algorithmFactory_;
54 
55  typedef std::map<std::string, std::pair<MobilityNewFunc, MobilityDeleteFunc> > MobilityFactory;
56  typedef MobilityFactory::value_type MobilityFactoryValueType;
60  MobilityFactory mobilityFactory_;
61 
62  typedef std::map<std::string, std::pair<OptimizerNewFunc, OptimizerDeleteFunc> > OptimizerFactory;
63  typedef OptimizerFactory::value_type OptimizerFactoryValueType;
67  OptimizerFactory optimizerFactory_;
68 
72  OpenHRP::ModelLoader_var modelLoader_;
73 
77  OpenHRP::OnlineViewer_var onlineViewer_;
78 
82  std::string algorithmName_;
83 
88 
92  std::string mobilityName_;
93 
98 
103 
108 
112  bool debug_;
113 
117  double dt_;
118 
126  template<typename X, typename X_ptr>
127  X_ptr checkCorbaServer(const std::string &n, CosNaming::NamingContext_var &cxt);
128 
132  std::vector<Configuration> path_;
133 
137  unsigned int countCollisionCheck_;
138 
142  TimeMeasure timeCollisionCheck_, timeForwardKinematics_;
143 
144  CORBA::ORB_var orb_;
145 
146  WorldPtr world_;
147 
148  OpenHRP::CollisionDetector_var collisionDetector_;
149  OpenHRP::CharacterPositionSequence_var allCharacterPositions_;
150 
151  void _setupCharacterData();
152  void _updateCharacterPositions();
153 
154  bool bboxMode_;
155 
156  std::vector<hrp::ColdetModelPair> checkPairs_;
157  //< point cloud created by vision or range sensor
158  std::vector<hrp::Vector3> pointCloud_;
159  double radius_;
160 
161  std::pair<std::string, std::string> collidingPair_;
162 
164 
165  bool defaultCheckCollision();
166  public:
171  WorldPtr world();
172 
177  hrp::BodyPtr robot();
178 
183  void setApplyConfigFunc(applyConfigFunc i_func);
184 
189  bool setConfiguration(const Configuration &pos);
190 
195  void getWorldState(OpenHRP::WorldState_out wstate);
196 
202  void setPointCloud(const std::vector<hrp::Vector3>& i_cloud, double i_radius);
203 
210  PathPlanner(unsigned int dim,
211  WorldPtr world = WorldPtr(),
212  bool isDebugMode = false);
213 
217  ~PathPlanner();
218 
226  void registerAlgorithm(const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc);
227 
235  void registerMobility(const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc);
236 
244  void registerOptimizer(const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc);
245 
250  void initPlanner(const std::string &nameServer);
251 
257  hrp::BodyPtr registerCharacter(const char* name,OpenHRP::BodyInfo_ptr cInfo);
258 
264  hrp::BodyPtr registerCharacter(const char *name, hrp::BodyPtr i_body);
265 
271  hrp::BodyPtr registerCharacterByURL(const char* name, const char* url);
272 
279  void setCharacterPosition(const char* character,
280  const OpenHRP::DblSequence& pos);
281 
290  void registerIntersectionCheckPair(const char* char1,
291  const char* name1,
292  const char* char2,
293  const char* name2,
294  CORBA::Double tolerance);
295 
296 
300  void initSimulation();
301 
307  void getMobilityNames(std::vector<std::string> &mobilitys);
308 
314  void getAlgorithmNames(std::vector<std::string> &algorithms);
315 
321  void getOptimizerNames(std::vector<std::string> &optimizers);
322 
330  bool getProperties(const std::string &algorithm,
331  std::vector<std::string> &names,
332  std::vector<std::string> &values);
333 
338  void setRobotName(const std::string &model);
339 
340 
345  void setAlgorithmName(const std::string &algorithm);
346 
347 
353  bool setMobilityName(const std::string &mobility);
354 
359  void setProperties(const std::map<std::string, std::string> &properties) {algorithm_->setProperties(properties);}
364  void setStartConfiguration(const Configuration &pos) {algorithm_->setStartConfiguration(pos);}
365 
370  void setGoalConfiguration(const Configuration &pos) {algorithm_->setGoalConfiguration(pos);}
371 
376  bool calcPath();
377 
381  void stopPlanning() {algorithm_->stopPlanning();}
382 
387  RoadmapPtr getRoadmap() { return algorithm_->getRoadmap();}
388 
393  std::vector<Configuration> getPath();
394 
399  std::vector<Configuration>& getWayPoints();
400 
405  Mobility* getMobility() {return mobility_;}
406 
411  Algorithm* getAlgorithm() {return algorithm_;}
412 
418 
423  bool checkCollision();
429  bool checkCollision(const Configuration &pos);
430 
436  bool checkCollision(const std::vector<Configuration> &path);
437 
442  void setDebug(bool debug) {debug_ = debug;}
443 
449  bool optimize(const std::string& optimizer);
450 
455  unsigned int countCollisionCheck() const { return timeCollisionCheck_.numCalls();}
456 
461  double timeCollisionCheck() const;
462 
463  double timeForwardKinematics() const;
464 
465  void boundingBoxMode(bool mode) { bboxMode_ = mode; }
466 
467  const std::pair<std::string, std::string> &collidingPair() { return collidingPair_; }
468 
470  customCollisionDetector_ = i_cd;
471  }
472  };
473 };
474 #endif // __PATH_PLANNER_H__
void setDebug(bool debug)
デバッグモードの変更
void setProperties(const std::map< std::string, std::string > &properties)
アルゴリズムに対して各種情報を設定する
Definition: Algorithm.cpp:27
std::vector< hrp::Vector3 > pointCloud_
void(* OptimizerDeleteFunc)(Optimizer *optimizer)
Definition: Optimizer.h:48
std::vector< hrp::ColdetModelPair > checkPairs_
CollisionDetector * customCollisionDetector_
static bool debug
double radius_
radius of spheres assigned to points
void(* MobilityDeleteFunc)(Mobility *mobility)
移動アルゴリズム解放関数
Definition: Mobility.h:106
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
std::vector< Configuration > path_
経路
OptimizerFactory::value_type OptimizerFactoryValueType
png_infop png_charpp name
Definition: png.h:2382
void setGoalConfiguration(const Configuration &pos)
ゴール位置を設定する
boost::function2< bool, PathPlanner *, const Configuration & > applyConfigFunc
double dt_
デバッグモード時に使用する現在時刻
AlgorithmFactory algorithmFactory_
経路計画アルゴリズムのファクトリ
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
void setProperties(const std::map< std::string, std::string > &properties)
アルゴリズムに対して各種情報を設定する
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
std::map< std::string, std::pair< MobilityNewFunc, MobilityDeleteFunc > > MobilityFactory
void getWorldState(WorldState &state, WorldBase &world)
OptimizerFactory optimizerFactory_
経路最適化アルゴリズムのファクトリ
bool debug_
デバッグモード
OpenHRP::CollisionDetector_var collisionDetector_
void setCollisionDetector(CollisionDetector *i_cd)
const std::pair< std::string, std::string > & collidingPair()
hrp::BodyPtr model_
経路計画の対象とするロボット
Mobility * getMobility()
移動能力を取得する
Mobility * mobility_
使用する移動能力
ConfigurationSpace cspace_
コンフィギュレーション空間
std::string algorithmName_
使用する経路計画アルゴリズム名
Algorithm * getAlgorithm()
計画アルゴリズムを取得する
unsigned int countCollisionCheck() const
干渉チェックを呼び出した回数を取得する
Algorithm * algorithm_
使用する経路計画アルゴリズム
Mobility *(* MobilityNewFunc)(PathPlanner *planner)
移動アルゴリズム生成関数
Definition: Mobility.h:101
unsigned int countCollisionCheck_
干渉チェック呼び出し回数
std::map< std::string, std::pair< OptimizerNewFunc, OptimizerDeleteFunc > > OptimizerFactory
RoadmapPtr getRoadmap()
ロードマップを取得する
Definition: Algorithm.h:159
MobilityFactory mobilityFactory_
移動能力のファクトリ
RoadmapPtr getRoadmap()
ロードマップを取得する
MobilityFactory::value_type MobilityFactoryValueType
OpenHRP::OnlineViewer_var onlineViewer_
デバッグモード時に使用する OnlineViewer への参照
void stopPlanning()
計算を止める
Definition: Algorithm.h:147
path
AlgorithmFactory::value_type AlgorithmFactoryValueType
Optimizer *(* OptimizerNewFunc)(PathPlanner *planner)
Definition: Optimizer.h:43
boost::shared_ptr< hrp::World< hrp::ConstraintForceSolver > > WorldPtr
経路計画アルゴリズム基底クラス
Definition: Algorithm.h:23
void stopPlanning()
計算を中止する
#define HRPPLANNER_API
Definition: exportdef.h:22
void setStartConfiguration(const Configuration &pos)
スタート位置を設定する
std::string mobilityName_
使用する移動能力名
Algorithm *(* AlgorithmNewFunc)(PathPlanner *planner)
Definition: Algorithm.h:187
std::pair< std::string, std::string > collidingPair_
void setGoalConfiguration(const Configuration &pos)
終了位置を設定する
Definition: Algorithm.h:126
boost::shared_ptr< Roadmap > RoadmapPtr
Definition: Roadmap.h:13
void setStartConfiguration(const Configuration &pos)
初期位置を設定する
Definition: Algorithm.h:120
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
Definition: clap.cpp:19
std::map< const std::string, std::pair< AlgorithmNewFunc, AlgorithmDeleteFunc > > AlgorithmFactory
void(* AlgorithmDeleteFunc)(Algorithm *algorithm)
Definition: Algorithm.h:192
OpenHRP::ModelLoader_var modelLoader_
ModelLoader への参照
OpenHRP::CharacterPositionSequence_var allCharacterPositions_


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04