hrplib/hrpPlanner/PathPlanner.cpp
Go to the documentation of this file.
1 #include <math.h>
2 #include <boost/bind.hpp>
3 // planning algorithms
4 #include "RRT.h"
5 #include "PRM.h"
6 // mobilities
7 #include "TGT.h"
8 #include "OmniWheel.h"
9 // optimizers
10 #include "ShortcutOptimizer.h"
12 //
13 #include "ConfigurationSpace.h"
14 #include "PathPlanner.h"
15 
16 #include <hrpCorba/OpenHRPCommon.hh>
17 #include <hrpModel/Body.h>
18 #include <hrpModel/Link.h>
20 
21 using namespace PathEngine;
22 using namespace hrp;
23 
24 //static const bool USE_INTERNAL_COLLISION_DETECTOR = false;
25 static const bool USE_INTERNAL_COLLISION_DETECTOR = true;
26 
27 // ----------------------------------------------
28 // ネームサーバからオブジェクトを取得
29 // ----------------------------------------------
30 template<typename X, typename X_ptr>
31 X_ptr PathPlanner::checkCorbaServer(const std::string &n, CosNaming::NamingContext_var &cxt)
32 {
33  CosNaming::Name ncName;
34  ncName.length(1);
35  ncName[0].id = CORBA::string_dup(n.c_str());
36  ncName[0].kind = CORBA::string_dup("");
37  X_ptr srv = NULL;
38  try {
39  srv = X::_narrow(cxt->resolve(ncName));
40  } catch(const CosNaming::NamingContext::NotFound &exc) {
41  std::cerr << n << " not found: ";
42  switch(exc.why) {
43  case CosNaming::NamingContext::missing_node:
44  std::cerr << "Missing Node" << std::endl;
45  case CosNaming::NamingContext::not_context:
46  std::cerr << "Not Context" << std::endl;
47  break;
48  case CosNaming::NamingContext::not_object:
49  std::cerr << "Not Object" << std::endl;
50  break;
51  }
52  return (X_ptr)NULL;
53  } catch(CosNaming::NamingContext::CannotProceed &exc) {
54  std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
55  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
56  std::cerr << "Resolve " << n << " InvalidName" << std::endl;
57  }
58  return srv;
59 }
60 
62 {
63  Link *baseLink = planner->robot()->rootLink();
64  // 目標値設定
65  baseLink->p(0) = cfg.value(0);
66  baseLink->p(1) = cfg.value(1);
67  Matrix33 R;
68 
69  double c = cos(cfg.value(2)), s = sin(cfg.value(2));
70  R(0,0) = c; R(0,1) = -s; R(0,2) = 0;
71  R(1,0) = s; R(1,1) = c; R(1,2) = 0;
72  R(2,0) = 0; R(2,1) = 0; R(2,2) = 1;
73  baseLink->setSegmentAttitude(R);
74  planner->robot()->calcForwardKinematics();
75 
76  return true;
77 }
78 
79 // ----------------------------------------------
80 // コンストラクタ
81 // ----------------------------------------------
82 PathPlanner::PathPlanner(unsigned int dim,
83  boost::shared_ptr<hrp::World<hrp::ConstraintForceSolver> > world,
84  bool isDebugMode)
85  : m_applyConfigFunc(setConfigurationToBaseXYTheta), algorithm_(NULL), mobility_(NULL), cspace_(dim), debug_(isDebugMode), world_(world), customCollisionDetector_(NULL)
86 {
87  if (isDebugMode) {
88  std::cerr << "PathPlanner::PathPlanner() : debug mode" << std::endl;
89  }
90 
91  if (!world_){
93  }
94 
95  // planning algorithms
96  registerAlgorithm("RRT", AlgorithmCreate<RRT>, AlgorithmDelete<RRT>);
97  registerAlgorithm("PRM", AlgorithmCreate<PRM>, AlgorithmDelete<PRM>);
98 
99  // mobilities
100  registerMobility("TurnGoTurn", MobilityCreate<TGT>, MobilityDelete<TGT>);
101  registerMobility("OmniWheel", MobilityCreate<OmniWheel>, MobilityDelete<OmniWheel>);
102  //registerMobility("ArcAndOdv", MobilityCreate<ArcAndOdv>, MobilityDelete<ArcAndOdv>);
103 
104  // optimizers
105  registerOptimizer("Shortcut",
106  OptimizerCreate<ShortcutOptimizer>,
107  OptimizerDelete<ShortcutOptimizer>);
108  registerOptimizer("RandomShortcut",
109  OptimizerCreate<RandomShortcutOptimizer>,
110  OptimizerDelete<RandomShortcutOptimizer>);
111 
112  allCharacterPositions_ = new OpenHRP::CharacterPositionSequence;
113 
114  //bboxMode_ = true;
115  bboxMode_ = false;
116 
117  dt_ = 0.01;
118 }
119 
120 // ----------------------------------------------
121 // デストラクタ
122 // ----------------------------------------------
124  if (debug_) {
125  std::cerr << "PathPlanner::~PathPlanner()" << std::endl;
126  }
127  if (algorithm_ != NULL) {
129  }
130 
131  if (mobility_ != NULL) {
133  }
134 }
135 
136 void PathPlanner::getOptimizerNames(std::vector<std::string> &optimizers) {
137  optimizers.clear();
138  OptimizerFactory::iterator it = optimizerFactory_.begin();
139  while (it != optimizerFactory_.end()) {
140  optimizers.push_back((*it).first);
141  it++;
142  }
143 }
144 
145 void PathPlanner::getMobilityNames(std::vector<std::string> &mobilities) {
146  mobilities.clear();
147  MobilityFactory::iterator it = mobilityFactory_.begin();
148  while (it != mobilityFactory_.end()) {
149  mobilities.push_back((*it).first);
150  it++;
151  }
152 }
153 
154 // ----------------------------------------------
155 // アルゴリズム一覧取得
156 // ----------------------------------------------
157 void PathPlanner::getAlgorithmNames(std::vector<std::string> &algorithms) {
158  algorithms.clear();
159  AlgorithmFactory::iterator it = algorithmFactory_.begin();
160  while (it != algorithmFactory_.end()) {
161  algorithms.push_back((*it).first);
162  it++;
163  }
164 }
165 
166 // ----------------------------------------------
167 // プロパティ一覧取得
168 // ----------------------------------------------
169 bool PathPlanner::getProperties(const std::string &algorithm,
170  std::vector<std::string> &names,
171  std::vector<std::string> &values)
172 {
173  if (debug_) {
174  std::cerr << "PathPlanner::getPropertyNames(" << algorithm << ")" << std::endl;
175  }
176  if (algorithmFactory_.count(algorithm) > 0) {
177  Algorithm* algorithmInst = algorithmFactory_[algorithm].first(this);
178  algorithmInst->getProperties(names, values);
179  algorithmFactory_[algorithm].second(algorithmInst);
180  }
181  else {
182  std::cerr << "algorithm not found" << std::endl;
183  return false;
184  }
185  return true;
186 }
187 
188 // ----------------------------------------------
189 // 初期化
190 // ----------------------------------------------
191 void PathPlanner::initPlanner(const std::string &nameServer) {
192 
193  if (debug_) {
194  std::cerr << "PathPlanner::initPlanner(" << nameServer << ")" << std::endl;
195  }
196 
197  try {
198  int ac = 0;
199  char* av[1];
200  av[0] = NULL;
201  orb_ = CORBA::ORB_init(ac, av);
202  //
203  // Resolve Root POA
204  //
205  CORBA::Object_var poaObj = orb_ -> resolve_initial_references("RootPOA");
206  PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
207 
208  //
209  // Get a reference to the POA manager
210  //
211  PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
212 
213  std::ostringstream stream;
214  stream << "corbaloc:iiop:" << nameServer << "/NameService";
215 
216  CosNaming::NamingContext_var cxT;
217  CORBA::Object_var nS = orb_->string_to_object(stream.str().c_str());
218  cxT = CosNaming::NamingContext::_narrow(nS);
219 
220  if (is_nil(cxT)) {
221  std::cerr << "name serivce not found" << std::endl;
222  }else{
223  std::cerr << "NameService OK." << std::endl;
224  }
225 
227  std::cerr << "CollisonDetectorFactory ";
228  OpenHRP::CollisionDetectorFactory_var cdFactory =
229  checkCorbaServer <OpenHRP::CollisionDetectorFactory,
230  OpenHRP::CollisionDetectorFactory_var> ("CollisionDetectorFactory", cxT);
231  if (CORBA::is_nil(cdFactory)) {
232  std::cerr << "not found" << std::endl;
233  }else{
234  std::cerr << "OK." << std::endl;
235  }
236  try{
237  collisionDetector_ = cdFactory->create();
238  }catch(...){
239  std::cerr << "failed to create CollisionDetector" << std::endl;
240  }
241  }
242 
243  std::cerr << "ModelLoader ";
244  modelLoader_ =
245  checkCorbaServer <OpenHRP::ModelLoader,
246  OpenHRP::ModelLoader_var> ("ModelLoader", cxT);
247 
248  if (CORBA::is_nil(modelLoader_)) {
249  std::cerr << "not found" << std::endl;
250  }else{
251  std::cerr << "OK." << std::endl;
252  }
253 
254  if (debug_) {
255  onlineViewer_ =
256  checkCorbaServer <OpenHRP::OnlineViewer,
257  OpenHRP::OnlineViewer_var> ("OnlineViewer", cxT);
258 
259  if (CORBA::is_nil(onlineViewer_)) {
260  std::cerr << "OnlineViewer not found" << std::endl;
261  }
262  }
263 
264  } catch (CORBA::SystemException& ex) {
265  std::cerr << ex._rep_id() << std::endl;
266  return;
267  }
268 
269  world_->clearBodies();
270  checkPairs_.clear();
271 
272 }
273 
274 void PathPlanner::setAlgorithmName(const std::string &algorithmName)
275 {
276  // アルゴリズム名とインスタンスをセット
277  algorithmName_ = algorithmName;
278  if (algorithmFactory_.count(algorithmName) > 0) {
279  if (algorithm_ != NULL) {
281  }
283  } else {
284  std::cerr << "algorithm(" << algorithmName << ") not found"
285  << std::endl;
286  }
287 }
288 
289 // ----------------------------------------------
290 // キャラクタをURLから登録
291 // ----------------------------------------------
292 BodyPtr PathPlanner::registerCharacterByURL(const char* name, const char* url){
293  if (debug_) {
294  std::cerr << "PathPlanner::registerCharacterByURL(" << name << ", " << url << ")" << std::endl;
295  }
296 
297  if (CORBA::is_nil(modelLoader_)) {
298  std::cerr << "nil reference to servers" << std::endl;
299  return BodyPtr();
300  }
301 
302  OpenHRP::BodyInfo_ptr cInfo = modelLoader_->getBodyInfo(url);
303  BodyPtr body = registerCharacter(name, cInfo);
304 
305  if (debug_) {
306  if (CORBA::is_nil(onlineViewer_)) {
307  std::cerr << "nil reference to OnlineViewer" << std::endl;
308  }
309  onlineViewer_->load(name, url);
310  }
311  return body;
312 }
313 
314 // ----------------------------------------------
315 // 位置の設定
316 // ----------------------------------------------
317 void PathPlanner::setCharacterPosition(const char* character,
318  const OpenHRP::DblSequence& pos)
319 {
320  if (debug_){
321  std::cerr << "PathPlanner::setCharacterPosition(" << character << ", "
322  << pos[0] << ", " << pos[1] << ", " << pos[2] << ")"
323  << std::endl;
324  }
325  BodyPtr body = world_->body(character);
326  if (!body) std::cerr << "PathPlanner::setCharacterPosition() : character("
327  << character << ") not found" << std::endl;
328  Link* l = body->rootLink();
329  l->p(0) = pos[0];
330  l->p(1) = pos[1];
331  l->p(2) = pos[2];
332  Matrix33 R;
333  getMatrix33FromRowMajorArray(R, pos.get_buffer(), 3);
334  l->setSegmentAttitude(R);
335 }
336 
337 void computeBoundingBox(BodyPtr body, double min[3], double max[3])
338 {
339  bool firsttime = true;
340  Vector3 v, p;
341  Link *l, *root=body->rootLink();
342  Matrix33 Rt = root->R.transpose();
343  float x, y, z;
344  for (unsigned int i=0; i<body->numLinks(); i++){
345  l = body->link(i);
346  for (int j=0; j<l->coldetModel->getNumVertices(); j++){
347  l->coldetModel->getVertex(j, x, y, z);
348  v[0] = x; v[1] = y; v[2] = z;
349  p = Rt*(l->R*v+l->p-root->p);
350  if (firsttime){
351  for (int k=0; k<3; k++) min[k] = max[k] = p[k];
352  firsttime = false;
353  }else{
354  for (int k=0; k<3; k++){
355  if (p[k] < min[k]) min[k] = p[k];
356  if (p[k] > max[k]) max[k] = p[k];
357  }
358  }
359  }
360  }
361  std::cerr << "bounding box of " << body->name() << ": ("
362  << min[0] << ", " << min[1] << ", " << min[2] << ") - ("
363  << max[0] << ", " << max[1] << ", " << max[2] << ")"
364  << std::endl;
365 }
366 
368 {
369  double min[3], max[3];
370  computeBoundingBox(body, min, max);
371 
372  ColdetModelPtr coldetModel(new ColdetModel());
373  coldetModel->setNumVertices(8);
374  coldetModel->setNumTriangles(12);
375  coldetModel->setVertex(0, max[0], max[1], max[2]);
376  coldetModel->setVertex(1, min[0], max[1], max[2]);
377  coldetModel->setVertex(2, min[0], min[1], max[2]);
378  coldetModel->setVertex(3, max[0], min[1], max[2]);
379  coldetModel->setVertex(4, max[0], max[1], min[2]);
380  coldetModel->setVertex(5, min[0], max[1], min[2]);
381  coldetModel->setVertex(6, min[0], min[1], min[2]);
382  coldetModel->setVertex(7, max[0], min[1], min[2]);
383  coldetModel->setTriangle(0, 0, 1, 2);
384  coldetModel->setTriangle(1, 0, 2, 3);
385  coldetModel->setTriangle(2, 0, 3, 7);
386  coldetModel->setTriangle(3, 0, 7, 4);
387  coldetModel->setTriangle(4, 0, 4, 5);
388  coldetModel->setTriangle(5, 0, 5, 1);
389  coldetModel->setTriangle(6, 3, 2, 6);
390  coldetModel->setTriangle(7, 3, 6, 7);
391  coldetModel->setTriangle(8, 1, 5, 6);
392  coldetModel->setTriangle(9, 1, 6, 2);
393  coldetModel->setTriangle(10, 4, 7, 6);
394  coldetModel->setTriangle(11, 4, 6, 5);
395  coldetModel->build();
396 
397  Link *root = new Link();
398  root->R = Matrix33::Identity();
399  root->Rs = Matrix33::Identity();
400  root->coldetModel = coldetModel;
401 
402  BodyPtr bboxBody = BodyPtr(new Body());
403  bboxBody->setRootLink(root);
404 
405  return bboxBody;
406 }
407 
408 // ----------------------------------------------
409 // キャラクタをBodyInfoから登録
410 // ----------------------------------------------
411 BodyPtr PathPlanner::registerCharacter(const char* name, OpenHRP::BodyInfo_ptr cInfo) {
412  if (debug_) {
413  std::cerr << "PathPlanner::registerCharacter(" << name << ", " << cInfo << ")" << std::endl;
414  }
415 
416  BodyPtr body(new Body());
417 
419  body->setName(name);
420  if(debug_){
421  //std::cerr << "Loaded Model:\n" << *body << std::endl;
422  }
423 
425  body = createBoundingBoxBody(body);
426  body->setName(name);
427  }
428 
430  collisionDetector_->registerCharacter(name, cInfo);
431  }
432  world_->addBody(body);
433  }
434  return body;
435 }
436 
438 {
439  i_body->setName(name);
440  world_->addBody(i_body);
441  return i_body;
442 }
443 
444 
445 
446 
447 // ----------------------------------------------
448 // ロボット名を設定
449 // ----------------------------------------------
450 void PathPlanner::setRobotName(const std::string &name) {
451  if (debug_) {
452  std::cerr << "PathPlanner::setRobotName(" << name << ")" << std::endl;
453  }
454 
455  model_ = world_->body(name);
456  if (!model_) {
457  std::cerr << "PathPlanner::setRobotName() : robot(" << name << ") not found" << std::endl;
458  return;
459  }
460 }
461 
462 // ----------------------------------------------
463 // ロボット名を設定
464 // ----------------------------------------------
465 bool PathPlanner::setMobilityName(const std::string &mobility)
466 {
467  if (debug_) {
468  std::cerr << "PathPlanner::setMobilityName(" << mobility << ")" << std::endl;
469  }
470 
471  mobilityName_ = mobility;
472  if (mobilityFactory_.count(mobilityName_) > 0) {
473  if (mobility_ != NULL) {
475  }
477  }
478  else {
479  std::cerr << "PathPlanner::setMobilityName() : mobility(" << mobility << ") not found" << std::endl;
480  return false;
481  }
482  return true;
483 }
484 
485 
486 
487 // ----------------------------------------------
488 // 衝突チェックペアを登録
489 // ----------------------------------------------
490 void PathPlanner::registerIntersectionCheckPair(const char* charName1,
491  const char* linkName1,
492  const char* charName2,
493  const char* linkName2,
494  CORBA::Double tolerance) {
495  if (debug_){
496  std::cerr << "PathPlanner::registerIntersectionCheckPair("
497  << charName1 << ", " << linkName1 << ", " << charName2
498  << ", " << linkName2
499  << ", " << tolerance << ")" << std::endl;
500  }
501  int bodyIndex1 = world_->bodyIndex(charName1);
502  int bodyIndex2 = world_->bodyIndex(charName2);
503 
504  if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
505 
506  BodyPtr body1 = world_->body(bodyIndex1);
507  BodyPtr body2 = world_->body(bodyIndex2);
508 
509  std::string emptyString = "";
510  std::vector<Link*> links1;
511  if(emptyString == linkName1){
512  const LinkTraverse& traverse = body1->linkTraverse();
513  links1.resize(traverse.numLinks());
514  std::copy(traverse.begin(), traverse.end(), links1.begin());
515  } else {
516  links1.push_back(body1->link(linkName1));
517  }
518 
519  std::vector<Link*> links2;
520  if(emptyString == linkName2){
521  const LinkTraverse& traverse = body2->linkTraverse();
522  links2.resize(traverse.numLinks());
523  std::copy(traverse.begin(), traverse.end(), links2.begin());
524  } else {
525  links2.push_back(body2->link(linkName2));
526  }
527 
528  for(size_t i=0; i < links1.size(); ++i){
529  for(size_t j=0; j < links2.size(); ++j){
530  Link* link1 = links1[i];
531  Link* link2 = links2[j];
532 
533  if(link1 && link2 && link1 != link2){
535  checkPairs_.push_back(ColdetModelPair(link1->coldetModel,
536  link2->coldetModel,
537  tolerance));
538  }else{
539  OpenHRP::LinkPair_var linkPair = new OpenHRP::LinkPair();
540  linkPair->charName1 = CORBA::string_dup(charName1);
541  linkPair->linkName1 = CORBA::string_dup(link1->name.c_str());
542  linkPair->charName2 = CORBA::string_dup(charName2);
543  linkPair->linkName2 = CORBA::string_dup(link2->name.c_str());
544  linkPair->tolerance = tolerance;
545  collisionDetector_->addCollisionPair(linkPair);
546  }
547  }
548  }
549  }
550  }
551 }
552 // ----------------------------------------------
553 // 初期化
554 // ----------------------------------------------
556  world_->setTimeStep(0.002);
557  world_->setCurrentTime(0.0);
558  world_->setRungeKuttaMethod();
559  world_->enableSensors(true);
560 
561  int n = world_->numBodies();
562  for(int i=0; i < n; ++i){
563  world_->body(i)->initializeConfiguration();
564  }
565 
567 
568  Vector3 g;
569  g[0] = g[1] = 0.0; g[2] = 9.8;
570  world_->setGravityAcceleration(g);
571 
572  world_->initialize();
573 }
574 
576 {
577  return m_applyConfigFunc(this, pos);
578 }
579 
581 #if 0
582  if (debug_) {
583  std::cerr << "checkCollision(" << pos << ")" << std::endl;
584  }
585 #endif
586  if (!setConfiguration(pos)) return true;
587 
588  // 干渉チェック
589  bool ret = checkCollision();
590 
591  if (debug_) {
592  // 結果を得る
593  OpenHRP::WorldState_var state;
594  getWorldState(state);
595 
596  static double nowTime = 0;
597  state->time = nowTime;
598  nowTime += dt_;
599 
600  onlineViewer_->update(state);
601  }
602  return ret;
603 }
604 
605 void PathPlanner::getWorldState(OpenHRP::WorldState_out wstate)
606 {
607  if(debug_){
608  std::cerr << "DynamicsSimulator_impl::getWorldState()\n";
609  }
610 
612 
613  wstate = new OpenHRP::WorldState;
614 
615  wstate->time = world_->currentTime();
616  wstate->characterPositions = allCharacterPositions_;
617 
618  if(debug_){
619  std::cerr << "getWorldState - exit" << std::endl;
620  }
621 }
622 
624 {
633  return ret;
634  }else{
635  return defaultCheckCollision();
636  }
637 }
638 
640 {
642 
644  Link *l;
645  for (unsigned int j=0; j<model_->numLinks(); j++){
646  l = model_->link(j);
647  l->coldetModel->setPosition(l->attitude(), l->p);
648  }
649  }else{
651  }
655  for (unsigned int i=0; i<checkPairs_.size(); i++){
656  if (checkPairs_[i].tolerance() == 0){
657  if (checkPairs_[i].checkCollision()){
658  collidingPair_.first = checkPairs_[i].model(0)->name();
659  collidingPair_.second = checkPairs_[i].model(1)->name();
661  return true;
662  }
663  } else{
664  if (checkPairs_[i].detectIntersection()) {
665  collidingPair_.first = checkPairs_[i].model(0)->name();
666  collidingPair_.second = checkPairs_[i].model(1)->name();
668  return true;
669  }
670  }
671  }
672  if (pointCloud_.size() > 0){
673  for (unsigned int i=0; i<model_->numLinks(); i++){
674  Link *l = model_->link(i);
675  if (l->coldetModel->checkCollisionWithPointCloud(pointCloud_,
676  radius_)){
678  return true;
679  }
680  }
681  }
683  return false;
684  }else{
685  OpenHRP::LinkPairSequence_var pairs = new OpenHRP::LinkPairSequence;
686  collisionDetector_->queryIntersectionForDefinedPairs(false, allCharacterPositions_.in(), pairs);
687 
689  return pairs->length() > 0;
690  }
691 }
692 
693 // ----------------------------------------------
694 // アルゴリズム登録
695 // ----------------------------------------------
696 void PathPlanner::registerAlgorithm(const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc) {
697  algorithmFactory_.insert(AlgorithmFactoryValueType(algorithmName, std::make_pair(newFunc, deleteFunc)));
698 }
699 
700 void PathPlanner::registerMobility(const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc) {
701  mobilityFactory_.insert(MobilityFactoryValueType(mobilityName, std::make_pair(newFunc, deleteFunc)));
702 }
703 void PathPlanner::registerOptimizer(const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc) {
704  optimizerFactory_.insert(OptimizerFactoryValueType(optimizerName, std::make_pair(newFunc, deleteFunc)));
705 }
706 bool PathPlanner::checkCollision(const std::vector<Configuration> &path) {
707  unsigned int checked = 1;
708  unsigned int div = 2;
709 
710  std::vector<bool> isVisited;
711  for (unsigned int i=0; i<path.size()-1; i++) {
712  isVisited.push_back(false);
713  }
714  isVisited.push_back(true); // the last Configuration is not checked
715 
716 
717  while (checked < (path.size()-1) || path.size()/div > 0) {
718  int step = path.size()/div;
719  for (unsigned int i=step; i<path.size(); i+=step) {
720  if (!isVisited[i]) {
721  checked++;
722  if (checkCollision(path[i])) {
723  return true;
724  }
725  isVisited[i] = true;
726  }
727  }
728  div++;
729  }
730  if (checked != path.size()-1) {
731  std::cerr << "checkCollision() : there are unchecked configurations."
732  << " path.size() = " << path.size() << ", checked = "
733  << checked << std::endl;
734  }
735  return false; // the first Configuration is not checked
736 }
738 {
739  path_.clear();
740  BodyPtr body;
741  Link *l;
742  for (unsigned int i=0; i<world_->numBodies(); i++){
743  body = world_->body(i);
744  body->calcForwardKinematics();
745  for (unsigned int j=0; j<body->numLinks(); j++){
746  l = body->link(j);
747  l->coldetModel->setPosition(l->attitude(), l->p);
748  }
749  }
750  std::cerr << "The number of collision check pairs = " << checkPairs_.size() << std::endl;
751  if (!algorithm_->preparePlanning()){
752  std::cerr << "preparePlanning() failed" << std::endl;
753  return false;
754  }
755 
757  path_ = algorithm_->getPath();
758  std::cerr << "connected directly" << std::endl;
759  return true;
760  }
761  std::cerr << "failed direct connection" << std::endl;
762 
763  if (algorithm_->calcPath()){
764  path_ = algorithm_->getPath();
765  return true;
766  }
767 
768  return false;
769 }
770 
771 bool PathPlanner::optimize(const std::string& optimizer)
772 {
773  if (optimizerFactory_.count(optimizer) > 0) {
774  Optimizer *opt = optimizerFactory_[optimizer].first(this);
775  path_ = opt->optimize(path_);
776  optimizerFactory_[optimizer].second(opt);
777  return true;
778  }else{
779  return false;
780  }
781 }
782 
783 std::vector<Configuration>& PathPlanner::getWayPoints()
784 {
785  return path_;
786 }
787 std::vector<Configuration> PathPlanner::getPath()
788 {
789  std::vector<Configuration> finalPath;
790  if (path_.size() == 0) return finalPath;
791 
792  for (unsigned int i=0; i<path_.size()-1; i++) {
793  std::vector<Configuration> localPath;
794  mobility_->getPath(path_[i], path_[i+1],localPath);
795  finalPath.insert(finalPath.end(), localPath.begin(), localPath.end()-1);
796  }
797  finalPath.push_back(path_[path_.size()-1]);
798 
799  return finalPath;
800 }
801 
803 {
804  if(debug_){
805  std::cerr << "PathPlanner::_setupCharacterData()\n";
806  }
807 
808  int n = world_->numBodies();
809  allCharacterPositions_->length(n);
810 
811  for(int i=0; i < n; ++i){
812  BodyPtr body = world_->body(i);
813 
814  int numLinks = body->numLinks();
815  OpenHRP::CharacterPosition& characterPosition = allCharacterPositions_[i];
816  characterPosition.characterName = CORBA::string_dup(body->name().c_str());
817  OpenHRP::LinkPositionSequence& linkPositions = characterPosition.linkPositions;
818  linkPositions.length(numLinks);
819 
820  if(debug_){
821  std::cerr << "character[" << i << "], nlinks = " << numLinks << "\n";
822  }
823  }
824 
825  if(debug_){
826  std::cerr << "_setupCharacterData() - exit" << std::endl;;
827  }
828 }
829 
830 
832 {
833  if(debug_){
834  std::cerr << "PathPlanner::_updateCharacterPositions()\n";
835  }
836 
837  int n = world_->numBodies();
838 
839  {
840 #pragma omp parallel for num_threads(3)
841  for(int i=0; i < n; ++i){
842  BodyPtr body = world_->body(i);
843  int numLinks = body->numLinks();
844 
845  OpenHRP::CharacterPosition& characterPosition = allCharacterPositions_[i];
846 
847  if(debug_){
848  std::cerr << "character[" << i << "], nlinks = " << numLinks << "\n";
849  }
850 
851  for(int j=0; j < numLinks; ++j) {
852  Link* link = body->link(j);
853  OpenHRP::LinkPosition& linkPosition = characterPosition.linkPositions[j];
854  setVector3(link->p, linkPosition.p);
855  setMatrix33ToRowMajorArray(link->segmentAttitude(), linkPosition.R);
856  }
857  }
858  }
859 
860  if(debug_){
861  std::cerr << "_updateCharacterData() - exit" << std::endl;
862  }
863 }
864 
865 
867 {
869 }
870 
872 {
874 }
875 
877 {
878  m_applyConfigFunc = i_func;
879 }
880 
882 {
883  return model_;
884 }
885 
887 {
888  return world_;
889 }
890 
891 void PathPlanner::setPointCloud(const std::vector<Vector3>& i_cloud,
892  double i_radius)
893 {
894  pointCloud_ = i_cloud;
895  radius_ = i_radius;
896 }
void initPlanner(const std::string &nameServer)
初期化。NameServer, ModelLoaderとの接続を行う。
NotFound
Definition: hrpPrep.py:129
bool preparePlanning()
経路計画の準備をし、初期位置と目標位置が有効なものであることをチェックする
Definition: Algorithm.cpp:64
std::vector< hrp::Vector3 > pointCloud_
int c
Definition: autoplay.py:16
bool setMobilityName(const std::string &mobility)
使用する移動能力を設定する
void setApplyConfigFunc(applyConfigFunc i_func)
コンフィギュレーションベクトルからロボットの姿勢をセットする関数をセットする
void(* OptimizerDeleteFunc)(Optimizer *optimizer)
Definition: Optimizer.h:48
TimeMeasure timeCollisionCheck_
干渉チェックに使用したクロック数
std::vector< hrp::ColdetModelPair > checkPairs_
bool setConfigurationToBaseXYTheta(PathPlanner *planner, const Configuration &cfg)
CollisionDetector * customCollisionDetector_
* x
Definition: IceUtils.h:98
virtual bool checkCollision()=0
void getProperties(std::vector< std::string > &names, std::vector< std::string > &values)
プロパティ一覧を取得する
Definition: Algorithm.cpp:37
state
void setPointCloud(const std::vector< hrp::Vector3 > &i_cloud, double i_radius)
干渉チェック対象となるポイントクラウドを設定する
double radius_
radius of spheres assigned to points
void(* MobilityDeleteFunc)(Mobility *mobility)
移動アルゴリズム解放関数
Definition: Mobility.h:106
std::vector< Configuration > path_
経路
static int min(int a, int b)
OptimizerFactory::value_type OptimizerFactoryValueType
bool tryDirectConnection()
初期位置と終了位置を直接結べないか検査する
Definition: Algorithm.cpp:51
png_infop png_charpp name
Definition: png.h:2382
経路最適化アルゴリズム実装用の抽象クラス
Definition: Optimizer.h:17
bool checkCollision()
現在の状態で干渉検出を行う
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
manager
HRPMODEL_API bool loadBodyFromBodyInfo(BodyPtr body, OpenHRP::BodyInfo_ptr bodyInfo, bool loadGeometryForCollisionDetection=false, Link *(*f)()=NULL)
boost::function2< bool, PathPlanner *, const Configuration & > applyConfigFunc
void setVector3(const Vector3 &v3, V &v, size_t top=0)
Definition: Eigen3d.h:130
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
BodyPtr createBoundingBoxBody(BodyPtr body)
png_uint_32 i
Definition: png.h:2735
double dt_
デバッグモード時に使用する現在時刻
AlgorithmFactory algorithmFactory_
経路計画アルゴリズムのファクトリ
bool optimize(const std::string &optimizer)
指定した方法で経路を最適化する
static const bool USE_INTERNAL_COLLISION_DETECTOR
void getOptimizerNames(std::vector< std::string > &optimizers)
経路最適化アルゴリズム名の一覧を取得する
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
WorldPtr world()
物理世界を取得する
OptimizerFactory optimizerFactory_
経路最適化アルゴリズムのファクトリ
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
bool debug_
デバッグモード
OpenHRP::CollisionDetector_var collisionDetector_
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
void registerOptimizer(const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc)
経路最適化アルゴリズムの登録
void setMatrix33ToRowMajorArray(const Matrix33 &m33, Array &a, size_t top=0)
Definition: Eigen3d.h:105
double timeCollisionCheck() const
干渉チェックに使用した時間[s]を取得する
bool getProperties(const std::string &algorithm, std::vector< std::string > &names, std::vector< std::string > &values)
経路計画アルゴリズムのプロパティ名一覧を取得する
std::vector< Configuration > & getWayPoints()
計画された経路を取得する
hrp::BodyPtr model_
経路計画の対象とするロボット
void computeBoundingBox(BodyPtr body, double min[3], double max[3])
Mobility * mobility_
使用する移動能力
void registerMobility(const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc)
移動能力の登録
bool setConfiguration(const Configuration &pos)
コンフィギュレーションをセットする
std::string algorithmName_
使用する経路計画アルゴリズム名
void initSimulation()
動力学シミュレータを初期化する
hrp::BodyPtr registerCharacterByURL(const char *name, const char *url)
キャラクタを動力学シミュレータに登録する。
void setRobotName(const std::string &model)
移動動作の設計対象にするモデルを設定する
void registerAlgorithm(const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc)
経路計画アルゴリズムの登録
Algorithm * algorithm_
使用する経路計画アルゴリズム
virtual void updatePositions()=0
void setCharacterPosition(const char *character, const OpenHRP::DblSequence &pos)
位置を設定する
virtual std::vector< Configuration > optimize(const std::vector< Configuration > &path)=0
経路を最適化する
Mobility *(* MobilityNewFunc)(PathPlanner *planner)
移動アルゴリズム生成関数
Definition: Mobility.h:101
virtual const std::pair< std::string, std::string > & collidingPair()=0
virtual bool getPath(Configuration &from, Configuration &to, std::vector< Configuration > &o_path) const
開始位置から目標地点への移動を補間して生成された姿勢列を取得する。
Definition: Mobility.cpp:26
void getMobilityNames(std::vector< std::string > &mobilitys)
移動能力名の一覧を取得する
MobilityFactory mobilityFactory_
移動能力のファクトリ
MobilityFactory::value_type MobilityFactoryValueType
hrp::BodyPtr robot()
ロボットを取得する
OpenHRP::OnlineViewer_var onlineViewer_
デバッグモード時に使用する OnlineViewer への参照
AlgorithmFactory::value_type AlgorithmFactoryValueType
std::vector< Link * >::const_iterator begin() const
Definition: LinkTraverse.h:64
PathPlanner(unsigned int dim, WorldPtr world=WorldPtr(), bool isDebugMode=false)
コンストラクタ
Optimizer *(* OptimizerNewFunc)(PathPlanner *planner)
Definition: Optimizer.h:43
boost::shared_ptr< hrp::World< hrp::ConstraintForceSolver > > WorldPtr
const std::vector< Configuration > & getPath()
結果を取得する
Definition: Algorithm.h:153
経路計画アルゴリズム基底クラス
Definition: Algorithm.h:23
hrp::BodyPtr registerCharacter(const char *name, OpenHRP::BodyInfo_ptr cInfo)
キャラクタを動力学シミュレータに登録する。
const double value(unsigned int i_rank) const
void getWorldState(OpenHRP::WorldState_out wstate)
物理世界の状況を取得する
bool calcPath()
経路計画を行う
void getAlgorithmNames(std::vector< std::string > &algorithms)
経路計画アルゴリズム名の一覧を取得する
std::string mobilityName_
使用する移動能力名
Algorithm *(* AlgorithmNewFunc)(PathPlanner *planner)
Definition: Algorithm.h:187
std::pair< std::string, std::string > collidingPair_
Definition: Body.h:44
std::vector< Configuration > getPath()
計画された経路の補間されたものを取得する
void registerIntersectionCheckPair(const char *char1, const char *name1, const char *char2, const char *name2, CORBA::Double tolerance)
衝突検出ペアを設定する
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
* y
Definition: IceUtils.h:97
double totalTime() const
std::vector< Link * >::const_iterator end() const
Definition: LinkTraverse.h:68
void setAlgorithmName(const std::string &algorithm)
使用する経路計画アルゴリズム名を指定する
X_ptr checkCorbaServer(const std::string &n, CosNaming::NamingContext_var &cxt)
CORBAサーバ取得
void(* AlgorithmDeleteFunc)(Algorithm *algorithm)
Definition: Algorithm.h:192
static int max(int a, int b)
OpenHRP::ModelLoader_var modelLoader_
ModelLoader への参照
unsigned int numLinks() const
Definition: LinkTraverse.h:40
OpenHRP::CharacterPositionSequence_var allCharacterPositions_
virtual bool calcPath()=0
経路計画を実行する


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