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 #include <stack>
00022 #include "BodyInfo_impl.h"
00023 #include <sys/stat.h>
00024 
00025 using namespace std;
00026 void xmlTextWriterWriteProperty(const xmlTextWriterPtr writer, const std::string name, const std::string value) {
00027   xmlTextWriterStartElement(writer, BAD_CAST "property");
00028   xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
00029   xmlTextWriterWriteAttribute(writer, BAD_CAST "value", BAD_CAST value.c_str());
00030   xmlTextWriterEndElement(writer);
00031 }
00032 
00033 std::string basename(const std::string name){
00034   std::string ret = name.substr(name.find_last_of('/')+1);
00035   return ret.substr(0,ret.find_last_of('.'));
00036 }
00037 
00038 // COPY FROM openhrp3/hrplib/hrpModel/ModelLoaderUtil.cpp
00039 class ModelLoaderHelper2
00040 {
00041 public:
00042   ModelLoaderHelper2() {
00043   }
00044 
00045   void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
00046   {
00047     int numSensors = sensorInfoSeq.length();
00048 
00049     for(int i=0 ; i < numSensors ; ++i ) {
00050         const SensorInfo& sensorInfo = sensorInfoSeq[i];
00051 
00052         int id = sensorInfo.id;
00053         if(id < 0) {
00054             std::cerr << "Warning:  sensor ID is not given to sensor " << sensorInfo.name
00055                       << "of model " << body->modelName() << "." << std::endl;
00056         } else {
00057             int sensorType = Sensor::COMMON;
00058 
00059             CORBA::String_var type0 = sensorInfo.type;
00060             string type(type0);
00061 
00062             if(type == "Force")             { sensorType = Sensor::FORCE; }
00063             else if(type == "RateGyro")     { sensorType = Sensor::RATE_GYRO; }
00064             else if(type == "Acceleration")     { sensorType = Sensor::ACCELERATION; }
00065             else if(type == "Vision")       { sensorType = Sensor::VISION; }
00066             else if(type == "Range")        { sensorType = Sensor::RANGE; }
00067 
00068             CORBA::String_var name0 = sensorInfo.name;
00069             string name(name0);
00070 
00071             Sensor* sensor = body->createSensor(link, sensorType, id, name);
00072 
00073             if(sensor) {
00074                 const DblArray3& p = sensorInfo.translation;
00075                 sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
00076 
00077                 const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
00078                 const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
00079                 sensor->localR = Rs * R;
00080             }
00081 #if 0
00082             if ( sensorType == Sensor::RANGE ) {
00083                 RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
00084                 range->scanAngle = sensorInfo.specValues[0];
00085                 range->scanStep = sensorInfo.specValues[1];
00086                 range->scanRate = sensorInfo.specValues[2];
00087                 range->maxDistance = sensorInfo.specValues[3];
00088             }else if (sensorType == Sensor::VISION) {
00089                 VisionSensor *vision = dynamic_cast<VisionSensor *>(sensor);
00090                 vision->near   = sensorInfo.specValues[0];
00091                 vision->far    = sensorInfo.specValues[1];
00092                 vision->fovy   = sensorInfo.specValues[2];
00093                 vision->width  = sensorInfo.specValues[4];
00094                 vision->height = sensorInfo.specValues[5];
00095                 int npixel = vision->width*vision->height;
00096                 switch((int)sensorInfo.specValues[3]){
00097                 case Camera::NONE: 
00098                     vision->imageType = VisionSensor::NONE; 
00099                     break;
00100                 case Camera::COLOR:
00101                     vision->imageType = VisionSensor::COLOR;
00102                     vision->image.resize(npixel*3);
00103                     break;
00104                 case Camera::MONO:
00105                     vision->imageType = VisionSensor::MONO;
00106                     vision->image.resize(npixel);
00107                     break;
00108                 case Camera::DEPTH:
00109                     vision->imageType = VisionSensor::DEPTH;
00110                     break;
00111                 case Camera::COLOR_DEPTH:
00112                     vision->imageType = VisionSensor::COLOR_DEPTH;
00113                     vision->image.resize(npixel*3);
00114                     break;
00115                 case Camera::MONO_DEPTH:
00116                     vision->imageType = VisionSensor::MONO_DEPTH;
00117                     vision->image.resize(npixel);
00118                     break;
00119                 }
00120                 vision->frameRate = sensorInfo.specValues[6];
00121             }
00122 #endif
00123         }
00124     }
00125   }
00126 
00127   inline double getLimitValue(DblSequence limitseq, double defaultValue)
00128   {
00129         return (limitseq.length() == 0) ? defaultValue : limitseq[0];
00130   }
00131 
00132   Link* createLink(int index, const Matrix33& parentRs)
00133   {
00134     const LinkInfo& linkInfo = linkInfoSeq[index];
00135     int jointId = linkInfo.jointId;
00136         
00137     Link* link = new Link(); //(*createLinkFunc)();
00138         
00139     CORBA::String_var name0 = linkInfo.name;
00140     link->name = string( name0 );
00141     link->jointId = jointId;
00142         
00143     Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
00144     link->b = parentRs * relPos;
00145 
00146     Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
00147     Matrix33 R = rodrigues(rotAxis, linkInfo.rotation[3]);
00148     link->Rs = (parentRs * R);
00149     const Matrix33& Rs = link->Rs;
00150 
00151     CORBA::String_var jointType = linkInfo.jointType;
00152     const std::string jt( jointType );
00153 
00154     if(jt == "fixed" ){
00155         link->jointType = Link::FIXED_JOINT;
00156     } else if(jt == "free" ){
00157         link->jointType = Link::FREE_JOINT;
00158     } else if(jt == "rotate" ){
00159         link->jointType = Link::ROTATIONAL_JOINT;
00160     } else if(jt == "slide" ){
00161         link->jointType = Link::SLIDE_JOINT;
00162     } else if(jt == "crawler"){
00163         link->jointType = Link::FIXED_JOINT;
00164         link->isCrawler = true;
00165     } else {
00166         link->jointType = Link::FREE_JOINT;
00167     }
00168 
00169     if(jointId < 0){
00170         if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
00171             std::cerr << "Warning:  Joint ID is not given to joint " << link->name
00172                       << " of model " << body->modelName() << "." << std::endl;
00173         }
00174     }
00175 
00176     link->a.setZero();
00177     link->d.setZero();
00178 
00179     Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
00180 
00181     if(link->jointType == Link::ROTATIONAL_JOINT || jt == "crawler"){
00182         link->a = axis;
00183     } else if(link->jointType == Link::SLIDE_JOINT){
00184         link->d = axis;
00185     }
00186 
00187     link->m  = linkInfo.mass;
00188     link->Ir = linkInfo.rotorInertia;
00189 
00190     link->gearRatio     = linkInfo.gearRatio;
00191     link->rotorResistor = linkInfo.rotorResistor;
00192     link->torqueConst   = linkInfo.torqueConst;
00193     link->encoderPulse  = linkInfo.encoderPulse;
00194 
00195     link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
00196 
00197     DblSequence ulimit  = linkInfo.ulimit;
00198     DblSequence llimit  = linkInfo.llimit;
00199     DblSequence uvlimit = linkInfo.uvlimit;
00200     DblSequence lvlimit = linkInfo.lvlimit;
00201     DblSequence climit = linkInfo.climit;
00202 
00203     double maxlimit = (numeric_limits<double>::max)();
00204 
00205     link->ulimit  = getLimitValue(ulimit,  +maxlimit);
00206     link->llimit  = getLimitValue(llimit,  -maxlimit);
00207     link->uvlimit = getLimitValue(uvlimit, +maxlimit);
00208     link->lvlimit = getLimitValue(lvlimit, -maxlimit);
00209     link->climit  = getLimitValue(climit,  +maxlimit);
00210 
00211     link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
00212 
00213     Matrix33 Io;
00214     getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
00215     link->I = Rs * Io * Rs.transpose();
00216 
00217     // a stack is used for keeping the same order of children
00218     std::stack<Link*> children;
00219         
00220     //##### [Changed] Link Structure (convert NaryTree to BinaryTree).
00221     int childNum = linkInfo.childIndices.length();
00222     for(int i = 0 ; i < childNum ; i++) {
00223         int childIndex = linkInfo.childIndices[i];
00224         Link* childLink = createLink(childIndex, Rs);
00225         if(childLink) {
00226             children.push(childLink);
00227         }
00228     }
00229     while(!children.empty()){
00230         link->addChild(children.top());
00231         children.pop();
00232     }
00233         
00234     createSensors(link, linkInfo.sensors, Rs);
00235     //createLights(link, linkInfo.lights, Rs);
00236 #if 0
00237     if(collisionDetectionModelLoading){
00238         createColdetModel(link, linkInfo);
00239     }
00240 #endif
00241     return link;
00242   }
00243 
00244   bool createBody(BodyPtr& body, BodyInfo_impl& bodyInfo)
00245   {
00246     this->body = body;
00247 
00248     const char* name = bodyInfo.name();
00249     body->setModelName(name);
00250     body->setName(name);
00251 
00252     int n = bodyInfo.links()->length();
00253     linkInfoSeq = bodyInfo.links();
00254     shapeInfoSeq = bodyInfo.shapes();
00255         extraJointInfoSeq = bodyInfo.extraJoints();
00256 
00257     int rootIndex = -1;
00258 
00259     for(int i=0; i < n; ++i){
00260         if(linkInfoSeq[i].parentIndex < 0){
00261             if(rootIndex < 0){
00262                 rootIndex = i;
00263             } else {
00264                  // more than one root !
00265                 rootIndex = -1;
00266                 break;
00267             }
00268         }
00269     }
00270 
00271     if(rootIndex >= 0){ // root exists
00272 
00273         Matrix33 Rs(Matrix33::Identity());
00274         Link* rootLink = createLink(rootIndex, Rs);
00275         body->setRootLink(rootLink);
00276         body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
00277 
00278         body->installCustomizer();
00279         body->initializeConfiguration();
00280 #if 0
00281                 setExtraJoints();
00282 #endif
00283         return true;
00284     }
00285 
00286     return false;
00287   }
00288 private:
00289   BodyPtr body;
00290   LinkInfoSequence_var linkInfoSeq;
00291   ShapeInfoSequence_var shapeInfoSeq;
00292   ExtraJointInfoSequence_var extraJointInfoSeq;
00293 };
00294 
00295 int main (int argc, char** argv)
00296 {
00297   std::string output;
00298   std::vector<std::string> inputs, filenames; // filenames is for conf file
00299   std::string conf_file_option, robothardware_conf_file_option, integrate("true"), dt("0.005"), timeStep(dt), joint_properties, method("EULER");
00300   bool use_highgain_mode(true);
00301   struct stat st;
00302   bool file_exist_flag = false;
00303 
00304   for (int i = 1; i < argc; ++ i) {
00305     std::string arg(argv[i]);
00306     coil::normalize(arg);
00307     if ( arg == "--output" ) {
00308       if (++i < argc) output = argv[i];
00309     } else if ( arg == "--integrate" ) {
00310       if (++i < argc) integrate = argv[i];
00311     } else if ( arg == "--dt" ) {
00312       if (++i < argc) dt = argv[i];
00313     } else if ( arg == "--timestep" ) {
00314       if (++i < argc) timeStep = argv[i];
00315     } else if ( arg == "--conf-file-option" ) {
00316       if (++i < argc) conf_file_option += std::string("\n") + argv[i];
00317     } else if ( arg == "--robothardware-conf-file-option" ) {
00318       if (++i < argc) robothardware_conf_file_option += std::string("\n") + argv[i];
00319     } else if ( arg == "--joint-properties" ) {
00320       if (++i < argc) joint_properties = argv[i];
00321     } else if ( arg == "--use-highgain-mode" ) {
00322       if (++i < argc) use_highgain_mode = (std::string(argv[i])==std::string("true")?true:false);
00323     } else if ( arg == "--method" ) {
00324       if (++i < argc) method = std::string(argv[i]);
00325     } else if ( arg.find("--gtest_output") == 0  ||arg.find("--text") == 0 || arg.find("__log") == 0 || arg.find("__name") == 0 ) { // skip
00326     } else {
00327       inputs.push_back(argv[i]);
00328     }
00329   }
00330 
00331   CORBA::ORB_var orb = CORBA::ORB::_nil();
00332 
00333   try
00334     {
00335       orb = CORBA::ORB_init(argc, argv);
00336 
00337       CORBA::Object_var obj;
00338 
00339       obj = orb->resolve_initial_references("RootPOA");
00340       PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);
00341       if(CORBA::is_nil(poa))
00342         {
00343           throw string("error: failed to narrow root POA.");
00344         }
00345 
00346       PortableServer::POAManager_var poaManager = poa->the_POAManager();
00347       if(CORBA::is_nil(poaManager))
00348         {
00349           throw string("error: failed to narrow root POA manager.");
00350         }
00351 
00352   xmlTextWriterPtr writer;
00353   if (stat(output.c_str(), &st) == 0) {
00354     file_exist_flag = true;
00355   }
00356   writer = xmlNewTextWriterFilename(output.c_str(), 0);
00357   xmlTextWriterSetIndent(writer, 4);
00358   xmlTextWriterStartElement(writer, BAD_CAST "grxui");
00359   {
00360     xmlTextWriterStartElement(writer, BAD_CAST "mode");
00361     xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "Simulation");
00362     {
00363       xmlTextWriterStartElement(writer, BAD_CAST "item");
00364       xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxSimulationItem");
00365       xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "simulationItem");
00366       {
00367         xmlTextWriterWriteProperty(writer, "integrate", integrate);
00368         xmlTextWriterWriteProperty(writer, "timeStep", timeStep);
00369         xmlTextWriterWriteProperty(writer, "totalTime", "2000000.0");
00370         xmlTextWriterWriteProperty(writer, "method", method);
00371       }
00372       xmlTextWriterEndElement(writer); // item
00373       // default WAIST offset
00374       hrp::Vector3 WAIST_offset_pos = hrp::Vector3::Zero();
00375       Eigen::AngleAxis<double> WAIST_offset_rot = Eigen::AngleAxis<double>(0, hrp::Vector3(0,0,1)); // rotation in VRML is represented by axis + angle
00376       for (std::vector<std::string>::iterator it = inputs.begin();
00377            it != inputs.end();
00378            it++) {
00379         // argument for VRML supports following two mode:
00380         //  1. xxx.wrl
00381         //    To specify VRML file. WAIST offsets is 0 transformation.
00382         //  2. xxx.wrl,0,0,0,0,0,1,0
00383         //    To specify both VRML and WAIST offsets.
00384         //    WAIST offset: for example, "0,0,0,0,0,1,0" -> position offset (3dof) + axis for rotation offset (3dof) + angle for rotation offset (1dof)
00385         coil::vstring filename_arg_str = coil::split(*it, ",");
00386         std::string filename = filename_arg_str[0];
00387         filenames.push_back(filename);
00388         if ( filename_arg_str.size () > 1 ){ // if WAIST offset is specified 
00389           for (size_t i = 0; i < 3; i++) {
00390             coil::stringTo(WAIST_offset_pos(i), filename_arg_str[i+1].c_str());
00391             coil::stringTo(WAIST_offset_rot.axis()(i), filename_arg_str[i+1+3].c_str());
00392           }
00393           coil::stringTo(WAIST_offset_rot.angle(), filename_arg_str[1+3+3].c_str());
00394         }
00395         hrp::BodyPtr body(new hrp::Body());
00396 
00397         BodyInfo_impl bI(poa);
00398         bI.loadModelFile(filename.c_str());
00399         ModelLoaderHelper2 helper;
00400         helper.createBody(body, bI);
00401 
00402         std::string name = body->name();
00403 
00404         xmlTextWriterStartElement(writer, BAD_CAST "item");
00405         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxRTSItem");
00406         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
00407         xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
00408 
00409         xmlTextWriterWriteProperty(writer, name+"(Robot)0.period", dt);
00410         if (use_highgain_mode) {
00411           xmlTextWriterWriteProperty(writer, "HGcontroller0.period", dt);
00412           xmlTextWriterWriteProperty(writer, "HGcontroller0.factory", "HGcontroller");
00413           if (it==inputs.begin()) {
00414             xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.qOut:"+name+"(Robot)0.qRef");
00415             xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.dqOut:"+name+"(Robot)0.dqRef");
00416             xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.ddqOut:"+name+"(Robot)0.ddqRef");
00417           }
00418         } else {
00419           xmlTextWriterWriteProperty(writer, "PDcontroller0.period", dt);
00420           xmlTextWriterWriteProperty(writer, "PDcontroller0.factory", "PDcontroller");
00421           if (it==inputs.begin()) {
00422             xmlTextWriterWriteProperty(writer, "connection", "PDcontroller0.torque:"+name+"(Robot)0.tauRef");
00423             xmlTextWriterWriteProperty(writer, "connection", ""+name+"(Robot)0.q:PDcontroller0.angle");
00424           }
00425         }
00426         xmlTextWriterEndElement(writer); // item
00427 
00428 
00429         xmlTextWriterStartElement(writer, BAD_CAST "item");
00430         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxModelItem");
00431         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST (basename(*it).c_str()));
00432         xmlTextWriterWriteAttribute(writer, BAD_CAST "url", BAD_CAST filename.c_str());
00433 
00434         xmlTextWriterWriteProperty(writer, "rtcName", name + "(Robot)0");
00435         if (use_highgain_mode) {
00436           xmlTextWriterWriteProperty(writer, "inport", "qRef:JOINT_VALUE");
00437           xmlTextWriterWriteProperty(writer, "inport", "dqRef:JOINT_VELOCITY");
00438           xmlTextWriterWriteProperty(writer, "inport", "ddqRef:JOINT_ACCELERATION");
00439           if (integrate == "false") { // For kinematics only simultion
00440               xmlTextWriterWriteProperty(writer, "inport", "basePoseRef:"+body->rootLink()->name+":ABS_TRANSFORM");
00441           }
00442         } else {
00443           xmlTextWriterWriteProperty(writer, "inport", "tauRef:JOINT_TORQUE");
00444         }
00445         xmlTextWriterWriteProperty(writer, "outport", "q:JOINT_VALUE");
00446     xmlTextWriterWriteProperty(writer, "outport", "dq:JOINT_VELOCITY");
00447     xmlTextWriterWriteProperty(writer, "outport", "tau:JOINT_TORQUE");
00448     xmlTextWriterWriteProperty(writer, "outport", body->rootLink()->name+":"+body->rootLink()->name+":ABS_TRANSFORM");
00449 
00450     // set outport for sensros
00451     int nforce = body->numSensors(hrp::Sensor::FORCE);
00452     if ( nforce > 0 ) std::cerr << "hrp::Sensor::FORCE";
00453     for (int i=0; i<nforce; i++){
00454         hrp::Sensor *s = body->sensor(hrp::Sensor::FORCE, i);
00455         // port name and sensor name is same in case of ForceSensor
00456         xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":FORCE_SENSOR");
00457         std::cerr << " " << s->name;
00458     }
00459     if ( nforce > 0 ) std::cerr << std::endl;
00460     int ngyro = body->numSensors(hrp::Sensor::RATE_GYRO);
00461     if ( ngyro > 0 ) std::cerr << "hrp::Sensor::GYRO";
00462     if(ngyro == 1){
00463       // port is named with no number when there is only one gyro
00464       hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, 0);
00465       xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":RATE_GYRO_SENSOR");
00466       std::cerr << " " << s->name;
00467     }else{
00468       for (int i=0; i<ngyro; i++){
00469         hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, i);
00470         std::stringstream str_strm;
00471         str_strm << s->name << i << ":" + s->name << ":RATE_GYRO_SENSOR";
00472         xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
00473         std::cerr << " " << s->name;
00474       }
00475     }
00476     if ( ngyro > 0 ) std::cerr << std::endl;
00477     int nacc = body->numSensors(hrp::Sensor::ACCELERATION);
00478     if ( nacc > 0 ) std::cerr << "hrp::Sensor::ACCELERATION";
00479     if(nacc == 1){
00480       // port is named with no number when there is only one acc
00481       hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, 0);      
00482       xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":ACCELERATION_SENSOR");
00483       std::cerr << " " << s->name;
00484     }else{
00485       for (int i=0; i<nacc; i++){
00486         hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, i);
00487         std::stringstream str_strm;
00488         str_strm << s->name << i << ":" << s->name << ":ACCELERATION_SENSOR";
00489         xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
00490         std::cerr << " " << s->name;
00491       }
00492     }
00493     if ( nacc > 0 ) std::cerr << std::endl;
00494     
00495         //
00496         std::string root_name = body->rootLink()->name;
00497         xmlTextWriterWriteProperty(writer, root_name+".NumOfAABB", "1");
00498 
00499         // write waist pos and rot by considering both VRML original WAIST and WAIST_offset_pos and WAIST_offset_rot from arguments
00500         std::ostringstream os;
00501         os << body->rootLink()->p[0] + WAIST_offset_pos(0) << "  "
00502            << body->rootLink()->p[1] + WAIST_offset_pos(1) << "  "
00503            << body->rootLink()->p[2] + WAIST_offset_pos(2); // 10cm margin
00504         xmlTextWriterWriteProperty(writer, root_name+".translation", os.str());
00505         os.str(""); // reset ostringstream
00506         Eigen::AngleAxis<double> tmpAA = Eigen::AngleAxis<double>(hrp::Matrix33(body->rootLink()->R * WAIST_offset_rot.toRotationMatrix()));
00507         os << tmpAA.axis()(0) << " "
00508            << tmpAA.axis()(1) << " "
00509            << tmpAA.axis()(2) << " "
00510            << tmpAA.angle();
00511         xmlTextWriterWriteProperty(writer, root_name+".rotation", os.str());
00512 
00513         if ( ! body->isStaticModel() ) {
00514           xmlTextWriterWriteProperty(writer, root_name+".mode", "Torque");
00515           xmlTextWriterWriteProperty(writer, "controller", basename(output));
00516         }
00517 
00518         // store joint properties
00519         //   [property 1],[value 1],....
00520         //   ex. --joint-properties RARM_JOINT0.angle,0.0,RARM_JOINT2.mode,HighGain,...
00521         coil::vstring joint_properties_arg_str = coil::split(joint_properties, ",");
00522         std::map <std::string, std::string> joint_properties_map;
00523         for (size_t i = 0; i < joint_properties_arg_str.size()/2; i++) {
00524           joint_properties_map.insert(std::pair<std::string, std::string>(joint_properties_arg_str[i*2], joint_properties_arg_str[i*2+1]));
00525         }
00526         if ( body->numJoints() > 0 ) std::cerr << "hrp::Joint";
00527         for(unsigned int i = 0; i < body->numJoints(); i++){
00528           if ( body->joint(i)->index > 0 ) {
00529             std::cerr << " " << body->joint(i)->name << "(" << body->joint(i)->jointId << ")";
00530             std::string joint_name = body->joint(i)->name;
00531             std::string j_property = joint_name+".angle";
00532             xmlTextWriterWriteProperty(writer, j_property,
00533                                        (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "0.0");
00534             j_property = joint_name+".mode";
00535             xmlTextWriterWriteProperty(writer, j_property,
00536                                        (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : (use_highgain_mode?"HighGain":"Torque"));
00537             j_property = joint_name+".NumOfAABB";
00538             xmlTextWriterWriteProperty(writer, j_property,
00539                                        (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "1");
00540           }
00541         }
00542         xmlTextWriterEndElement(writer); // item
00543         if ( body->numJoints() > 0 ) std::cerr << std::endl;
00544 
00545         //
00546         // comment out self collision settings according to issues at https://github.com/fkanehiro/hrpsys-base/issues/122
00547         // xmlTextWriterStartElement(writer, BAD_CAST "item");
00548         // xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
00549         // xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name).c_str());
00550         // xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
00551         // {
00552         //   xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
00553         //   xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
00554         //   xmlTextWriterWriteProperty(writer, "jointName2", "");
00555         //   xmlTextWriterWriteProperty(writer, "jointName1", "");
00556         //   xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
00557         //   xmlTextWriterWriteProperty(writer, "objectName2", name);
00558         //   xmlTextWriterWriteProperty(writer, "objectName1", name);
00559         //   xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
00560         //   xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
00561         // }
00562         // xmlTextWriterEndElement(writer); // item
00563 
00564         xmlTextWriterStartElement(writer, BAD_CAST "view");
00565         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.GrxRobotHardwareClientView");
00566         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "RobotHardware RTC Client");
00567         xmlTextWriterWriteProperty(writer, "robotHost", "localhost");
00568         xmlTextWriterWriteProperty(writer, "StateHolderRTC", "StateHolder0");
00569         xmlTextWriterWriteProperty(writer, "interval", "100");
00570         xmlTextWriterWriteProperty(writer, "RobotHardwareServiceRTC", "RobotHardware0");
00571         xmlTextWriterWriteProperty(writer, "robotPort", "2809");
00572         xmlTextWriterWriteProperty(writer, "ROBOT", name.c_str());
00573         xmlTextWriterEndElement(writer); // item
00574 
00575         xmlTextWriterStartElement(writer, BAD_CAST "view");
00576         xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.Grx3DView");
00577         xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "3DView");
00578         xmlTextWriterWriteProperty(writer, "view.mode", "Room");
00579         xmlTextWriterWriteProperty(writer, "showCoM", "false");
00580         xmlTextWriterWriteProperty(writer, "showCoMonFloor", "false");
00581         xmlTextWriterWriteProperty(writer, "showDistance", "false");
00582         xmlTextWriterWriteProperty(writer, "showIntersection", "false");
00583         xmlTextWriterWriteProperty(writer, "eyeHomePosition", "-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 ");
00584         xmlTextWriterWriteProperty(writer, "showCollision", "true");
00585         xmlTextWriterWriteProperty(writer, "showActualState", "true");
00586         xmlTextWriterWriteProperty(writer, "showScale", "true");
00587         xmlTextWriterEndElement(writer); // view
00588       }
00589 
00590       {
00591         for (std::vector<std::string>::iterator it1 = inputs.begin(); it1 != inputs.end(); it1++) {
00592           std::string name1 = basename(*it1);
00593           for (std::vector<std::string>::iterator it2 = it1+1; it2 != inputs.end(); it2++) {
00594             std::string name2 = basename(*it2);
00595 
00596             xmlTextWriterStartElement(writer, BAD_CAST "item");
00597             xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
00598             xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name2+"_#"+name1+"_").c_str());
00599             {
00600               xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
00601               xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
00602               xmlTextWriterWriteProperty(writer, "jointName2", "");
00603               xmlTextWriterWriteProperty(writer, "jointName1", "");
00604               xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
00605               xmlTextWriterWriteProperty(writer, "objectName2", name2);
00606               xmlTextWriterWriteProperty(writer, "objectName1", name1);
00607               xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
00608               xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
00609             }
00610             xmlTextWriterEndElement(writer); // item
00611           }
00612         }
00613       }
00614     }
00615     xmlTextWriterEndElement(writer); // mode
00616   }
00617   xmlTextWriterEndElement(writer); // grxui
00618 
00619   xmlFreeTextWriter(writer);
00620 
00621   if (file_exist_flag) {
00622     std::cerr << "\033[1;31mOver\033[0m";
00623   }
00624   std::cerr << "Writing project files to .... " << output << std::endl;
00625   {
00626       std::string conf_file = output.substr(0,output.find_last_of('.'))+".conf";
00627       if (stat(conf_file.c_str(), &st) == 0) {
00628         std::cerr << "\033[1;31mOver\033[0m";
00629       }
00630       std::fstream s(conf_file.c_str(), std::ios::out);
00631   
00632       s << "model: file://" << filenames[0] << std::endl;
00633       s << "dt: " << dt << std::endl;
00634       s << conf_file_option << std::endl;
00635       std::cerr << "Writing conf files to ....... " << conf_file << std::endl;
00636   }
00637 
00638   {
00639       std::string conf_file = output.substr(0,output.find_last_of('.'))+".RobotHardware.conf";
00640       if (stat(conf_file.c_str(), &st) == 0) {
00641         std::cerr << "\033[1;31mOver\033[0m";
00642       }
00643       std::fstream s(conf_file.c_str(), std::ios::out);
00644   
00645       s << "model: file://" << filenames[0] << std::endl;
00646       s << "exec_cxt.periodic.type: hrpExecutionContext" << std::endl;
00647       s << "exec_cxt.periodic.rate: " << static_cast<size_t>(1/atof(dt.c_str())+0.5) << std::endl; // rounding to specify integer rate value
00648       s << robothardware_conf_file_option << std::endl;
00649       std::cerr << "Writing hardware files to ... " << conf_file << std::endl;
00650   }
00651 
00652     }
00653   catch (CORBA::SystemException& ex)
00654     {
00655       cerr << ex._rep_id() << endl;
00656     }
00657   catch (const string& error)
00658     {
00659       cerr << error << endl;
00660     }
00661 
00662   try
00663     {
00664       orb->destroy();
00665     }
00666   catch(...)
00667     {
00668 
00669     }
00670 
00671   return 0;
00672 }


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