scheduler.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 8; -*-
00002 #include <hrpUtil/OnlineViewerUtil.h>
00003 #include <hrpModel/ModelLoaderUtil.h>
00004 #include <hrpCorba/DynamicsSimulator.hh>
00005 #include <hrpCorba/Controller.hh>
00006 #include <hrpUtil/Eigen3d.h>
00007 #include <fstream>
00008 
00009 using namespace std;
00010 using namespace hrp;
00011 using namespace OpenHRP;
00012 
00013 template <typename X, typename X_ptr>
00014 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
00015 {
00016         CosNaming::Name ncName;
00017         ncName.length(1);
00018         ncName[0].id = CORBA::string_dup(n.c_str());
00019         ncName[0].kind = CORBA::string_dup("");
00020         X_ptr srv = NULL;
00021         try {
00022                 srv = X::_narrow(cxt->resolve(ncName));
00023         } catch(const CosNaming::NamingContext::NotFound &exc) {
00024                 std::cerr << n << " not found: ";
00025                 switch(exc.why) {
00026                 case CosNaming::NamingContext::missing_node:
00027                         std::cerr << "Missing Node" << std::endl;
00028                 case CosNaming::NamingContext::not_context:
00029                         std::cerr << "Not Context" << std::endl;
00030                         break;
00031                 case CosNaming::NamingContext::not_object:
00032                         std::cerr << "Not Object" << std::endl;
00033                         break;
00034                 }
00035                 return (X_ptr)NULL;
00036         } catch(CosNaming::NamingContext::CannotProceed &exc) {
00037                 std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00038         } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00039                 std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00040         }
00041         return srv;
00042 }
00043 
00044 int main(int argc, char* argv[]) 
00045 {
00046         double timeStep = 0.001;  // (s)
00047     double controlTimeStep = 0.002;
00048         double EndTime = 13.0;
00049 
00050         string Model[2], ModelFname[2];
00051         double world_gravity = 9.8;  // default gravity acceleration [m/s^2]
00052     double statFric,slipFric;
00053         statFric = slipFric = 0.5;   // static/slip friction coefficient 
00054     double culling_thresh = 0.01;
00055 
00056         for (int i=1; i<argc; i++){
00057         if (strcmp("-ORBconfig", argv[i])==0 || strcmp("-ORBInitRef", argv[i])==0 ){
00058             argv[++i];  // skip ORB parameter
00059         }else if (strcmp("-url", argv[i])==0){
00060             for(int j = 0; j < 2; j++){
00061                 ModelFname[j] = argv[++i];
00062             }
00063                 }else if (strcmp("-timeStep", argv[i])==0){
00064                         timeStep = atof(argv[++i]);
00065         }
00066         }
00067 
00068     for(int j = 0; j < 2; j++){
00069         Model[j] = "file://" + ModelFname[j];
00070         cout << "Model: " << Model[j] << endl;
00071     }
00072 
00073     //================== Model Load ===============================
00074     BodyInfo_var floor = loadBodyInfo(Model[0].c_str(), argc, argv);
00075     if(!floor){
00076         cerr << "ModelLoader: " << Model[0] << " cannot be loaded" << endl;
00077         return 1;
00078     }
00079     BodyInfo_var body = loadBodyInfo(Model[1].c_str(), argc, argv);
00080     if(!body){
00081         cerr << "ModelLoader: " << Model[1] << " cannot be loaded" << endl;
00082         return 1;
00083     }
00084     
00085     //================== CORBA init ===============================
00086         // initialize CORBA 
00087         CORBA::ORB_var orb;
00088         orb = CORBA::ORB_init(argc, argv);
00089 
00090     // ROOT POA
00091         CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
00092         PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
00093                 
00094         // get reference to POA manager
00095         PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
00096 
00097     CosNaming::NamingContext_var cxt;
00098         {
00099                 CORBA::Object_var       nS = orb->resolve_initial_references("NameService");
00100         cxt = CosNaming::NamingContext::_narrow(nS);
00101         }
00102 
00103     //==================== OnlineViewer (GrxUI) setup ===============
00104     OnlineViewer_var olv = getOnlineViewer(argc, argv);
00105                 
00106     if (CORBA::is_nil( olv )) {
00107                 std::cerr << "OnlineViewer not found" << std::endl;
00108                 return 1;
00109     }
00110     try {
00111         olv->load(floor->name(), Model[0].c_str());
00112         olv->load(body->name(), Model[1].c_str());
00113         olv->clearLog();
00114     } catch (CORBA::SystemException& ex) {
00115         cerr << "Failed to connect GrxUI." << endl;
00116         return 1;
00117     }
00118 
00119         //================= DynamicsSimulator setup ======================
00120         DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
00121         dynamicsSimulatorFactory =
00122                 checkCorbaServer <DynamicsSimulatorFactory, DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxt);
00123                 
00124         if (CORBA::is_nil(dynamicsSimulatorFactory)) {
00125                 std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
00126         }
00127         DynamicsSimulator_var dynamicsSimulator = dynamicsSimulatorFactory->create();
00128 
00129         cout << "** Dynamics server setup ** " << endl;
00130     cout << "Character  :" << floor->name() << endl;
00131         dynamicsSimulator->registerCharacter(floor->name(), floor);                         
00132         cout << "Character  :" << body->name() << endl;
00133     dynamicsSimulator->registerCharacter(body->name(), body);
00134 
00135     dynamicsSimulator->init(timeStep, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);
00136     DblSequence3 g;
00137     g.length(3);
00138     g[0] = 0.0;
00139     g[1] = 0.0;
00140     g[2] = world_gravity;
00141     dynamicsSimulator->setGVector(g);
00142 
00143     // initial position and orientation
00144     Vector3  waist_p;
00145     Matrix33 waist_R;
00146     waist_p << 0, 0, 0.7135;
00147         waist_R = Matrix33::Identity();
00148 
00149         DblSequence trans;
00150         trans.length(12);
00151         for(int i=0; i<3; i++) trans[i]   = waist_p(i);
00152         for(int i=0; i<3; i++){
00153                 for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
00154         }
00155         dynamicsSimulator->setCharacterLinkData( body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, trans );
00156     DblSequence angle;
00157     angle.length(29);
00158     angle[0] = 0.0;         angle[1] = -0.0360373;  angle[2] = 0.0;         angle[3] = 0.0785047;
00159     angle[4] = -0.0424675;  angle[5] = 0.0;         angle[6] = 0.174533;    angle[7] = -0.00349066;
00160     angle[8] = 0.0;         angle[9] = -1.5708;     angle[10] = 0.0;        angle[11] = 0.0;
00161     angle[12] = 0.0;        angle[13] = 0.0;        angle[14] = -0.0360373; angle[15] = 0.0;
00162     angle[16] = 0.0785047;  angle[17] = -0.0424675; angle[18] = 0.0;        angle[19] = 0.174533;
00163     angle[20] = -0.00349066;angle[21] = 0.0;        angle[22] = -1.5708;    angle[23] = 0.0;
00164     angle[24] = 0.0;        angle[25] = 0.0;        angle[26] = 0.0;        angle[27] = 0.0;
00165     angle[28] = 0.0;
00166     dynamicsSimulator->setCharacterAllLinkData( body->name(), DynamicsSimulator::JOINT_VALUE, angle );
00167     dynamicsSimulator->calcWorldForwardKinematics();
00168 
00169     DblSequence6 K, C;    // spring-damper parameters are not used now
00170     K.length(0);
00171     C.length(0);
00172     dynamicsSimulator->registerCollisionCheckPair(floor->name(),"", body->name() ,"",
00173                                                             statFric,slipFric,K,C,culling_thresh,0.0);
00174     dynamicsSimulator->initSimulation();
00175         
00176     // ==================  Controller setup ==========================
00177     Controller_var controller;
00178     controller = checkCorbaServer <Controller, Controller_var> ("SamplePDController", cxt);
00179 
00180     if (CORBA::is_nil(controller)) {
00181         std::cerr << "Controller not found" << std::endl;
00182     }
00183 
00184     controller->setModelName(body->name());
00185     controller->setDynamicsSimulator(dynamicsSimulator);
00186     controller->initialize();
00187     controller->setTimeStep(controlTimeStep);
00188     controller->start();
00189 
00190     // ==================  log file   ======================
00191         static ofstream log_file;
00192         log_file.open("samplePD.log");
00193                 
00194         // ==================  main loop   ======================
00195     WorldState_var state;
00196         int i=0;
00197     int j = 0;
00198         double time=0.0;
00199     double controlTime=0.0;
00200         while ( 1 ) {
00201         bool control=false;
00202         if(controlTime <= time){
00203             control=true;
00204             j++;
00205         }
00206 
00207         if(control)
00208             controller->input();
00209 
00210         i++;
00211         time = timeStep * i;
00212                 controlTime = controlTimeStep * j;
00213 
00214         if(control)
00215             controller->control();
00216                
00217         // ================== simulate one step ==============
00218                 dynamicsSimulator->stepSimulation();                                        
00219                
00220         // ================== viewer update ====================
00221         try {
00222                     dynamicsSimulator -> getWorldState( state );
00223                         olv->update( state );
00224                 } catch (CORBA::SystemException& ex) {
00225                     return 1;
00226         }
00227         
00228         // ===================== get robot status ===================
00229                 DblSequence_var waist_pR;  // position and attitude
00230                 DblSequence_var waist_vw;  // linear and angular velocities
00231                 dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, waist_pR);
00232                 dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_VELOCITY,  waist_vw);
00233 
00234                 // ================== log data save =====================
00235                 log_file << time << " ";
00236                 log_file << waist_vw[2] << " ";
00237                 log_file << endl;
00238 
00239         if(control)
00240             controller->output();
00241 
00242                 if( time > EndTime ) break;
00243 
00244         }
00245 
00246         controller->stop();
00247         log_file.close();
00248         dynamicsSimulator->destroy();
00249 
00250         return 0;
00251 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56