2 #include <boost/bind.hpp>
16 #include <hrpCorba/OpenHRPCommon.hh>
30 template<
typename X,
typename X_ptr>
33 CosNaming::Name ncName;
35 ncName[0].id = CORBA::string_dup(
n.c_str());
36 ncName[0].kind = CORBA::string_dup(
"");
39 srv = X::_narrow(cxt->resolve(ncName));
41 std::cerr <<
n <<
" not found: ";
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;
48 case CosNaming::NamingContext::not_object:
49 std::cerr <<
"Not Object" << std::endl;
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;
63 Link *baseLink = planner->
robot()->rootLink();
65 baseLink->
p(0) = cfg.
value(0);
66 baseLink->
p(1) = cfg.
value(1);
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;
74 planner->
robot()->calcForwardKinematics();
85 : m_applyConfigFunc(
setConfigurationToBaseXYTheta), algorithm_(NULL), mobility_(NULL), cspace_(dim), debug_(isDebugMode), world_(world), customCollisionDetector_(NULL)
88 std::cerr <<
"PathPlanner::PathPlanner() : debug mode" << std::endl;
101 registerMobility(
"OmniWheel", MobilityCreate<OmniWheel>, MobilityDelete<OmniWheel>);
106 OptimizerCreate<ShortcutOptimizer>,
107 OptimizerDelete<ShortcutOptimizer>);
109 OptimizerCreate<RandomShortcutOptimizer>,
110 OptimizerDelete<RandomShortcutOptimizer>);
125 std::cerr <<
"PathPlanner::~PathPlanner()" << std::endl;
140 optimizers.push_back((*it).first);
149 mobilities.push_back((*it).first);
161 algorithms.push_back((*it).first);
170 std::vector<std::string> &names,
171 std::vector<std::string> &values)
174 std::cerr <<
"PathPlanner::getPropertyNames(" << algorithm <<
")" << std::endl;
182 std::cerr <<
"algorithm not found" << std::endl;
194 std::cerr <<
"PathPlanner::initPlanner(" << nameServer <<
")" << std::endl;
201 orb_ = CORBA::ORB_init(ac, av);
205 CORBA::Object_var poaObj =
orb_ -> resolve_initial_references(
"RootPOA");
206 PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
211 PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
213 std::ostringstream stream;
214 stream <<
"corbaloc:iiop:" << nameServer <<
"/NameService";
216 CosNaming::NamingContext_var cxT;
217 CORBA::Object_var nS =
orb_->string_to_object(stream.str().c_str());
218 cxT = CosNaming::NamingContext::_narrow(nS);
221 std::cerr <<
"name serivce not found" << std::endl;
223 std::cerr <<
"NameService OK." << std::endl;
227 std::cerr <<
"CollisonDetectorFactory ";
228 OpenHRP::CollisionDetectorFactory_var cdFactory =
230 OpenHRP::CollisionDetectorFactory_var> (
"CollisionDetectorFactory", cxT);
231 if (CORBA::is_nil(cdFactory)) {
232 std::cerr <<
"not found" << std::endl;
234 std::cerr <<
"OK." << std::endl;
239 std::cerr <<
"failed to create CollisionDetector" << std::endl;
243 std::cerr <<
"ModelLoader ";
246 OpenHRP::ModelLoader_var> (
"ModelLoader", cxT);
249 std::cerr <<
"not found" << std::endl;
251 std::cerr <<
"OK." << std::endl;
257 OpenHRP::OnlineViewer_var> (
"OnlineViewer", cxT);
260 std::cerr <<
"OnlineViewer not found" << std::endl;
264 }
catch (CORBA::SystemException& ex) {
265 std::cerr << ex._rep_id() << std::endl;
284 std::cerr <<
"algorithm(" << algorithmName <<
") not found"
294 std::cerr <<
"PathPlanner::registerCharacterByURL(" <<
name <<
", " << url <<
")" << std::endl;
298 std::cerr <<
"nil reference to servers" << std::endl;
302 OpenHRP::BodyInfo_ptr cInfo =
modelLoader_->getBodyInfo(url);
307 std::cerr <<
"nil reference to OnlineViewer" << std::endl;
318 const OpenHRP::DblSequence& pos)
321 std::cerr <<
"PathPlanner::setCharacterPosition(" << character <<
", "
322 << pos[0] <<
", " << pos[1] <<
", " << pos[2] <<
")"
326 if (!body) std::cerr <<
"PathPlanner::setCharacterPosition() : character("
327 << character <<
") not found" << std::endl;
328 Link* l = body->rootLink();
339 bool firsttime =
true;
341 Link *l, *root=body->rootLink();
344 for (
unsigned int i=0;
i<body->numLinks();
i++){
346 for (
int j=0; j<l->
coldetModel->getNumVertices(); j++){
348 v[0] = x; v[1] = y; v[2] = z;
349 p = Rt*(l->
R*v+l->
p-root->
p);
351 for (
int k=0; k<3; k++)
min[k] =
max[k] = p[k];
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];
361 std::cerr <<
"bounding box of " << body->name() <<
": ("
362 <<
min[0] <<
", " <<
min[1] <<
", " <<
min[2] <<
") - ("
363 <<
max[0] <<
", " <<
max[1] <<
", " <<
max[2] <<
")"
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();
398 root->
R = Matrix33::Identity();
399 root->
Rs = Matrix33::Identity();
403 bboxBody->setRootLink(root);
413 std::cerr <<
"PathPlanner::registerCharacter(" <<
name <<
", " << cInfo <<
")" << std::endl;
439 i_body->setName(
name);
452 std::cerr <<
"PathPlanner::setRobotName(" <<
name <<
")" << std::endl;
457 std::cerr <<
"PathPlanner::setRobotName() : robot(" <<
name <<
") not found" << std::endl;
468 std::cerr <<
"PathPlanner::setMobilityName(" << mobility <<
")" << std::endl;
479 std::cerr <<
"PathPlanner::setMobilityName() : mobility(" << mobility <<
") not found" << std::endl;
491 const char* linkName1,
492 const char* charName2,
493 const char* linkName2,
494 CORBA::Double tolerance) {
496 std::cerr <<
"PathPlanner::registerIntersectionCheckPair("
497 << charName1 <<
", " << linkName1 <<
", " << charName2
499 <<
", " << tolerance <<
")" << std::endl;
501 int bodyIndex1 =
world_->bodyIndex(charName1);
502 int bodyIndex2 =
world_->bodyIndex(charName2);
504 if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
509 std::string emptyString =
"";
510 std::vector<Link*> links1;
511 if(emptyString == linkName1){
514 std::copy(traverse.
begin(), traverse.
end(), links1.begin());
516 links1.push_back(body1->link(linkName1));
519 std::vector<Link*> links2;
520 if(emptyString == linkName2){
523 std::copy(traverse.
begin(), traverse.
end(), links2.begin());
525 links2.push_back(body2->link(linkName2));
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];
533 if(link1 && link2 && link1 != link2){
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;
556 world_->setTimeStep(0.002);
557 world_->setCurrentTime(0.0);
558 world_->setRungeKuttaMethod();
559 world_->enableSensors(
true);
562 for(
int i=0;
i <
n; ++
i){
563 world_->body(
i)->initializeConfiguration();
569 g[0] = g[1] = 0.0; g[2] = 9.8;
570 world_->setGravityAcceleration(g);
583 std::cerr <<
"checkCollision(" << pos <<
")" << std::endl;
593 OpenHRP::WorldState_var state;
596 static double nowTime = 0;
597 state->time = nowTime;
608 std::cerr <<
"DynamicsSimulator_impl::getWorldState()\n";
613 wstate =
new OpenHRP::WorldState;
615 wstate->time =
world_->currentTime();
619 std::cerr <<
"getWorldState - exit" << std::endl;
645 for (
unsigned int j=0; j<
model_->numLinks(); j++){
673 for (
unsigned int i=0;
i<
model_->numLinks();
i++){
685 OpenHRP::LinkPairSequence_var pairs =
new OpenHRP::LinkPairSequence;
689 return pairs->length() > 0;
707 unsigned int checked = 1;
708 unsigned int div = 2;
710 std::vector<bool> isVisited;
711 for (
unsigned int i=0;
i<path.size()-1;
i++) {
712 isVisited.push_back(
false);
714 isVisited.push_back(
true);
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) {
730 if (checked != path.size()-1) {
731 std::cerr <<
"checkCollision() : there are unchecked configurations."
732 <<
" path.size() = " << path.size() <<
", checked = "
733 << checked << std::endl;
742 for (
unsigned int i=0;
i<
world_->numBodies();
i++){
744 body->calcForwardKinematics();
745 for (
unsigned int j=0; j<body->numLinks(); j++){
750 std::cerr <<
"The number of collision check pairs = " <<
checkPairs_.size() << std::endl;
752 std::cerr <<
"preparePlanning() failed" << std::endl;
758 std::cerr <<
"connected directly" << std::endl;
761 std::cerr <<
"failed direct connection" << std::endl;
789 std::vector<Configuration> finalPath;
790 if (
path_.size() == 0)
return finalPath;
792 for (
unsigned int i=0;
i<
path_.size()-1;
i++) {
793 std::vector<Configuration> localPath;
795 finalPath.insert(finalPath.end(), localPath.begin(), localPath.end()-1);
805 std::cerr <<
"PathPlanner::_setupCharacterData()\n";
811 for(
int i=0;
i <
n; ++
i){
814 int numLinks = body->numLinks();
816 characterPosition.characterName = CORBA::string_dup(body->name().c_str());
817 OpenHRP::LinkPositionSequence& linkPositions = characterPosition.linkPositions;
818 linkPositions.length(numLinks);
821 std::cerr <<
"character[" <<
i <<
"], nlinks = " << numLinks <<
"\n";
826 std::cerr <<
"_setupCharacterData() - exit" << std::endl;;
834 std::cerr <<
"PathPlanner::_updateCharacterPositions()\n";
840 #pragma omp parallel for num_threads(3)
841 for(
int i=0;
i <
n; ++
i){
843 int numLinks = body->numLinks();
848 std::cerr <<
"character[" <<
i <<
"], nlinks = " << numLinks <<
"\n";
851 for(
int j=0; j < numLinks; ++j) {
852 Link* link = body->link(j);
853 OpenHRP::LinkPosition& linkPosition = characterPosition.linkPositions[j];
861 std::cerr <<
"_updateCharacterData() - exit" << std::endl;