scheduler.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 8; -*-
2 #include <hrpUtil/OnlineViewerUtil.h>
3 #include <hrpModel/ModelLoaderUtil.h>
4 #include <hrpCorba/DynamicsSimulator.hh>
5 #include <hrpCorba/Controller.hh>
6 #include <hrpUtil/Eigen3d.h>
7 #include <fstream>
8 
9 using namespace std;
10 using namespace hrp;
11 using namespace OpenHRP;
12 
13 template <typename X, typename X_ptr>
14 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
15 {
16  CosNaming::Name ncName;
17  ncName.length(1);
18  ncName[0].id = CORBA::string_dup(n.c_str());
19  ncName[0].kind = CORBA::string_dup("");
20  X_ptr srv = NULL;
21  try {
22  srv = X::_narrow(cxt->resolve(ncName));
23  } catch(const CosNaming::NamingContext::NotFound &exc) {
24  std::cerr << n << " not found: ";
25  switch(exc.why) {
26  case CosNaming::NamingContext::missing_node:
27  std::cerr << "Missing Node" << std::endl;
28  case CosNaming::NamingContext::not_context:
29  std::cerr << "Not Context" << std::endl;
30  break;
31  case CosNaming::NamingContext::not_object:
32  std::cerr << "Not Object" << std::endl;
33  break;
34  }
35  return (X_ptr)NULL;
36  } catch(CosNaming::NamingContext::CannotProceed &exc) {
37  std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
38  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
39  std::cerr << "Resolve " << n << " InvalidName" << std::endl;
40  }
41  return srv;
42 }
43 
44 int main(int argc, char* argv[])
45 {
46  double timeStep = 0.001; // (s)
47  double controlTimeStep = 0.002;
48  double EndTime = 13.0;
49 
50  string Model[2], ModelFname[2];
51  double world_gravity = 9.8; // default gravity acceleration [m/s^2]
52  double statFric,slipFric;
53  statFric = slipFric = 0.5; // static/slip friction coefficient
54  double culling_thresh = 0.01;
55 
56  for (int i=1; i<argc; i++){
57  if (strcmp("-ORBconfig", argv[i])==0 || strcmp("-ORBInitRef", argv[i])==0 ){
58  argv[++i]; // skip ORB parameter
59  }else if (strcmp("-url", argv[i])==0){
60  for(int j = 0; j < 2; j++){
61  ModelFname[j] = argv[++i];
62  }
63  }else if (strcmp("-timeStep", argv[i])==0){
64  timeStep = atof(argv[++i]);
65  }
66  }
67 
68  for(int j = 0; j < 2; j++){
69  Model[j] = "file://" + ModelFname[j];
70  cout << "Model: " << Model[j] << endl;
71  }
72 
73  //================== Model Load ===============================
74  BodyInfo_var floor = loadBodyInfo(Model[0].c_str(), argc, argv);
75  if(!floor){
76  cerr << "ModelLoader: " << Model[0] << " cannot be loaded" << endl;
77  return 1;
78  }
79  BodyInfo_var body = loadBodyInfo(Model[1].c_str(), argc, argv);
80  if(!body){
81  cerr << "ModelLoader: " << Model[1] << " cannot be loaded" << endl;
82  return 1;
83  }
84 
85  //================== CORBA init ===============================
86  // initialize CORBA
87  CORBA::ORB_var orb;
88  orb = CORBA::ORB_init(argc, argv);
89 
90  // ROOT POA
91  CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
92  PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
93 
94  // get reference to POA manager
95  PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
96 
97  CosNaming::NamingContext_var cxt;
98  {
99  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
100  cxt = CosNaming::NamingContext::_narrow(nS);
101  }
102 
103  //==================== OnlineViewer (GrxUI) setup ===============
104  OnlineViewer_var olv = getOnlineViewer(argc, argv);
105 
106  if (CORBA::is_nil( olv )) {
107  std::cerr << "OnlineViewer not found" << std::endl;
108  return 1;
109  }
110  try {
111  olv->load(floor->name(), Model[0].c_str());
112  olv->load(body->name(), Model[1].c_str());
113  olv->clearLog();
114  } catch (CORBA::SystemException& ex) {
115  cerr << "Failed to connect GrxUI." << endl;
116  return 1;
117  }
118 
119  //================= DynamicsSimulator setup ======================
120  DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
121  dynamicsSimulatorFactory =
122  checkCorbaServer <DynamicsSimulatorFactory, DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxt);
123 
124  if (CORBA::is_nil(dynamicsSimulatorFactory)) {
125  std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
126  }
127  DynamicsSimulator_var dynamicsSimulator = dynamicsSimulatorFactory->create();
128 
129  cout << "** Dynamics server setup ** " << endl;
130  cout << "Character :" << floor->name() << endl;
131  dynamicsSimulator->registerCharacter(floor->name(), floor);
132  cout << "Character :" << body->name() << endl;
133  dynamicsSimulator->registerCharacter(body->name(), body);
134 
135  dynamicsSimulator->init(timeStep, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);
136  DblSequence3 g;
137  g.length(3);
138  g[0] = 0.0;
139  g[1] = 0.0;
140  g[2] = world_gravity;
141  dynamicsSimulator->setGVector(g);
142 
143  // initial position and orientation
144  Vector3 waist_p;
145  Matrix33 waist_R;
146  waist_p << 0, 0, 0.7135;
147  waist_R = Matrix33::Identity();
148 
149  DblSequence trans;
150  trans.length(12);
151  for(int i=0; i<3; i++) trans[i] = waist_p(i);
152  for(int i=0; i<3; i++){
153  for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
154  }
155  dynamicsSimulator->setCharacterLinkData( body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, trans );
156  DblSequence angle;
157  angle.length(29);
158  angle[0] = 0.0; angle[1] = -0.0360373; angle[2] = 0.0; angle[3] = 0.0785047;
159  angle[4] = -0.0424675; angle[5] = 0.0; angle[6] = 0.174533; angle[7] = -0.00349066;
160  angle[8] = 0.0; angle[9] = -1.5708; angle[10] = 0.0; angle[11] = 0.0;
161  angle[12] = 0.0; angle[13] = 0.0; angle[14] = -0.0360373; angle[15] = 0.0;
162  angle[16] = 0.0785047; angle[17] = -0.0424675; angle[18] = 0.0; angle[19] = 0.174533;
163  angle[20] = -0.00349066;angle[21] = 0.0; angle[22] = -1.5708; angle[23] = 0.0;
164  angle[24] = 0.0; angle[25] = 0.0; angle[26] = 0.0; angle[27] = 0.0;
165  angle[28] = 0.0;
166  dynamicsSimulator->setCharacterAllLinkData( body->name(), DynamicsSimulator::JOINT_VALUE, angle );
167  dynamicsSimulator->calcWorldForwardKinematics();
168 
169  DblSequence6 K, C; // spring-damper parameters are not used now
170  K.length(0);
171  C.length(0);
172  dynamicsSimulator->registerCollisionCheckPair(floor->name(),"", body->name() ,"",
173  statFric,slipFric,K,C,culling_thresh,0.0);
174  dynamicsSimulator->initSimulation();
175 
176  // ================== Controller setup ==========================
177  Controller_var controller;
178  controller = checkCorbaServer <Controller, Controller_var> ("SamplePDController", cxt);
179 
180  if (CORBA::is_nil(controller)) {
181  std::cerr << "Controller not found" << std::endl;
182  }
183 
184  controller->setModelName(body->name());
185  controller->setDynamicsSimulator(dynamicsSimulator);
186  controller->initialize();
187  controller->setTimeStep(controlTimeStep);
188  controller->start();
189 
190  // ================== log file ======================
191  static ofstream log_file;
192  log_file.open("samplePD.log");
193 
194  // ================== main loop ======================
195  WorldState_var state;
196  int i=0;
197  int j = 0;
198  double time=0.0;
199  double controlTime=0.0;
200  while ( 1 ) {
201  bool control=false;
202  if(controlTime <= time){
203  control=true;
204  j++;
205  }
206 
207  if(control)
208  controller->input();
209 
210  i++;
211  time = timeStep * i;
212  controlTime = controlTimeStep * j;
213 
214  if(control)
215  controller->control();
216 
217  // ================== simulate one step ==============
218  dynamicsSimulator->stepSimulation();
219 
220  // ================== viewer update ====================
221  try {
222  dynamicsSimulator -> getWorldState( state );
223  olv->update( state );
224  } catch (CORBA::SystemException& ex) {
225  return 1;
226  }
227 
228  // ===================== get robot status ===================
229  DblSequence_var waist_pR; // position and attitude
230  DblSequence_var waist_vw; // linear and angular velocities
231  dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, waist_pR);
232  dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_VELOCITY, waist_vw);
233 
234  // ================== log data save =====================
235  log_file << time << " ";
236  log_file << waist_vw[2] << " ";
237  log_file << endl;
238 
239  if(control)
240  controller->output();
241 
242  if( time > EndTime ) break;
243 
244  }
245 
246  controller->stop();
247  log_file.close();
248  dynamicsSimulator->destroy();
249 
250  return 0;
251 }
hrpPrep.NotFound
NotFound
Definition: hrpPrep.py:129
hrp::loadBodyInfo
HRPMODEL_API OpenHRP::BodyInfo_var loadBodyInfo(const char *url, int &argc, char *argv[])
Definition: hrplib/hrpModel/ModelLoaderUtil.cpp:632
state
state
checkCorbaServer
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
Definition: scheduler.cpp:14
manager
manager
i
png_uint_32 i
Definition: png.h:2735
trans
png_infop png_bytep * trans
Definition: png.h:2435
getWorldState
void getWorldState(WorldState &state, WorldBase &world)
Definition: hrpModel/OnlineViewerUtil.cpp:25
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
j
def j(str, encoding="cp932")
main
int main(int argc, char *argv[])
Definition: scheduler.cpp:44
hrp::getOnlineViewer
HRP_UTIL_EXPORT OpenHRP::OnlineViewer_var getOnlineViewer(int argc, char **argv)
Definition: hrpUtil/OnlineViewerUtil.cpp:61

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:41