ModelLoaderUtil.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * The University of Tokyo
00009  * National Institute of Advanced Industrial Science and Technology (AIST)
00010  * General Robotix Inc. 
00011  */
00016 #include <hrpCorba/OpenHRPCommon.hh>
00017 #include <stack>
00018 #include "ModelLoaderUtil.h"
00019 #include "Sensor.h"
00020 
00021 #include "World.h"
00022 #include "psim.h"
00023 
00024 using namespace OpenHRP;
00025 using namespace std;
00026 
00027 //#include <fstream>
00028 //static std::ofstream logfile("model.log");
00029 //static std::ofstream logfile;
00030 
00031 static void array_to_mat33(const DblArray4& a, fMat33& mat)
00032 {
00033     const double sth = sin(a[3]);
00034     const double vth = 1.0 - cos(a[3]);
00035 
00036     double ax = a[0];
00037     double ay = a[1];
00038     double az = a[2];
00039 
00040     const double axx = ax*ax*vth;
00041     const double ayy = ay*ay*vth;
00042     const double azz = az*az*vth;
00043     const double axy = ax*ay*vth;
00044     const double ayz = ay*az*vth;
00045     const double azx = az*ax*vth;
00046 
00047     ax *= sth;
00048     ay *= sth;
00049     az *= sth;
00050 
00051     mat(0,0) = 1.0 - azz - ayy;
00052         mat(0,1) =  -az + axy;
00053         mat(0,2) = ay + azx;
00054         mat(1,0) = az + axy;
00055         mat(1,1) = 1.0 - azz - axx;
00056         mat(1,2) = -ax + ayz;
00057         mat(2,0) = -ay + azx;
00058         mat(2,1) = ax + ayz;
00059         mat(2,2) = 1.0 - ayy - axx;
00060 }
00061 
00062 static void array_to_mat33(const DblArray9& a, fMat33& mat)
00063 {
00064         mat(0,0) = a[0];
00065         mat(1,0) = a[1];
00066         mat(2,0) = a[2];
00067         mat(0,1) = a[3];
00068         mat(1,1) = a[4];
00069         mat(2,1) = a[5];
00070         mat(0,2) = a[6];
00071         mat(1,2) = a[7];
00072         mat(2,2) = a[8];
00073 }
00074 
00075 static void array_to_vec3(const DblArray3& a, fVec3& vec)
00076 {
00077         vec(0) = a[0];
00078         vec(1) = a[1];
00079         vec(2) = a[2];
00080 }
00081 
00082 static void createSensors(::World* world, Joint* jnt,  SensorInfoSequence iSensors)
00083 {
00084         int numSensors = iSensors.length();
00085 
00086         for(int i=0; i < numSensors; ++i)
00087         {
00088                 SensorInfo iSensor = iSensors[i];
00089 
00090                 //int id = iSensor->id();
00091                 int id = iSensor.id;
00092 
00093                 if(id < 0){
00094                         std::cerr << "Warning:  sensor ID is not given to sensor " << iSensor.name
00095                                           << "of character " << jnt->CharName() << "." << std::endl;
00096                 } else {
00097 
00098                         int sensorType = Sensor::COMMON;
00099 
00100                         //switch(iSensor.type) {
00101                         //case ::FORCE_SENSOR:          sensorType = Sensor::FORCE;                             break;
00102                         //case ::RATE_GYRO:                     sensorType = Sensor::RATE_GYRO;                 break;
00103                         //case ::ACCELERATION_SENSOR: sensorType = Sensor::ACCELERATION;                break;
00104                         //case ::PRESSURE_SENSOR:               sensorType = Sensor::PRESSURE;                  break;
00105                         //case ::PHOTO_INTERRUPTER:     sensorType = Sensor::PHOTO_INTERRUPTER; break;
00106                         //case ::VISION_SENSOR:         sensorType = Sensor::VISION;                    break;
00107                         //case ::TORQUE_SENSOR:         sensorType = Sensor::TORQUE;                    break;
00108                         //}
00109 
00110                         CORBA::String_var type0 = iSensor.type;
00111                         string type(type0);
00112 
00113                         if( type == "Force" )                           { sensorType = Sensor::FORCE; }                 // 6Ž²ÍEZ¥ó¥µ
00114                         else if( type == "RateGyro" )           { sensorType = Sensor::RATE_GYRO; }             // ¥E¼¥È¥¸¥ã¥¤¥úÁ»¥ó¥µ
00115                         else if( type == "Acceleration" )       { sensorType = Sensor::ACCELERATION; }  // ±E¡¦x¥»¥ó¥µ
00116                         else if( type == "Vision" )                     { sensorType = Sensor::VISION; }                // ¥Ó¥¸¥ç¥ó¥»¥ó¥µ
00117 
00118 
00119                         CORBA::String_var name0 = iSensor.name;
00120                         string name(name0);
00121                         const DblArray3& p = iSensor.translation;
00122                         static fVec3 localPos;
00123                         static fMat33 localR;
00124                         array_to_vec3(p, localPos);
00125                         const DblArray4& rot = iSensor.rotation;
00126                         array_to_mat33(rot, localR);
00127                         world->addSensor(jnt, sensorType, id, name, localPos, localR);
00128                 }
00129         }
00130 }
00131 
00132 static inline double getLimitValue(DblSequence_var limitseq, double defaultValue)
00133 {
00134         return (limitseq->length() == 0) ? defaultValue : limitseq[0];
00135 }
00136 
00137 
00138 static Joint* createLink(::World* world, const char* charname, int index, LinkInfoSequence_var iLinks, Joint* pjoint)
00139 {
00140         Chain* _chain = (Chain*)world->Chain();
00141         LinkInfo iLink = iLinks[index];
00142 
00143 //      logfile << "create: " << iLink->name() << ", jointId = " << iLink->jointId() << endl;
00144         CORBA::String_var name = iLink.name;
00145         std::string myname;
00146         char sep[2];
00147         sep[0] = charname_separator;
00148         sep[1] = '\0';
00149         myname = std::string(name) + std::string(sep) + std::string(charname);
00150         Joint* jnt = new Joint(myname.c_str());
00151 
00152         _chain->AddJoint(jnt, pjoint);
00153 
00154         int jointId = iLink.jointId;
00155         jnt->i_joint = jointId;
00156 
00157         CORBA::String_var jointType = iLink.jointType;
00158         const std::string jt(jointType);
00159 
00160         if(jt == "fixed")
00161         {
00162                 jnt->j_type = ::JFIXED;
00163         }
00164         else if(jt == "free")
00165         {
00166                 jnt->j_type = ::JFREE;
00167         }
00168         else if(jt == "rotate")
00169         {
00170                 jnt->j_type = ::JROTATE;
00171         }
00172         else if(jt == "slide")
00173         {
00174                 jnt->j_type = ::JSLIDE;
00175         }
00176         else
00177         {
00178                 jnt->j_type = ::JFREE;
00179         }
00180 
00181         if(jointId < 0)
00182         {
00183                 if(jnt->j_type == ::JROTATE || jnt->j_type == ::JSLIDE)
00184                 {
00185                         std::cerr << "Warning:  Joint ID is not given to joint " << jnt->name
00186                                           << " of character " << charname << "." << std::endl;
00187                 }
00188         }
00189 
00190         const DblArray3& t =iLink.translation;
00191         static fVec3 rel_pos;
00192         array_to_vec3(t, rel_pos);
00193 
00194         const DblArray4& r = iLink.rotation;
00195         static fMat33 rel_att;
00196         array_to_mat33(r, rel_att);
00197 
00198         // joint axis is always set to z axis; use init_att as the origin
00199         // of the joint axis
00200         if(jnt->j_type == ::JROTATE || jnt->j_type == ::JSLIDE)
00201         {
00202                 const DblArray3& a = iLink.jointAxis;
00203                 static fVec3 loc_axis;
00204                 array_to_vec3(a, loc_axis);
00205 //              logfile << "loc_axis = " << loc_axis << endl;
00206 //              logfile << "rel_att = " << rel_att << endl;
00207 //              logfile << "rel_pos = " << rel_pos << endl;
00208 #if 0
00209                 static fMat33 init_att;
00210                 static fVec3 p_axis;
00211                 p_axis.mul(rel_att, loc_axis);  // joint axis in parent frame -> z axis
00212                 static fVec3 x, y;
00213                 x.set(1.0, 0.0, 0.0);
00214                 y.set(0.0, 1.0, 0.0);
00215                 double zx = p_axis*x;
00216                 x -= zx * p_axis;
00217                 double xlen = x.length();
00218                 if(xlen > 1e-8)
00219                 {
00220                         x /= xlen;
00221                         y.cross(p_axis, x);
00222                 }
00223                 else
00224                 {
00225                         double yz = y*p_axis;
00226                         y -= yz * p_axis;
00227                         double ylen = y.length();
00228                         y /= ylen;
00229                         x.cross(y, p_axis);
00230                 }
00231                 init_att(0,0) = x(0); init_att(1,0) = x(1); init_att(2,0) = x(2);
00232                 init_att(0,1) = y(0); init_att(1,1) = y(1); init_att(2,1) = y(2);
00233                 init_att(0,2) = p_axis(0); init_att(1,2) = p_axis(1); init_att(2,2) = p_axis(2);
00234                 if(jnt->j_type == JROTATE)
00235                         jnt->SetRotateJointType(rel_pos, init_att, AXIS_Z);
00236                 else if(jnt->j_type == JSLIDE)
00237                         jnt->SetSlideJointType(rel_pos, init_att, AXIS_Z);
00238 //              logfile << "init_att = " << init_att << endl;
00239 #else
00240                 AxisIndex axis = AXIS_NULL;
00241                 if(loc_axis(0) > 0.95) axis = AXIS_X;
00242                 else if(loc_axis(1) > 0.95) axis = AXIS_Y;
00243                 else if(loc_axis(2) > 0.95) axis = AXIS_Z;
00244         else{ 
00245             std::cerr << "unsupported joint axis for " << myname
00246                       << ", only X, Y and Z axes are supported" << std::endl;
00247         }
00248                 assert(axis != AXIS_NULL);
00249                 if(jnt->j_type == JROTATE)
00250                         jnt->SetRotateJointType(rel_pos, rel_att, axis);
00251                 else if(jnt->j_type == JSLIDE)
00252                         jnt->SetSlideJointType(rel_pos, rel_att, axis);
00253 #endif
00254 //              logfile << "n_dof = " << jnt->n_dof << endl;
00255         }
00256         else if(jnt->j_type == ::JSPHERE)
00257         {
00258                 jnt->SetSphereJointType(rel_pos, rel_att);
00259         }
00260         else if(jnt->j_type == ::JFIXED)
00261         {
00262                 jnt->SetFixedJointType(rel_pos, rel_att);
00263         }
00264         else if(jnt->j_type == ::JFREE)
00265         {
00266 //              logfile << "rel_pos = " << rel_pos << endl;
00267 //              logfile << "rel_att = " << rel_att << endl;
00268                 jnt->SetFreeJointType(rel_pos, rel_att);
00269         }
00270         
00271         jnt->mass = iLink.mass;
00272 
00273         //double equivalentInertia = iLink.equivalentInertia();
00274 
00275         //if(equivalentInertia == 0.0){
00276         //      jnt->rotor_inertia = iLink.rotorInertia;
00277         //      jnt->gear_ratio = iLink.gearRatio;
00278         //} else {
00279         //      //jnt->rotor_inertia = equivalentInertia;
00280         //      jnt->gear_ratio = 1.0;
00281         //}
00282                 
00283         //link->Jm2             = iLink.equivalentInertia();
00284         //link->torqueConst     = iLink.torqueConst();
00285         //if (link->Jm2 == 0){
00286         //      link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
00287         //}
00288         //link->encoderPulse    = iLink.encoderPulse();
00289 
00290         //DblSequence_var ulimit  = iLink.ulimit();
00291         //DblSequence_var llimit  = iLink.llimit();
00292         //DblSequence_var uvlimit = iLink.uvlimit();
00293         //DblSequence_var lvlimit = iLink.lvlimit();
00294 
00295         //double maxlimit = numeric_limits<double>::max();
00296 
00297         //link->ulimit  = getLimitValue(ulimit,  +maxlimit);
00298         //link->llimit  = getLimitValue(llimit,  -maxlimit);
00299         //link->uvlimit = getLimitValue(uvlimit, +maxlimit);
00300         //link->lvlimit = getLimitValue(lvlimit, -maxlimit);
00301 
00302         const DblArray3& rc = iLink.centerOfMass;
00303         static fVec3 com;
00304         array_to_vec3(rc, com);
00305         jnt->loc_com.set(com);
00306 
00307         const DblArray9& I = iLink.inertia;
00308         static fMat33 inertia;
00309         array_to_mat33(I, inertia);
00310         jnt->inertia.set(inertia);
00311 
00312         //int sindex = iLinks[index].sister();
00313         //      createLink(world, charname, sindex, iLinks, pjoint);
00314 
00315         for( unsigned int i = 0 ; i < iLinks[index].childIndices.length() ; i++ ) 
00316         {
00317                 if( 0 <= iLinks[index].childIndices[i] )
00318                 {
00319                         createLink(world, charname, iLinks[index].childIndices[i], iLinks, jnt);
00320                 }
00321         }
00322 
00323         createSensors(world, jnt, iLink.sensors);
00324 
00325         return jnt;
00326 }
00327 
00328 
00329 int loadBodyFromBodyInfo(::World* world, const char* _name, BodyInfo_ptr bodyInfo)
00330 {
00331 //      logfile << "loadBody(" << _name << ")" << endl;
00332         pSim* _chain = world->Chain();
00333         _chain->BeginCreateChain(true);
00334         // no root
00335         if(!_chain->Root())
00336         {
00337                 _chain->AddRoot("space");
00338         }
00339 
00340         CORBA::String_var name = _name;
00341 
00342         int n = bodyInfo->links()->length();
00343         LinkInfoSequence_var iLinks = bodyInfo->links();
00344         int failed = true;
00345 
00346         for(int i=0; i < n; ++i)
00347         {
00348                 if(iLinks[i].parentIndex < 0)  // root of the character
00349                 {
00350                         static fMat33 Rs;
00351                         Rs.identity();
00352                         Joint* r = createLink(world, name, i, iLinks, _chain->Root());
00353                         world->addCharacter(r, _name, bodyInfo->links());
00354                         failed = false;
00355                         break;
00356                 }
00357         }
00358 #if 0
00359         int rootIndex = -1;
00360         if(body){
00361                 matrix33 Rs(tvmet::identity<matrix33>());
00362                 Link* rootLink = createLink(body, rootIndex, iLinks, Rs, importLinkShape);
00363                 body->setRootLink(rootLink);
00364 
00365                 DblArray3_var p = iLinks[rootIndex]->translation();
00366                 vector3 pos(p[0u], p[1u], p[2u]);
00367                 const DblArray4 &R = iLinks[rootIndex]->rotation();
00368                 matrix33 att;
00369                 getMatrix33FromRowMajorArray(att, R);
00370                 body->setDefaultRootPosition(pos, att);
00371 
00372                 body->installCustomizer();
00373 
00374                 body->initializeConfiguration();
00375         }
00376 #endif
00377         _chain->EndCreateChain();
00378 //      logfile << "end of loadBody" << endl;
00379 //      logfile << "total dof = " << _chain->NumDOF() << endl;
00380         return failed;
00381 }
00382 
00383 
00384 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, CosNaming::NamingContext_var cxt)
00385 {
00386     CosNaming::Name ncName;
00387     ncName.length(1);
00388     ncName[0].id = CORBA::string_dup("ModelLoader");
00389     ncName[0].kind = CORBA::string_dup("");
00390     ModelLoader_var modelLoader = NULL;
00391     try {
00392         modelLoader = ModelLoader::_narrow(cxt->resolve(ncName));
00393     } catch(const CosNaming::NamingContext::NotFound &exc) {
00394         std::cerr << "ModelLoader not found: ";
00395         switch(exc.why) {
00396         case CosNaming::NamingContext::missing_node:
00397             std::cerr << "Missing Node" << std::endl;
00398         case CosNaming::NamingContext::not_context:
00399             std::cerr << "Not Context" << std::endl;
00400             break;
00401         case CosNaming::NamingContext::not_object:
00402             std::cerr << "Not Object" << std::endl;
00403             break;
00404         }
00405         return 0;
00406     } catch(CosNaming::NamingContext::CannotProceed &exc) {
00407         std::cerr << "Resolve ModelLoader CannotProceed" << std::endl;
00408     } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00409         std::cerr << "Resolve ModelLoader InvalidName" << std::endl;
00410     }
00411 
00412         BodyInfo_var bodyInfo;
00413         try {
00414                 bodyInfo = modelLoader->getBodyInfo(url);
00415         } catch(CORBA::SystemException& ex) {
00416                 std::cerr << "CORBA::SystemException raised by ModelLoader: " << ex._rep_id() << std::endl;
00417                 return 0;
00418         } catch(ModelLoader::ModelLoaderException& ex){
00419                 std::cerr << "ModelLoaderException ( " << ex.description << ") : " << ex.description << std::endl;
00420         }
00421 
00422         if(CORBA::is_nil(bodyInfo)){
00423                 return 0;
00424         }
00425 
00426         return loadBodyFromBodyInfo(world, name, bodyInfo);
00427 }
00428 
00429 
00430 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, CORBA_ORB_var orb)
00431 {
00432     CosNaming::NamingContext_var cxt;
00433     try {
00434       CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00435       cxt = CosNaming::NamingContext::_narrow(nS);
00436     } catch(CORBA::SystemException& ex) {
00437                 std::cerr << "NameService doesn't exist" << std::endl;
00438                 return 0;
00439         }
00440 
00441         return loadBodyFromModelLoader(world, name, url, cxt);
00442 }
00443 
00444 
00445 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, int argc, char *argv[])
00446 {
00447     CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
00448     return loadBodyFromModelLoader(world, name, url, orb);
00449 }
00450 
00451 
00452 int loadBodyFromModelLoader(::World* world, const char* name, const char *URL, istringstream &strm)
00453 {
00454     vector<string> argvec;
00455     while (!strm.eof()){
00456         string arg;
00457         strm >> arg;
00458         argvec.push_back(arg);
00459     }
00460     int argc = argvec.size();
00461     char **argv = new char *[argc];
00462     for (int i=0; i<argc; i++){
00463         argv[i] = (char *)argvec[i].c_str();
00464     }
00465 
00466         int ret = loadBodyFromModelLoader(world, name, URL, argc, argv);
00467 
00468     delete [] argv;
00469 
00470     return ret;
00471 }


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