00001
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
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();
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
00218 std::stack<Link*> children;
00219
00220
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
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
00265 rootIndex = -1;
00266 break;
00267 }
00268 }
00269 }
00270
00271 if(rootIndex >= 0){
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;
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 ) {
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);
00373
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));
00376 for (std::vector<std::string>::iterator it = inputs.begin();
00377 it != inputs.end();
00378 it++) {
00379
00380
00381
00382
00383
00384
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 ){
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);
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") {
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
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
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
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
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
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);
00504 xmlTextWriterWriteProperty(writer, root_name+".translation", os.str());
00505 os.str("");
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
00519
00520
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);
00543 if ( body->numJoints() > 0 ) std::cerr << std::endl;
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
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);
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);
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);
00611 }
00612 }
00613 }
00614 }
00615 xmlTextWriterEndElement(writer);
00616 }
00617 xmlTextWriterEndElement(writer);
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;
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 }