ProjectGenerator.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/Manager.h>
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/Body.h>
00013 #include <hrpModel/Link.h>
00014 #include <hrpModel/Sensor.h>
00015 #include <hrpModel/ModelLoaderUtil.h>
00016 #include <libxml/encoding.h>
00017 #include <libxml/xmlwriter.h>
00018 #include <string>
00019 #include <sstream>
00020 #include <fstream>
00021 
00022 void xmlTextWriterWriteProperty(const xmlTextWriterPtr writer, const std::string name, const std::string value) {
00023   xmlTextWriterStartElement(writer, BAD_CAST "property");
00024   xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
00025   xmlTextWriterWriteAttribute(writer, BAD_CAST "value", BAD_CAST value.c_str());
00026   xmlTextWriterEndElement(writer);
00027 }
00028 
00029 std::string basename(const std::string name){
00030   std::string ret = name.substr(name.find_last_of('/')+1);
00031   return ret.substr(0,ret.find_last_of('.'));
00032 }
00033 
00034 int main (int argc, char** argv)
00035 {
00036   std::string output;
00037   std::vector<std::string> inputs, filenames; // filenames is for conf file
00038   std::string conf_file_option, robothardware_conf_file_option, integrate("true"), dt("0.005"), timeStep(dt), joint_properties;
00039   bool use_highgain_mode(true);
00040 
00041   int rtmargc=0;
00042   std::vector<char *> rtmargv;
00043   rtmargv.push_back(argv[0]);
00044   rtmargc++;
00045   for (int i = 1; i < argc; ++ i) {
00046     std::string arg(argv[i]);
00047     coil::normalize(arg);
00048     if ( arg == "--output" ) {
00049       if (++i < argc) output = argv[i];
00050     } else if ( arg == "--integrate" ) {
00051       if (++i < argc) integrate = argv[i];
00052     } else if ( arg == "--dt" ) {
00053       if (++i < argc) dt = argv[i];
00054     } else if ( arg == "--timestep" ) {
00055       if (++i < argc) timeStep = argv[i];
00056     } else if ( arg == "--conf-file-option" ) {
00057       if (++i < argc) conf_file_option += std::string("\n") + argv[i];
00058     } else if ( arg == "--robothardware-conf-file-option" ) {
00059       if (++i < argc) robothardware_conf_file_option += std::string("\n") + argv[i];
00060     } else if ( arg == "--joint-properties" ) {
00061       if (++i < argc) joint_properties = argv[i];
00062     } else if ( arg == "--use-highgain-mode" ) {
00063       if (++i < argc) use_highgain_mode = (std::string(argv[i])==std::string("true")?true:false);
00064     } else if ( arg.find("--gtest_output") == 0  ||arg.find("--text") == 0 || arg.find("__log") == 0 || arg.find("__name") == 0 ) { // skip
00065     } else if ( arg[0] == '-'  ) {
00066       rtmargv.push_back(argv[i]);
00067       rtmargv.push_back(argv[i+1]);
00068       rtmargc+=2;
00069       ++i;
00070     } else {
00071       inputs.push_back(argv[i]);
00072     }
00073   }
00074 
00075   RTC::Manager* manager;
00076   manager = RTC::Manager::init(rtmargc, rtmargv.data());
00077 
00078   std::string nameServer = manager->getConfig()["corba.nameservers"];
00079   int comPos = nameServer.find(",");
00080   if (comPos < 0){
00081     comPos = nameServer.length();
00082   }
00083   nameServer = nameServer.substr(0, comPos);
00084   RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
00085 
00086   xmlTextWriterPtr writer;
00087   writer = xmlNewTextWriterFilename(output.c_str(), 0);
00088   xmlTextWriterSetIndent(writer, 4);
00089   xmlTextWriterStartElement(writer, BAD_CAST "grxui");
00090   {
00091     xmlTextWriterStartElement(writer, BAD_CAST "mode");
00092     xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "Simulation");
00093     {
00094       xmlTextWriterStartElement(writer, BAD_CAST "item");
00095       xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxSimulationItem");
00096       xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "simulationItem");
00097       {
00098         xmlTextWriterWriteProperty(writer, "integrate", integrate);
00099         xmlTextWriterWriteProperty(writer, "timeStep", timeStep);
00100         xmlTextWriterWriteProperty(writer, "totalTime", "2000000.0");
00101         xmlTextWriterWriteProperty(writer, "method", "EULER");
00102       }
00103       xmlTextWriterEndElement(writer); // item
00104       // default WAIST offset
00105       hrp::Vector3 WAIST_offset_pos = hrp::Vector3::Zero();
00106       Eigen::AngleAxis<double> WAIST_offset_rot = Eigen::AngleAxis<double>(0, hrp::Vector3(0,0,1)); // rotation in VRML is represented by axis + angle
00107       for (std::vector<std::string>::iterator it = inputs.begin();
00108            it != inputs.end();
00109            it++) {
00110         // argument for VRML supports following two mode:
00111         //  1. xxx.wrl
00112         //    To specify VRML file. WAIST offsets is 0 transformation.
00113         //  2. xxx.wrl,0,0,0,0,0,1,0
00114         //    To specify both VRML and WAIST offsets.
00115         //    WAIST offset: for example, "0,0,0,0,0,1,0" -> position offset (3dof) + axis for rotation offset (3dof) + angle for rotation offset (1dof)
00116         coil::vstring filename_arg_str = coil::split(*it, ",");
00117         std::string filename = filename_arg_str[0];
00118         filenames.push_back(filename);
00119         if ( filename_arg_str.size () > 1 ){ // if WAIST offset is specified 
00120           for (size_t i = 0; i < 3; i++) {
00121             coil::stringTo(WAIST_offset_pos(i), filename_arg_str[i+1].c_str());
00122             coil::stringTo(WAIST_offset_rot.axis()(i), filename_arg_str[i+1+3].c_str());
00123           }
00124           coil::stringTo(WAIST_offset_rot.angle(), filename_arg_str[1+3+3].c_str());
00125         }
00126         hrp::BodyPtr body(new hrp::Body());
00127         if (!loadBodyFromModelLoader(body, filename.c_str(),
00128                                      CosNaming::NamingContext::_duplicate(naming.getRootContext()),
00129                                      true)){
00130           std::cerr << "failed to load model[" << filename << "]" << std::endl;
00131           return 1;
00132         }
00133         std::string name = body->name();
00134 
00135         xmlTextWriterStartElement(writer, BAD_CAST "item");
00136         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxRTSItem");
00137         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
00138         xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
00139 
00140         xmlTextWriterWriteProperty(writer, name+"(Robot)0.period", dt);
00141         if (use_highgain_mode) {
00142           xmlTextWriterWriteProperty(writer, "HGcontroller0.period", dt);
00143           xmlTextWriterWriteProperty(writer, "HGcontroller0.factory", "HGcontroller");
00144           if (it==inputs.begin()) {
00145             xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.qOut:"+name+"(Robot)0.qRef");
00146             xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.dqOut:"+name+"(Robot)0.dqRef");
00147             xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.ddqOut:"+name+"(Robot)0.ddqRef");
00148           }
00149         } else {
00150           xmlTextWriterWriteProperty(writer, "PDcontroller0.period", dt);
00151           xmlTextWriterWriteProperty(writer, "PDcontroller0.factory", "PDcontroller");
00152           if (it==inputs.begin()) {
00153             xmlTextWriterWriteProperty(writer, "connection", "PDcontroller0.torque:"+name+"(Robot)0.tauRef");
00154             xmlTextWriterWriteProperty(writer, "connection", ""+name+"(Robot)0.q:PDcontroller0.angle");
00155           }
00156         }
00157         xmlTextWriterEndElement(writer); // item
00158 
00159 
00160         xmlTextWriterStartElement(writer, BAD_CAST "item");
00161         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxModelItem");
00162         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST (basename(*it).c_str()));
00163         xmlTextWriterWriteAttribute(writer, BAD_CAST "url", BAD_CAST filename.c_str());
00164 
00165         xmlTextWriterWriteProperty(writer, "rtcName", name + "(Robot)0");
00166         if (use_highgain_mode) {
00167           xmlTextWriterWriteProperty(writer, "inport", "qRef:JOINT_VALUE");
00168           xmlTextWriterWriteProperty(writer, "inport", "dqRef:JOINT_VELOCITY");
00169           xmlTextWriterWriteProperty(writer, "inport", "ddqRef:JOINT_ACCELERATION");
00170           if (integrate == "false") { // For kinematics only simultion
00171               xmlTextWriterWriteProperty(writer, "inport", "basePoseRef:"+body->rootLink()->name+":ABS_TRANSFORM");
00172           }
00173         } else {
00174           xmlTextWriterWriteProperty(writer, "inport", "tauRef:JOINT_TORQUE");
00175         }
00176         xmlTextWriterWriteProperty(writer, "outport", "q:JOINT_VALUE");
00177     xmlTextWriterWriteProperty(writer, "outport", "tau:JOINT_TORQUE");
00178 
00179     // set outport for sensros
00180     unsigned int nforce = body->numSensors(hrp::Sensor::FORCE);
00181     if ( nforce > 0 ) std::cerr << "hrp::Sensor::FORCE";
00182     for (unsigned int i=0; i<nforce; i++){
00183         hrp::Sensor *s = body->sensor(hrp::Sensor::FORCE, i);
00184         // port name and sensor name is same in case of ForceSensor
00185         xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":FORCE_SENSOR");
00186         std::cerr << " " << s->name;
00187     }
00188     if ( nforce > 0 ) std::cerr << std::endl;
00189     unsigned int ngyro = body->numSensors(hrp::Sensor::RATE_GYRO);
00190     if ( ngyro > 0 ) std::cerr << "hrp::Sensor::GYRO";
00191     if(ngyro == 1){
00192       // port is named with no number when there is only one gyro
00193       hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, 0);
00194       xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":RATE_GYRO_SENSOR");
00195       std::cerr << " " << s->name;
00196     }else{
00197       for (unsigned int i=0; i<ngyro; i++){
00198         hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, i);
00199         std::stringstream str_strm;
00200         str_strm << s->name << i << ":" + s->name << ":RATE_GYRO_SENSOR";
00201         xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
00202         std::cerr << " " << s->name;
00203       }
00204     }
00205     if ( ngyro > 0 ) std::cerr << std::endl;
00206     unsigned int nacc = body->numSensors(hrp::Sensor::ACCELERATION);
00207     if ( nacc > 0 ) std::cerr << "hrp::Sensor::ACCELERATION";
00208     if(nacc == 1){
00209       // port is named with no number when there is only one acc
00210       hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, 0);      
00211       xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":ACCELERATION_SENSOR");
00212       std::cerr << " " << s->name;
00213     }else{
00214       for (unsigned int i=0; i<nacc; i++){
00215         hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, i);
00216         std::stringstream str_strm;
00217         str_strm << s->name << i << ":" << s->name << ":ACCELERATION_SENSOR";
00218         xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
00219         std::cerr << " " << s->name;
00220       }
00221     }
00222     if ( nacc > 0 ) std::cerr << std::endl;
00223     
00224         //
00225         std::string root_name = body->rootLink()->name;
00226         xmlTextWriterWriteProperty(writer, root_name+".NumOfAABB", "1");
00227 
00228         // write waist pos and rot by considering both VRML original WAIST and WAIST_offset_pos and WAIST_offset_rot from arguments
00229         std::ostringstream os;
00230         os << body->rootLink()->p[0] + WAIST_offset_pos(0) << "  "
00231            << body->rootLink()->p[1] + WAIST_offset_pos(1) << "  "
00232            << body->rootLink()->p[2] + WAIST_offset_pos(2); // 10cm margin
00233         xmlTextWriterWriteProperty(writer, root_name+".translation", os.str());
00234         os.str(""); // reset ostringstream
00235         Eigen::AngleAxis<double> tmpAA = Eigen::AngleAxis<double>(hrp::Matrix33(body->rootLink()->R * WAIST_offset_rot.toRotationMatrix()));
00236         os << tmpAA.axis()(0) << " "
00237            << tmpAA.axis()(1) << " "
00238            << tmpAA.axis()(2) << " "
00239            << tmpAA.angle();
00240         xmlTextWriterWriteProperty(writer, root_name+".rotation", os.str());
00241 
00242         if ( ! body->isStaticModel() ) {
00243           xmlTextWriterWriteProperty(writer, root_name+".mode", "Torque");
00244           xmlTextWriterWriteProperty(writer, "controller", basename(output));
00245         }
00246 
00247         // store joint properties
00248         //   [property 1],[value 1],....
00249         //   ex. --joint-properties RARM_JOINT0.angle,0.0,RARM_JOINT2.mode,HighGain,...
00250         coil::vstring joint_properties_arg_str = coil::split(joint_properties, ",");
00251         std::map <std::string, std::string> joint_properties_map;
00252         for (size_t i = 0; i < joint_properties_arg_str.size()/2; i++) {
00253           joint_properties_map.insert(std::pair<std::string, std::string>(joint_properties_arg_str[i*2], joint_properties_arg_str[i*2+1]));
00254         }
00255         if ( body->numJoints() > 0 ) std::cerr << "hrp::Joint";
00256         for(unsigned int i = 0; i < body->numJoints(); i++){
00257           if ( body->joint(i)->index > 0 ) {
00258             std::cerr << " " << body->joint(i)->name << "(" << body->joint(i)->jointId << ")";
00259             std::string joint_name = body->joint(i)->name;
00260             std::string j_property = joint_name+".angle";
00261             xmlTextWriterWriteProperty(writer, j_property,
00262                                        (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "0.0");
00263             j_property = joint_name+".mode";
00264             xmlTextWriterWriteProperty(writer, j_property,
00265                                        (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : (use_highgain_mode?"HighGain":"Torque"));
00266             j_property = joint_name+".NumOfAABB";
00267             xmlTextWriterWriteProperty(writer, j_property,
00268                                        (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "1");
00269           }
00270         }
00271         xmlTextWriterEndElement(writer); // item
00272         if ( body->numJoints() > 0 ) std::cerr << std::endl;
00273 
00274         //
00275         // comment out self collision settings according to issues at https://github.com/fkanehiro/hrpsys-base/issues/122
00276         // xmlTextWriterStartElement(writer, BAD_CAST "item");
00277         // xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
00278         // xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name).c_str());
00279         // xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
00280         // {
00281         //   xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
00282         //   xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
00283         //   xmlTextWriterWriteProperty(writer, "jointName2", "");
00284         //   xmlTextWriterWriteProperty(writer, "jointName1", "");
00285         //   xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
00286         //   xmlTextWriterWriteProperty(writer, "objectName2", name);
00287         //   xmlTextWriterWriteProperty(writer, "objectName1", name);
00288         //   xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
00289         //   xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
00290         // }
00291         // xmlTextWriterEndElement(writer); // item
00292 
00293         xmlTextWriterStartElement(writer, BAD_CAST "view");
00294         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.GrxRobotHardwareClientView");
00295         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "RobotHardware RTC Client");
00296         xmlTextWriterWriteProperty(writer, "robotHost", "localhost");
00297         xmlTextWriterWriteProperty(writer, "StateHolderRTC", "StateHolder0");
00298         xmlTextWriterWriteProperty(writer, "interval", "100");
00299         xmlTextWriterWriteProperty(writer, "RobotHardwareServiceRTC", "RobotHardware0");
00300         xmlTextWriterWriteProperty(writer, "robotPort", "2809");
00301         xmlTextWriterWriteProperty(writer, "ROBOT", name.c_str());
00302         xmlTextWriterEndElement(writer); // item
00303 
00304         xmlTextWriterStartElement(writer, BAD_CAST "view");
00305         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.Grx3DView");
00306         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "3DView");
00307         xmlTextWriterWriteProperty(writer, "view.mode", "Room");
00308         xmlTextWriterWriteProperty(writer, "showCoM", "false");
00309         xmlTextWriterWriteProperty(writer, "showCoMonFloor", "false");
00310         xmlTextWriterWriteProperty(writer, "showDistance", "false");
00311         xmlTextWriterWriteProperty(writer, "showIntersection", "false");
00312         xmlTextWriterWriteProperty(writer, "eyeHomePosition", "-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 ");
00313         xmlTextWriterWriteProperty(writer, "showCollision", "true");
00314         xmlTextWriterWriteProperty(writer, "showActualState", "true");
00315         xmlTextWriterWriteProperty(writer, "showScale", "true");
00316         xmlTextWriterEndElement(writer); // view
00317       }
00318 
00319       {
00320         for (std::vector<std::string>::iterator it1 = inputs.begin(); it1 != inputs.end(); it1++) {
00321           std::string name1 = basename(*it1);
00322           for (std::vector<std::string>::iterator it2 = it1+1; it2 != inputs.end(); it2++) {
00323             std::string name2 = basename(*it2);
00324 
00325             xmlTextWriterStartElement(writer, BAD_CAST "item");
00326             xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
00327             xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name2+"_#"+name1+"_").c_str());
00328             {
00329               xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
00330               xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
00331               xmlTextWriterWriteProperty(writer, "jointName2", "");
00332               xmlTextWriterWriteProperty(writer, "jointName1", "");
00333               xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
00334               xmlTextWriterWriteProperty(writer, "objectName2", name2);
00335               xmlTextWriterWriteProperty(writer, "objectName1", name1);
00336               xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
00337               xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
00338             }
00339             xmlTextWriterEndElement(writer); // item
00340           }
00341         }
00342       }
00343     }
00344     xmlTextWriterEndElement(writer); // mode
00345   }
00346   xmlTextWriterEndElement(writer); // grxui
00347 
00348   xmlFreeTextWriter(writer);
00349 
00350   {
00351       std::string conf_file = output.substr(0,output.find_last_of('.'))+".conf";
00352       std::fstream s(conf_file.c_str(), std::ios::out);
00353   
00354       s << "model: file://" << filenames[0] << std::endl;
00355       s << "dt: " << dt << std::endl;
00356       s << conf_file_option << std::endl;
00357   }
00358 
00359   {
00360       std::string conf_file = output.substr(0,output.find_last_of('.'))+".RobotHardware.conf";
00361       std::fstream s(conf_file.c_str(), std::ios::out);
00362   
00363       s << "model: file://" << filenames[0] << std::endl;
00364       s << "exec_cxt.periodic.type: hrpExecutionContext" << std::endl;
00365       s << "exec_cxt.periodic.rate: " << static_cast<size_t>(1/atof(dt.c_str())+0.5) << std::endl; // rounding to specify integer rate value
00366       s << robothardware_conf_file_option << std::endl;
00367   }
00368   std::cerr << "\033[1;31mProjectGenerator in hrpsys-base is old, so please use projectGenerator in openhrp3.\033[0m" << std::endl;
00369   return 0;
00370 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:18