viewSimulator.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 8; -*-
2 #include <iostream>
3 #include <hrpModel/ModelLoaderUtil.h>
4 #include <hrpCorba/DynamicsSimulator.hh>
5 #include <hrpCorba/Controller.hh>
6 #include <hrpCorba/ViewSimulator.hh>
7 #include <hrpUtil/Eigen3d.h>
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.002; // (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  ViewSimulator_var viewSimulator;
104  viewSimulator = checkCorbaServer <ViewSimulator, ViewSimulator_var> ("ViewSimulator", cxt);
105 
106  if (CORBA::is_nil(viewSimulator)) {
107  std::cerr << "viewSimulator not found" << std::endl;
108  return 1;
109  }
110  viewSimulator->registerCharacter(floor->name(), floor);
111  viewSimulator->registerCharacter(body->name(), body);
112 
113  //================= DynamicsSimulator setup ======================
114  DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
115  dynamicsSimulatorFactory =
116  checkCorbaServer <DynamicsSimulatorFactory, DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxt);
117 
118  if (CORBA::is_nil(dynamicsSimulatorFactory)) {
119  std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
120  }
121  DynamicsSimulator_var dynamicsSimulator = dynamicsSimulatorFactory->create();
122 
123  cout << "** Dynamics server setup ** " << endl;
124  cout << "Character :" << floor->name() << endl;
125  dynamicsSimulator->registerCharacter(floor->name(), floor);
126  cout << "Character :" << body->name() << endl;
127  dynamicsSimulator->registerCharacter(body->name(), body);
128 
129  dynamicsSimulator->init(timeStep, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);
130  DblSequence3 g;
131  g.length(3);
132  g[0] = 0.0;
133  g[1] = 0.0;
134  g[2] = world_gravity;
135  dynamicsSimulator->setGVector(g);
136 
137  // initial position and orientation
138  Vector3 waist_p;
139  Matrix33 waist_R;
140  waist_p << 0, 0, 0.7135;
141  waist_R = Matrix33::Identity();
142 
143  DblSequence trans;
144  trans.length(12);
145  for(int i=0; i<3; i++) trans[i] = waist_p(i);
146  for(int i=0; i<3; i++){
147  for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
148  }
149  dynamicsSimulator->setCharacterLinkData( body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, trans );
150  DblSequence angle;
151  angle.length(29);
152  angle[0] = 0.0; angle[1] = -0.0360373; angle[2] = 0.0; angle[3] = 0.0785047;
153  angle[4] = -0.0424675; angle[5] = 0.0; angle[6] = 0.174533; angle[7] = -0.00349066;
154  angle[8] = 0.0; angle[9] = -1.5708; angle[10] = 0.0; angle[11] = 0.0;
155  angle[12] = 0.0; angle[13] = 0.0; angle[14] = -0.0360373; angle[15] = 0.0;
156  angle[16] = 0.0785047; angle[17] = -0.0424675; angle[18] = 0.0; angle[19] = 0.174533;
157  angle[20] = -0.00349066;angle[21] = 0.0; angle[22] = -1.5708; angle[23] = 0.0;
158  angle[24] = 0.0; angle[25] = 0.0; angle[26] = 0.0; angle[27] = 0.0;
159  angle[28] = 0.0;
160  dynamicsSimulator->setCharacterAllLinkData( body->name(), DynamicsSimulator::JOINT_VALUE, angle );
161  dynamicsSimulator->calcWorldForwardKinematics();
162 
163  DblSequence6 K, C; // spring-damper parameters are not used now
164  K.length(0);
165  C.length(0);
166  dynamicsSimulator->registerCollisionCheckPair(floor->name(),"", body->name() ,"",
167  statFric,slipFric,K,C,culling_thresh,0.0);
168  dynamicsSimulator->initSimulation();
169 
170  // ================== Controller setup ==========================
171  Controller_var controller;
172  controller = checkCorbaServer <Controller, Controller_var> ("SamplePDController", cxt);
173 
174  if (CORBA::is_nil(controller)) {
175  std::cerr << "Controller not found" << std::endl;
176  }
177 
178  controller->setModelName(body->name());
179  controller->setDynamicsSimulator(dynamicsSimulator);
180  controller->initialize();
181  controller->setTimeStep(controlTimeStep);
182  controller->start();
183 
184  // ================== main loop ======================
185  WorldState_var state;
186  int i=0;
187  int j = 0;
188  double time=0.0;
189  double controlTime=0.0;
190  while ( 1 ) {
191  bool control=false;
192  if(controlTime <= time){
193  control=true;
194  j++;
195  }
196 
197  if(control)
198  controller->input();
199 
200  i++;
201  time = timeStep * i;
202  controlTime = controlTimeStep * j;
203 
204  if(control)
205  controller->control();
206 
207  // ================== simulate one step ==============
208  dynamicsSimulator->stepSimulation();
209 
210  // ================== viewer update ====================
211  try {
212  dynamicsSimulator -> getWorldState( state );
213  if((int)(time*500)%30==0){
214  viewSimulator->updateScene( state );
215  CameraSequence_var cameras;
216  viewSimulator->getCameraSequence(cameras);
217  for(int k=0; k<cameras->length(); k++){
218  ImageData_var imageData = cameras[k]->getImageData();
219  std::cout << "camera(" << k << ") ";
220  for(int l=0; l<imageData->floatData.length(); l+=100)
221  std::cout <<imageData->floatData[l] << " " ;
222  std::cout << std::endl;
223  }
224  }
225  } catch (CORBA::SystemException& ex) {
226  return 1;
227  }
228 
229  // ===================== get robot status ===================
230  DblSequence_var waist_pR; // position and attitude
231  DblSequence_var waist_vw; // linear and angular velocities
232  dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, waist_pR);
233  dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_VELOCITY, waist_vw);
234 
235  if(control)
236  controller->output();
237 
238  if( time > EndTime ) break;
239 
240  }
241 
242  dynamicsSimulator->destroy();
243 
244  return 0;
245 }
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: viewSimulator.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: viewSimulator.cpp:44

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