clap.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 8; -*-
00002 
00003 #include <stdio.h>
00004 #include <string>
00005 #include <hrpUtil/OnlineViewerUtil.h>
00006 #include <hrpModel/ModelLoaderUtil.h>
00007 #include <hrpCorba/DynamicsSimulator.hh>
00008 
00009 using namespace std;
00010 using namespace hrp;
00011 using namespace OpenHRP;
00012 
00013 enum {X, Y, Z};
00014 #define deg2rad(x) ( 3.14159265358979 / 180*(x) )
00015 #define DOF 29    // 初期姿勢配列の長さ
00016 
00017 // サーバ群の取得
00018 template <typename X, typename X_ptr> 
00019 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
00020 {
00021     CosNaming::Name ncName;
00022     ncName.length(1);
00023     ncName[0].id = CORBA::string_dup(n.c_str());
00024     ncName[0].kind = CORBA::string_dup("");
00025     X_ptr srv = NULL;
00026     try 
00027     {
00028         srv = X::_narrow(cxt->resolve(ncName));
00029     } 
00030     catch(const CosNaming::NamingContext::NotFound &exc) 
00031     {
00032         std::cerr << n << " not found: ";
00033         switch(exc.why) 
00034         {
00035             case CosNaming::NamingContext::missing_node:
00036                 std::cerr << "Missing Node" << std::endl;
00037                 break;
00038             case CosNaming::NamingContext::not_context:
00039                 std::cerr << "Not Context" << std::endl;
00040                 break;
00041             case CosNaming::NamingContext::not_object:
00042                 std::cerr << "Not Object" << std::endl;
00043                 break;
00044         }
00045         return (X_ptr)NULL;
00046     } 
00047     catch(CosNaming::NamingContext::CannotProceed &exc) 
00048     {
00049         std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00050     } 
00051     catch(CosNaming::NamingContext::AlreadyBound &exc) 
00052     {
00053         std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00054     }
00055     return srv;
00056 }
00057 
00058 
00059 int main(int argc, char* argv[])
00060 {
00061     int i;
00062     string url = "file://";
00063     double ddp = 0.001;
00064     double timeK = 10.0;
00065 
00066     // -urlでモデルのURLを指定  
00067     for(i=0; i < argc - 1; i++)
00068     {
00069         if( strcmp(argv[i], "-url") == 0)
00070         {
00071             url += argv[++i];
00072         } else if( strcmp(argv[i], "-ddp") == 0)
00073         {
00074             ddp = strtod( argv[++i], NULL);
00075         } else if( strcmp(argv[i], "-timeK") == 0)
00076         {
00077             timeK = strtod( argv[++i], NULL);
00078         }
00079     }
00080 
00081     string robot_url = url+"sample.wrl";
00082     string floor_url = url+"floor.wrl";
00083 
00084     const char *ROBOT_URL = robot_url.c_str();
00085     const char *FLOOR_URL = floor_url.c_str();
00086 
00088 
00089     // CORBA初期化
00090     CORBA::ORB_var orb;
00091     orb = CORBA::ORB_init(argc, argv);
00092 
00093     // ROOT POA
00094     CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
00095     PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
00096 
00097     // POAマネージャへの参照を取得
00098     PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
00099     
00100     // NamingServiceの参照取得
00101     CosNaming::NamingContext_var cxT;
00102     {
00103       CORBA::Object_var    nS = orb->resolve_initial_references("NameService");
00104       cxT = CosNaming::NamingContext::_narrow(nS);
00105     }
00106 
00108 
00109     // DynamicsSimulatorの取得
00110     DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
00111     dynamicsSimulatorFactory = 
00112         checkCorbaServer <DynamicsSimulatorFactory,
00113         DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxT);
00114 
00115     if (CORBA::is_nil(dynamicsSimulatorFactory)) 
00116     {
00117         std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
00118     }
00119 
00120     DynamicsSimulator_var dynamicsSimulator 
00121         = dynamicsSimulatorFactory->create();
00122 
00123     // モデルの読み込み・登録
00124     BodyInfo_var robotInfo = loadBodyInfo(ROBOT_URL, argc, argv);
00125     dynamicsSimulator->registerCharacter("robot", robotInfo);
00126 
00127     // 床の読み込み・登録
00128     BodyInfo_var floorInfo = loadBodyInfo(FLOOR_URL, argc, argv);
00129     dynamicsSimulator->registerCharacter("floor", floorInfo);
00130 
00131 
00133 
00134     // DynamicsSimulatorの初期化
00135     dynamicsSimulator->init(0.002, 
00136         DynamicsSimulator::RUNGE_KUTTA, 
00137         DynamicsSimulator::ENABLE_SENSOR);
00138 
00139     // 重力ベクトルの設定
00140     DblSequence3 gVector;
00141     gVector.length(3);
00142     gVector[0] = gVector[1] = 0;
00143     gVector[2] = 9.8;
00144     dynamicsSimulator->setGVector(gVector);
00145     
00146     // 関節駆動モードの設定
00147     dynamicsSimulator->setCharacterAllJointModes(
00148         "robot", DynamicsSimulator::TORQUE_MODE);
00149 
00150     // 初期姿勢
00151     double init_pos[] = {0.00E+00, -3.60E-02, 0,  7.85E-02, -4.25E-02,  0.00E+00,
00152                          1.75E-01, -3.49E-03, 0, -1.57E+00,  0.00E+00,  0.00E+00,
00153                          0.00E+00,  0.00E+00, -3.60E-02, 0,  7.85E-02, -4.25E-02,
00154                          0.00E+00,  1.75E-01,  3.49E-03, 0, -1.57E+00,  0.00E+00,
00155                          0.00E+00,  0.00E+00, 0, 0, 0};
00156 
00157     // 初期姿勢を関節角にセット
00158     DblSequence q;
00159     q.length(DOF);
00160     for (int i=0; i<DOF; i++) 
00161     {
00162         q[i] = init_pos[i];
00163     }
00164     dynamicsSimulator->setCharacterAllLinkData(
00165         "robot", DynamicsSimulator::JOINT_VALUE, q);
00166 
00167     // 順運動学計算
00168     dynamicsSimulator->calcWorldForwardKinematics();
00169 
00170     // 干渉チェックペアの設定−両手 
00171     DblSequence6 dc, sc;
00172     dc.length(0);
00173     sc.length(0);
00174 
00175     dynamicsSimulator->registerCollisionCheckPair(
00176         "robot",
00177         "RARM_WRIST_R",
00178         "robot",
00179         "LARM_WRIST_R",
00180         0.5,
00181         0.5,
00182         dc,
00183         sc,
00184         0.0,
00185         0.0);
00186 
00187     dynamicsSimulator->initSimulation();
00188 
00190 
00191     // OnlineViewerの取得
00192     OnlineViewer_var onlineViewer = getOnlineViewer(argc, argv);
00193     // OnlineViewerに対するモデルロード
00194     try 
00195     {
00196         onlineViewer->load("robot", ROBOT_URL);
00197         onlineViewer->load("floor", FLOOR_URL);
00198         onlineViewer->setLogName("clap");
00199         onlineViewer->clearLog();
00200     } 
00201     catch (CORBA::SystemException& ex) 
00202     {
00203         std::cerr << "Failed to connect GrxUI." << endl;
00204         return 1;
00205     }
00207 
00208     // 逆運動学計算の準備 
00209     double RARM_p[] = {0.197403, -0.210919, 0.93732};
00210     double RARM_R[] = {0.174891, -0.000607636, -0.984588,
00211                        0.00348999, 0.999994, 2.77917e-06,
00212                        0.984582, -0.00343669, 0.174892};
00213 
00214     double LARM_p[] = {0.197403, 0.210919, 0.93732};
00215     double LARM_R[] = {0.174891, 0.000607636, -0.984588,
00216                        -0.00348999, 0.999994, -2.77917e-06,
00217                        0.984582, 0.00343669, 0.174892};
00218     double dp = 0.0;
00219 
00221 
00222     WorldState_var state;        
00223     while (1) 
00224     {
00225         // 逆運動学計算 
00226         LinkPosition link;
00227         link.p[0] = RARM_p[0];
00228         link.p[1] = RARM_p[1] + dp;
00229         link.p[2] = RARM_p[2];
00230         for (int i=0; i<9; i++) 
00231           link.R[i] = RARM_R[i];
00232         dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
00233                                   CORBA::string_dup("CHEST"),
00234                                   CORBA::string_dup("RARM_WRIST_R"),
00235                                   link);
00236 
00237         link.p[0] = LARM_p[0];
00238         link.p[1] = LARM_p[1] -  dp;
00239         link.p[2] = LARM_p[2];
00240         for (int i=0; i<9; i++) 
00241           link.R[i] = LARM_R[i];
00242         dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
00243                                   CORBA::string_dup("CHEST"),
00244                                   CORBA::string_dup("LARM_WRIST_R"),
00245                                   link);                            
00246 
00247         dynamicsSimulator->calcWorldForwardKinematics();
00248         dp += ddp;
00249         // OnlineViewer更新 
00250         try 
00251         {
00252             dynamicsSimulator->getWorldState(state);
00253             state->time = dp*timeK;
00254             onlineViewer->update(state);
00255         }
00256         catch (CORBA::SystemException& ex) 
00257         {
00258             std::cerr << "OnlineViewer could not be updated." << endl;
00259             std::cerr << "ex._rep_id() = " << ex._rep_id() << endl;
00260             return 1;
00261         }
00262 
00263         if(state->time >= 200.0){
00264             std::cout << state->time << endl;
00265         }
00266 
00267         // 干渉チェック
00268         dynamicsSimulator->checkCollision(true);
00269 
00270         if (state->collisions.length() > 0) 
00271         {            
00272             if (state->collisions[0].points.length() > 0) 
00273             {
00274                 break;
00275             }
00276         }
00277 
00278     }
00279 
00280     return 0;
00281 
00282 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:15