projectGenerator.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <rtm/Manager.h>
11 #include <rtm/CorbaNaming.h>
12 #include <hrpModel/Body.h>
13 #include <hrpModel/Link.h>
14 #include <hrpModel/Sensor.h>
16 #include <libxml/encoding.h>
17 #include <libxml/xmlwriter.h>
18 #include <string>
19 #include <sstream>
20 #include <fstream>
21 #include <stack>
22 #include "BodyInfo_impl.h"
23 #include <sys/stat.h>
24 
25 using namespace std;
26 void xmlTextWriterWriteProperty(const xmlTextWriterPtr writer, const std::string name, const std::string value) {
27  xmlTextWriterStartElement(writer, BAD_CAST "property");
28  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
29  xmlTextWriterWriteAttribute(writer, BAD_CAST "value", BAD_CAST value.c_str());
30  xmlTextWriterEndElement(writer);
31 }
32 
33 std::string basename(const std::string name){
34  std::string ret = name.substr(name.find_last_of('/')+1);
35  return ret.substr(0,ret.find_last_of('.'));
36 }
37 
38 // COPY FROM openhrp3/hrplib/hrpModel/ModelLoaderUtil.cpp
40 {
41 public:
43  }
44 
45  void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
46  {
47  int numSensors = sensorInfoSeq.length();
48 
49  for(int i=0 ; i < numSensors ; ++i ) {
50  const SensorInfo& sensorInfo = sensorInfoSeq[i];
51 
52  int id = sensorInfo.id;
53  if(id < 0) {
54  std::cerr << "Warning: sensor ID is not given to sensor " << sensorInfo.name
55  << "of model " << body->modelName() << "." << std::endl;
56  } else {
57  int sensorType = Sensor::COMMON;
58 
59  CORBA::String_var type0 = sensorInfo.type;
60  string type(type0);
61 
62  if(type == "Force") { sensorType = Sensor::FORCE; }
63  else if(type == "RateGyro") { sensorType = Sensor::RATE_GYRO; }
64  else if(type == "Acceleration") { sensorType = Sensor::ACCELERATION; }
65  else if(type == "Vision") { sensorType = Sensor::VISION; }
66  else if(type == "Range") { sensorType = Sensor::RANGE; }
67 
68  CORBA::String_var name0 = sensorInfo.name;
69  string name(name0);
70 
71  Sensor* sensor = body->createSensor(link, sensorType, id, name);
72 
73  if(sensor) {
74  const DblArray3& p = sensorInfo.translation;
75  sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
76 
77  const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
78  const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
79  sensor->localR = Rs * R;
80  }
81 #if 0
82  if ( sensorType == Sensor::RANGE ) {
83  RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
84  range->scanAngle = sensorInfo.specValues[0];
85  range->scanStep = sensorInfo.specValues[1];
86  range->scanRate = sensorInfo.specValues[2];
87  range->maxDistance = sensorInfo.specValues[3];
88  }else if (sensorType == Sensor::VISION) {
89  VisionSensor *vision = dynamic_cast<VisionSensor *>(sensor);
90  vision->near = sensorInfo.specValues[0];
91  vision->far = sensorInfo.specValues[1];
92  vision->fovy = sensorInfo.specValues[2];
93  vision->width = sensorInfo.specValues[4];
94  vision->height = sensorInfo.specValues[5];
95  int npixel = vision->width*vision->height;
96  switch((int)sensorInfo.specValues[3]){
97  case Camera::NONE:
98  vision->imageType = VisionSensor::NONE;
99  break;
100  case Camera::COLOR:
101  vision->imageType = VisionSensor::COLOR;
102  vision->image.resize(npixel*3);
103  break;
104  case Camera::MONO:
105  vision->imageType = VisionSensor::MONO;
106  vision->image.resize(npixel);
107  break;
108  case Camera::DEPTH:
109  vision->imageType = VisionSensor::DEPTH;
110  break;
111  case Camera::COLOR_DEPTH:
112  vision->imageType = VisionSensor::COLOR_DEPTH;
113  vision->image.resize(npixel*3);
114  break;
115  case Camera::MONO_DEPTH:
116  vision->imageType = VisionSensor::MONO_DEPTH;
117  vision->image.resize(npixel);
118  break;
119  }
120  vision->frameRate = sensorInfo.specValues[6];
121  }
122 #endif
123  }
124  }
125  }
126 
127  inline double getLimitValue(DblSequence limitseq, double defaultValue)
128  {
129  return (limitseq.length() == 0) ? defaultValue : limitseq[0];
130  }
131 
132  Link* createLink(int index, const Matrix33& parentRs)
133  {
134  const LinkInfo& linkInfo = linkInfoSeq[index];
135  int jointId = linkInfo.jointId;
136 
137  Link* link = new Link(); //(*createLinkFunc)();
138 
139  CORBA::String_var name0 = linkInfo.name;
140  link->name = string( name0 );
141  link->jointId = jointId;
142 
143  Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
144  link->b = parentRs * relPos;
145 
146  Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
147  Matrix33 R = rodrigues(rotAxis, linkInfo.rotation[3]);
148  link->Rs = (parentRs * R);
149  const Matrix33& Rs = link->Rs;
150 
151  CORBA::String_var jointType = linkInfo.jointType;
152  const std::string jt( jointType );
153 
154  if(jt == "fixed" ){
155  link->jointType = Link::FIXED_JOINT;
156  } else if(jt == "free" ){
157  link->jointType = Link::FREE_JOINT;
158  } else if(jt == "rotate" ){
159  link->jointType = Link::ROTATIONAL_JOINT;
160  } else if(jt == "slide" ){
161  link->jointType = Link::SLIDE_JOINT;
162  } else if(jt == "crawler"){
163  link->jointType = Link::FIXED_JOINT;
164  link->isCrawler = true;
165  } else {
166  link->jointType = Link::FREE_JOINT;
167  }
168 
169  if(jointId < 0){
170  if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
171  std::cerr << "Warning: Joint ID is not given to joint " << link->name
172  << " of model " << body->modelName() << "." << std::endl;
173  }
174  }
175 
176  link->a.setZero();
177  link->d.setZero();
178 
179  Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
180 
181  if(link->jointType == Link::ROTATIONAL_JOINT || jt == "crawler"){
182  link->a = axis;
183  } else if(link->jointType == Link::SLIDE_JOINT){
184  link->d = axis;
185  }
186 
187  link->m = linkInfo.mass;
188  link->Ir = linkInfo.rotorInertia;
189 
190  link->gearRatio = linkInfo.gearRatio;
191  link->rotorResistor = linkInfo.rotorResistor;
192  link->torqueConst = linkInfo.torqueConst;
193  link->encoderPulse = linkInfo.encoderPulse;
194 
195  link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
196 
197  DblSequence ulimit = linkInfo.ulimit;
198  DblSequence llimit = linkInfo.llimit;
199  DblSequence uvlimit = linkInfo.uvlimit;
200  DblSequence lvlimit = linkInfo.lvlimit;
201  DblSequence climit = linkInfo.climit;
202 
203  double maxlimit = (numeric_limits<double>::max)();
204 
205  link->ulimit = getLimitValue(ulimit, +maxlimit);
206  link->llimit = getLimitValue(llimit, -maxlimit);
207  link->uvlimit = getLimitValue(uvlimit, +maxlimit);
208  link->lvlimit = getLimitValue(lvlimit, -maxlimit);
209  link->climit = getLimitValue(climit, +maxlimit);
210 
211  link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
212 
213  Matrix33 Io;
214  getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
215  link->I = Rs * Io * Rs.transpose();
216 
217  // a stack is used for keeping the same order of children
218  std::stack<Link*> children;
219 
220  //##### [Changed] Link Structure (convert NaryTree to BinaryTree).
221  int childNum = linkInfo.childIndices.length();
222  for(int i = 0 ; i < childNum ; i++) {
223  int childIndex = linkInfo.childIndices[i];
224  Link* childLink = createLink(childIndex, Rs);
225  if(childLink) {
226  children.push(childLink);
227  }
228  }
229  while(!children.empty()){
230  link->addChild(children.top());
231  children.pop();
232  }
233 
234  createSensors(link, linkInfo.sensors, Rs);
235  //createLights(link, linkInfo.lights, Rs);
236 #if 0
237  if(collisionDetectionModelLoading){
238  createColdetModel(link, linkInfo);
239  }
240 #endif
241  return link;
242  }
243 
244  bool createBody(BodyPtr& body, BodyInfo_impl& bodyInfo)
245  {
246  this->body = body;
247 
248  const char* name = bodyInfo.name();
249  body->setModelName(name);
250  body->setName(name);
251 
252  int n = bodyInfo.links()->length();
253  linkInfoSeq = bodyInfo.links();
254  shapeInfoSeq = bodyInfo.shapes();
255  extraJointInfoSeq = bodyInfo.extraJoints();
256 
257  int rootIndex = -1;
258 
259  for(int i=0; i < n; ++i){
260  if(linkInfoSeq[i].parentIndex < 0){
261  if(rootIndex < 0){
262  rootIndex = i;
263  } else {
264  // more than one root !
265  rootIndex = -1;
266  break;
267  }
268  }
269  }
270 
271  if(rootIndex >= 0){ // root exists
272 
273  Matrix33 Rs(Matrix33::Identity());
274  Link* rootLink = createLink(rootIndex, Rs);
275  body->setRootLink(rootLink);
276  body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
277 
278  body->installCustomizer();
279  body->initializeConfiguration();
280 #if 0
281  setExtraJoints();
282 #endif
283  return true;
284  }
285 
286  return false;
287  }
288 private:
290  LinkInfoSequence_var linkInfoSeq;
291  ShapeInfoSequence_var shapeInfoSeq;
292  ExtraJointInfoSequence_var extraJointInfoSeq;
293 };
294 
295 int main (int argc, char** argv)
296 {
297  std::string output;
298  std::vector<std::string> inputs, filenames; // filenames is for conf file
299  std::string conf_file_option, robothardware_conf_file_option, integrate("true"), dt("0.005"), timeStep(dt), joint_properties, method("EULER");
300  bool use_highgain_mode(true);
301  struct stat st;
302  bool file_exist_flag = false;
303 
304  for (int i = 1; i < argc; ++ i) {
305  std::string arg(argv[i]);
306  coil::normalize(arg);
307  if ( arg == "--output" ) {
308  if (++i < argc) output = argv[i];
309  } else if ( arg == "--integrate" ) {
310  if (++i < argc) integrate = argv[i];
311  } else if ( arg == "--dt" ) {
312  if (++i < argc) dt = argv[i];
313  } else if ( arg == "--timestep" ) {
314  if (++i < argc) timeStep = argv[i];
315  } else if ( arg == "--conf-file-option" ) {
316  if (++i < argc) conf_file_option += std::string("\n") + argv[i];
317  } else if ( arg == "--robothardware-conf-file-option" ) {
318  if (++i < argc) robothardware_conf_file_option += std::string("\n") + argv[i];
319  } else if ( arg == "--joint-properties" ) {
320  if (++i < argc) joint_properties = argv[i];
321  } else if ( arg == "--use-highgain-mode" ) {
322  if (++i < argc) use_highgain_mode = (std::string(argv[i])==std::string("true")?true:false);
323  } else if ( arg == "--method" ) {
324  if (++i < argc) method = std::string(argv[i]);
325  } else if ( arg.find("--gtest_output") == 0 ||arg.find("--text") == 0 || arg.find("__log") == 0 || arg.find("__name") == 0 ) { // skip
326  } else {
327  inputs.push_back(argv[i]);
328  }
329  }
330 
331  CORBA::ORB_var orb = CORBA::ORB::_nil();
332 
333  try
334  {
335  orb = CORBA::ORB_init(argc, argv);
336 
337  CORBA::Object_var obj;
338 
339  obj = orb->resolve_initial_references("RootPOA");
340  PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);
341  if(CORBA::is_nil(poa))
342  {
343  throw string("error: failed to narrow root POA.");
344  }
345 
346  PortableServer::POAManager_var poaManager = poa->the_POAManager();
347  if(CORBA::is_nil(poaManager))
348  {
349  throw string("error: failed to narrow root POA manager.");
350  }
351 
352  xmlTextWriterPtr writer;
353  if (stat(output.c_str(), &st) == 0) {
354  file_exist_flag = true;
355  }
356  writer = xmlNewTextWriterFilename(output.c_str(), 0);
357  xmlTextWriterSetIndent(writer, 4);
358  xmlTextWriterStartElement(writer, BAD_CAST "grxui");
359  {
360  xmlTextWriterStartElement(writer, BAD_CAST "mode");
361  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "Simulation");
362  {
363  xmlTextWriterStartElement(writer, BAD_CAST "item");
364  xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxSimulationItem");
365  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "simulationItem");
366  {
367  xmlTextWriterWriteProperty(writer, "integrate", integrate);
368  xmlTextWriterWriteProperty(writer, "timeStep", timeStep);
369  xmlTextWriterWriteProperty(writer, "totalTime", "2000000.0");
370  xmlTextWriterWriteProperty(writer, "method", method);
371  }
372  xmlTextWriterEndElement(writer); // item
373  // default WAIST offset
374  hrp::Vector3 WAIST_offset_pos = hrp::Vector3::Zero();
375  Eigen::AngleAxis<double> WAIST_offset_rot = Eigen::AngleAxis<double>(0, hrp::Vector3(0,0,1)); // rotation in VRML is represented by axis + angle
376  for (std::vector<std::string>::iterator it = inputs.begin();
377  it != inputs.end();
378  it++) {
379  // argument for VRML supports following two mode:
380  // 1. xxx.wrl
381  // To specify VRML file. WAIST offsets is 0 transformation.
382  // 2. xxx.wrl,0,0,0,0,0,1,0
383  // To specify both VRML and WAIST offsets.
384  // WAIST offset: for example, "0,0,0,0,0,1,0" -> position offset (3dof) + axis for rotation offset (3dof) + angle for rotation offset (1dof)
385  coil::vstring filename_arg_str = coil::split(*it, ",");
386  std::string filename = filename_arg_str[0];
387  filenames.push_back(filename);
388  if ( filename_arg_str.size () > 1 ){ // if WAIST offset is specified
389  for (size_t i = 0; i < 3; i++) {
390  coil::stringTo(WAIST_offset_pos(i), filename_arg_str[i+1].c_str());
391  coil::stringTo(WAIST_offset_rot.axis()(i), filename_arg_str[i+1+3].c_str());
392  }
393  coil::stringTo(WAIST_offset_rot.angle(), filename_arg_str[1+3+3].c_str());
394  }
395  hrp::BodyPtr body(new hrp::Body());
396 
397  BodyInfo_impl bI(poa);
398  bI.loadModelFile(filename.c_str());
399  ModelLoaderHelper2 helper;
400  helper.createBody(body, bI);
401 
402  std::string name = body->name();
403 
404  xmlTextWriterStartElement(writer, BAD_CAST "item");
405  xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxRTSItem");
406  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
407  xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
408 
409  xmlTextWriterWriteProperty(writer, name+"(Robot)0.period", dt);
410  if (use_highgain_mode) {
411  xmlTextWriterWriteProperty(writer, "HGcontroller0.period", dt);
412  xmlTextWriterWriteProperty(writer, "HGcontroller0.factory", "HGcontroller");
413  if (it==inputs.begin()) {
414  xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.qOut:"+name+"(Robot)0.qRef");
415  xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.dqOut:"+name+"(Robot)0.dqRef");
416  xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.ddqOut:"+name+"(Robot)0.ddqRef");
417  }
418  } else {
419  xmlTextWriterWriteProperty(writer, "PDcontroller0.period", dt);
420  xmlTextWriterWriteProperty(writer, "PDcontroller0.factory", "PDcontroller");
421  if (it==inputs.begin()) {
422  xmlTextWriterWriteProperty(writer, "connection", "PDcontroller0.torque:"+name+"(Robot)0.tauRef");
423  xmlTextWriterWriteProperty(writer, "connection", ""+name+"(Robot)0.q:PDcontroller0.angle");
424  }
425  }
426  xmlTextWriterEndElement(writer); // item
427 
428 
429  xmlTextWriterStartElement(writer, BAD_CAST "item");
430  xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxModelItem");
431  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST (basename(*it).c_str()));
432  xmlTextWriterWriteAttribute(writer, BAD_CAST "url", BAD_CAST filename.c_str());
433 
434  xmlTextWriterWriteProperty(writer, "rtcName", name + "(Robot)0");
435  if (use_highgain_mode) {
436  xmlTextWriterWriteProperty(writer, "inport", "qRef:JOINT_VALUE");
437  xmlTextWriterWriteProperty(writer, "inport", "dqRef:JOINT_VELOCITY");
438  xmlTextWriterWriteProperty(writer, "inport", "ddqRef:JOINT_ACCELERATION");
439  if (integrate == "false") { // For kinematics only simultion
440  xmlTextWriterWriteProperty(writer, "inport", "basePoseRef:"+body->rootLink()->name+":ABS_TRANSFORM");
441  }
442  } else {
443  xmlTextWriterWriteProperty(writer, "inport", "tauRef:JOINT_TORQUE");
444  }
445  xmlTextWriterWriteProperty(writer, "outport", "q:JOINT_VALUE");
446  xmlTextWriterWriteProperty(writer, "outport", "dq:JOINT_VELOCITY");
447  xmlTextWriterWriteProperty(writer, "outport", "tau:JOINT_TORQUE");
448  xmlTextWriterWriteProperty(writer, "outport", body->rootLink()->name+":"+body->rootLink()->name+":ABS_TRANSFORM");
449 
450  // set outport for sensros
451  int nforce = body->numSensors(hrp::Sensor::FORCE);
452  if ( nforce > 0 ) std::cerr << "hrp::Sensor::FORCE";
453  for (int i=0; i<nforce; i++){
454  hrp::Sensor *s = body->sensor(hrp::Sensor::FORCE, i);
455  // port name and sensor name is same in case of ForceSensor
456  xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":FORCE_SENSOR");
457  std::cerr << " " << s->name;
458  }
459  if ( nforce > 0 ) std::cerr << std::endl;
460  int ngyro = body->numSensors(hrp::Sensor::RATE_GYRO);
461  if ( ngyro > 0 ) std::cerr << "hrp::Sensor::GYRO";
462  if(ngyro == 1){
463  // port is named with no number when there is only one gyro
464  hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, 0);
465  xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":RATE_GYRO_SENSOR");
466  std::cerr << " " << s->name;
467  }else{
468  for (int i=0; i<ngyro; i++){
469  hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, i);
470  std::stringstream str_strm;
471  str_strm << s->name << i << ":" + s->name << ":RATE_GYRO_SENSOR";
472  xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
473  std::cerr << " " << s->name;
474  }
475  }
476  if ( ngyro > 0 ) std::cerr << std::endl;
477  int nacc = body->numSensors(hrp::Sensor::ACCELERATION);
478  if ( nacc > 0 ) std::cerr << "hrp::Sensor::ACCELERATION";
479  if(nacc == 1){
480  // port is named with no number when there is only one acc
481  hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, 0);
482  xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":ACCELERATION_SENSOR");
483  std::cerr << " " << s->name;
484  }else{
485  for (int i=0; i<nacc; i++){
486  hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, i);
487  std::stringstream str_strm;
488  str_strm << s->name << i << ":" << s->name << ":ACCELERATION_SENSOR";
489  xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
490  std::cerr << " " << s->name;
491  }
492  }
493  if ( nacc > 0 ) std::cerr << std::endl;
494 
495  //
496  std::string root_name = body->rootLink()->name;
497  xmlTextWriterWriteProperty(writer, root_name+".NumOfAABB", "1");
498 
499  // write waist pos and rot by considering both VRML original WAIST and WAIST_offset_pos and WAIST_offset_rot from arguments
500  std::ostringstream os;
501  os << body->rootLink()->p[0] + WAIST_offset_pos(0) << " "
502  << body->rootLink()->p[1] + WAIST_offset_pos(1) << " "
503  << body->rootLink()->p[2] + WAIST_offset_pos(2); // 10cm margin
504  xmlTextWriterWriteProperty(writer, root_name+".translation", os.str());
505  os.str(""); // reset ostringstream
506  Eigen::AngleAxis<double> tmpAA = Eigen::AngleAxis<double>(hrp::Matrix33(body->rootLink()->R * WAIST_offset_rot.toRotationMatrix()));
507  os << tmpAA.axis()(0) << " "
508  << tmpAA.axis()(1) << " "
509  << tmpAA.axis()(2) << " "
510  << tmpAA.angle();
511  xmlTextWriterWriteProperty(writer, root_name+".rotation", os.str());
512 
513  if ( ! body->isStaticModel() ) {
514  xmlTextWriterWriteProperty(writer, root_name+".mode", "Torque");
515  xmlTextWriterWriteProperty(writer, "controller", basename(output));
516  }
517 
518  // store joint properties
519  // [property 1],[value 1],....
520  // ex. --joint-properties RARM_JOINT0.angle,0.0,RARM_JOINT2.mode,HighGain,...
521  coil::vstring joint_properties_arg_str = coil::split(joint_properties, ",");
522  std::map <std::string, std::string> joint_properties_map;
523  for (size_t i = 0; i < joint_properties_arg_str.size()/2; i++) {
524  joint_properties_map.insert(std::pair<std::string, std::string>(joint_properties_arg_str[i*2], joint_properties_arg_str[i*2+1]));
525  }
526  if ( body->numJoints() > 0 ) std::cerr << "hrp::Joint";
527  for(unsigned int i = 0; i < body->numJoints(); i++){
528  if ( body->joint(i)->index > 0 ) {
529  std::cerr << " " << body->joint(i)->name << "(" << body->joint(i)->jointId << ")";
530  std::string joint_name = body->joint(i)->name;
531  std::string j_property = joint_name+".angle";
532  xmlTextWriterWriteProperty(writer, j_property,
533  (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "0.0");
534  j_property = joint_name+".mode";
535  xmlTextWriterWriteProperty(writer, j_property,
536  (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : (use_highgain_mode?"HighGain":"Torque"));
537  j_property = joint_name+".NumOfAABB";
538  xmlTextWriterWriteProperty(writer, j_property,
539  (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "1");
540  }
541  }
542  xmlTextWriterEndElement(writer); // item
543  if ( body->numJoints() > 0 ) std::cerr << std::endl;
544 
545  //
546  // comment out self collision settings according to issues at https://github.com/fkanehiro/hrpsys-base/issues/122
547  // xmlTextWriterStartElement(writer, BAD_CAST "item");
548  // xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
549  // xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name).c_str());
550  // xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
551  // {
552  // xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
553  // xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
554  // xmlTextWriterWriteProperty(writer, "jointName2", "");
555  // xmlTextWriterWriteProperty(writer, "jointName1", "");
556  // xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
557  // xmlTextWriterWriteProperty(writer, "objectName2", name);
558  // xmlTextWriterWriteProperty(writer, "objectName1", name);
559  // xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
560  // xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
561  // }
562  // xmlTextWriterEndElement(writer); // item
563 
564  xmlTextWriterStartElement(writer, BAD_CAST "view");
565  xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.GrxRobotHardwareClientView");
566  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "RobotHardware RTC Client");
567  xmlTextWriterWriteProperty(writer, "robotHost", "localhost");
568  xmlTextWriterWriteProperty(writer, "StateHolderRTC", "StateHolder0");
569  xmlTextWriterWriteProperty(writer, "interval", "100");
570  xmlTextWriterWriteProperty(writer, "RobotHardwareServiceRTC", "RobotHardware0");
571  xmlTextWriterWriteProperty(writer, "robotPort", "2809");
572  xmlTextWriterWriteProperty(writer, "ROBOT", name.c_str());
573  xmlTextWriterEndElement(writer); // item
574 
575  xmlTextWriterStartElement(writer, BAD_CAST "view");
576  xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.Grx3DView");
577  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "3DView");
578  xmlTextWriterWriteProperty(writer, "view.mode", "Room");
579  xmlTextWriterWriteProperty(writer, "showCoM", "false");
580  xmlTextWriterWriteProperty(writer, "showCoMonFloor", "false");
581  xmlTextWriterWriteProperty(writer, "showDistance", "false");
582  xmlTextWriterWriteProperty(writer, "showIntersection", "false");
583  xmlTextWriterWriteProperty(writer, "eyeHomePosition", "-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 ");
584  xmlTextWriterWriteProperty(writer, "showCollision", "true");
585  xmlTextWriterWriteProperty(writer, "showActualState", "true");
586  xmlTextWriterWriteProperty(writer, "showScale", "true");
587  xmlTextWriterEndElement(writer); // view
588  }
589 
590  {
591  for (std::vector<std::string>::iterator it1 = inputs.begin(); it1 != inputs.end(); it1++) {
592  std::string name1 = basename(*it1);
593  for (std::vector<std::string>::iterator it2 = it1+1; it2 != inputs.end(); it2++) {
594  std::string name2 = basename(*it2);
595 
596  xmlTextWriterStartElement(writer, BAD_CAST "item");
597  xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
598  xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name2+"_#"+name1+"_").c_str());
599  {
600  xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
601  xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
602  xmlTextWriterWriteProperty(writer, "jointName2", "");
603  xmlTextWriterWriteProperty(writer, "jointName1", "");
604  xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
605  xmlTextWriterWriteProperty(writer, "objectName2", name2);
606  xmlTextWriterWriteProperty(writer, "objectName1", name1);
607  xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
608  xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
609  }
610  xmlTextWriterEndElement(writer); // item
611  }
612  }
613  }
614  }
615  xmlTextWriterEndElement(writer); // mode
616  }
617  xmlTextWriterEndElement(writer); // grxui
618 
619  xmlFreeTextWriter(writer);
620 
621  if (file_exist_flag) {
622  std::cerr << "\033[1;31mOver\033[0m";
623  }
624  std::cerr << "Writing project files to .... " << output << std::endl;
625  {
626  std::string conf_file = output.substr(0,output.find_last_of('.'))+".conf";
627  if (stat(conf_file.c_str(), &st) == 0) {
628  std::cerr << "\033[1;31mOver\033[0m";
629  }
630  std::fstream s(conf_file.c_str(), std::ios::out);
631 
632  s << "model: file://" << filenames[0] << std::endl;
633  s << "dt: " << dt << std::endl;
634  s << conf_file_option << std::endl;
635  std::cerr << "Writing conf files to ....... " << conf_file << std::endl;
636  }
637 
638  {
639  std::string conf_file = output.substr(0,output.find_last_of('.'))+".RobotHardware.conf";
640  if (stat(conf_file.c_str(), &st) == 0) {
641  std::cerr << "\033[1;31mOver\033[0m";
642  }
643  std::fstream s(conf_file.c_str(), std::ios::out);
644 
645  s << "model: file://" << filenames[0] << std::endl;
646  s << "exec_cxt.periodic.type: hrpExecutionContext" << std::endl;
647  s << "exec_cxt.periodic.rate: " << static_cast<size_t>(1/atof(dt.c_str())+0.5) << std::endl; // rounding to specify integer rate value
648  s << robothardware_conf_file_option << std::endl;
649  std::cerr << "Writing hardware files to ... " << conf_file << std::endl;
650  }
651 
652  }
653  catch (CORBA::SystemException& ex)
654  {
655  cerr << ex._rep_id() << endl;
656  }
657  catch (const string& error)
658  {
659  cerr << error << endl;
660  }
661 
662  try
663  {
664  orb->destroy();
665  }
666  catch(...)
667  {
668 
669  }
670 
671  return 0;
672 }
std::string normalize(std::string &str)
Link * createLink(int index, const Matrix33 &parentRs)
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
void xmlTextWriterWriteProperty(const xmlTextWriterPtr writer, const std::string name, const std::string value)
char * filename
Definition: cdjpeg.h:133
bool stringTo(To &val, const char *str)
std::vector< unsigned char > image
virtual LinkInfoSequence * links()
png_infop png_charpp name
Definition: png.h:2382
png_voidp int value
Definition: png.h:2113
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
void error(char *msg) const
Definition: minigzip.c:87
virtual ShapeInfoSequence * shapes()
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
png_uint_32 i
Definition: png.h:2735
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
std::string basename(const std::string name)
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
std::vector< std::string > vstring
OpenHRP::vector3 Vector3
ExtraJointInfoSequence_var extraJointInfoSeq
ShapeInfoSequence_var shapeInfoSeq
bool createBody(BodyPtr &body, BodyInfo_impl &bodyInfo)
int method
Definition: png.h:1847
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
void loadModelFile(const std::string &filename)
This function loads a model file and creates a BodyInfo object.
void createSensors(Link *link, const SensorInfoSequence &sensorInfoSeq, const Matrix33 &Rs)
LinkInfoSequence_var linkInfoSeq
static Joint * createLink(::World *world, const char *charname, int index, LinkInfoSequence_var iLinks, Joint *pjoint)
double getLimitValue(DblSequence limitseq, double defaultValue)
virtual char * name()
Definition: Body.h:44
virtual ExtraJointInfoSequence * extraJoints()
char * arg
Definition: cdjpeg.h:136
int main(int argc, char **argv)
void * writer(void *arg)
static int max(int a, int b)
double getLimitValue(DblSequence limitseq, double defaultValue)
output(gif_dest_ptr dinfo, int code)
Definition: wrgif.c:105
static void createSensors(::World *world, Joint *jnt, SensorInfoSequence iSensors)


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:40