main.cpp
Go to the documentation of this file.
00001 #include <hrpModel/ModelLoaderUtil.h>
00002 #include <hrpModel/Link.h>
00003 #include <hrpUtil/OnlineViewerUtil.h>
00004 #include <hrpModel/OnlineViewerUtil.h>
00005 #include <iostream>
00006 #include <fstream>
00007 #include "scc.h"
00008 
00009 using namespace OpenHRP;
00010 using namespace hrp;
00011 
00012 int main(int argc, char *argv[])
00013 {
00014     if (argc < 3){
00015         std::cerr << "Usage: " << argv[0] << "[VRML model] [log file] [-olv] [linkName1:linkNam2 ...]" 
00016                   << std::endl;
00017         return 1;
00018     }
00019 
00020     hrp::LinkNamePairList pairs;
00021     bool useOLV=false;
00022     for (int i=3; i<argc; i++){
00023         std::string str = argv[i];
00024         std::string::size_type pos;
00025         if ((pos = str.find(":")) != std::string::npos){
00026             std::string link1 = str.substr(0, pos);
00027             std::string link2 = str.substr(pos+1);
00028             std::cerr << "[" << link1 << "],[" << link2 << "]" << std::endl;
00029             pairs.push_back(std::make_pair(link1, link2));
00030         }else if(str=="-olv"){
00031             useOLV=true;
00032         }
00033     }
00034 
00035     hrp::BodyPtr robot = hrp::BodyPtr(new hrp::Body());
00036     if (!loadBodyFromModelLoader(robot, argv[1], 
00037                                  argc, argv, true)){
00038         std::cerr << "Error: failed to load model[" << argv[1] << "]" 
00039                   << std::endl;
00040         return 2;
00041     }
00042 
00043     hrp::SelfCollisionChecker scc(robot, pairs);
00044     std::cerr << scc.numOfCheckPairs() << " pairs are defined" << std::endl;
00045 
00046     OpenHRP::OnlineViewer_var olv;
00047     if (useOLV){
00048         olv = getOnlineViewer(argc, argv);
00049         olv->load(robot->name().c_str(), argv[1]);
00050         olv->clearLog();
00051     }
00052     OpenHRP::WorldState wstate;
00053     wstate.characterPositions.length(1);
00054     setupCharacterPosition(wstate.characterPositions[0], robot);
00055     
00056     std::ifstream ifs(argv[2]);
00057     double tm, q[robot->numJoints()];
00058 
00059     ifs >> tm;
00060     while (!ifs.eof()){
00061         for (unsigned int i=0; i<robot->numJoints(); i++){
00062             ifs >> q[i];
00063         }
00064         pairs = scc.check(q);
00065         for (unsigned int i=0; i<pairs.size(); i++){
00066             std::cout << tm << " " << pairs[i].first << ":" << pairs[i].second
00067                       << std::endl;
00068         }
00069         if (useOLV){
00070             wstate.time = tm;
00071             updateCharacterPosition(wstate.characterPositions[0], robot);
00072             olv->update(wstate);
00073         }
00074         ifs >> tm;
00075     }
00076 
00077     return 0;
00078 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55