ModelLoaderUtil.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00015 #include "ModelLoaderUtil.h"
00016 #include "Link.h"
00017 #include "Sensor.h"
00018 #include "Light.h"
00019 #include <hrpUtil/Eigen3d.h>
00020 #include <hrpUtil/Eigen4d.h>
00021 #include <hrpCorba/OpenHRPCommon.hh>
00022 #include <hrpCorba/ViewSimulator.hh>
00023 #include <hrpCollision/ColdetModel.h>
00024 #include <stack>
00025 
00026 using namespace std;
00027 using namespace hrp;
00028 using namespace OpenHRP;
00029 
00030 
00031 namespace {
00032 
00033     const bool debugMode = false;
00034 
00035     ostream& operator<<(ostream& os, DblSequence_var& data)
00036     {
00037         int size = data->length();
00038         for(int i=0; i < size-1; ++i){
00039             cout << data[i] << ", ";
00040         }
00041         cout << data[size-1];
00042 
00043         return os;
00044     }
00045 
00046 
00047     ostream& operator<<(ostream& os, DblArray3_var& data)
00048     {
00049         cout << data[CORBA::ULong(0)] << ", " << data[CORBA::ULong(1)] << ", " << data[CORBA::ULong(2)];
00050         return os;
00051     }
00052 
00053 
00054     ostream& operator<<(ostream& os, DblArray9_var& data)
00055     {
00056         for(CORBA::ULong i=0; i < 8; ++i){
00057             cout << data[i] << ", ";
00058         }
00059         cout << data[CORBA::ULong(9)];
00060         return os;
00061     }
00062 
00063 
00064     void dumpBodyInfo(BodyInfo_ptr bodyInfo)
00065     {
00066         cout << "<<< BodyInfo >>>\n";
00067 
00068         CORBA::String_var charaName = bodyInfo->name();
00069 
00070         cout << "name: " << charaName << "\n";
00071 
00072         LinkInfoSequence_var linkInfoSeq = bodyInfo->links();
00073 
00074         int numLinks = linkInfoSeq->length();
00075         cout << "num links: " << numLinks << "\n";
00076 
00077         for(int i=0; i < numLinks; ++i){
00078 
00079             const LinkInfo& linkInfo = linkInfoSeq[i];
00080             CORBA::String_var linkName = linkInfo.name;
00081 
00082             cout << "<<< LinkInfo: " << linkName << " (index " << i << ") >>>\n";
00083             cout << "parentIndex: " << linkInfo.parentIndex << "\n";
00084 
00085             const ShortSequence& childIndices = linkInfo.childIndices;
00086             if(childIndices.length() > 0){
00087                 cout << "childIndices: ";
00088                 for(CORBA::ULong i=0; i < childIndices.length(); ++i){
00089                     cout << childIndices[i] << " ";
00090                 }
00091                 cout << "\n";
00092             }
00093 
00094             const SensorInfoSequence& sensorInfoSeq = linkInfo.sensors;
00095 
00096             int numSensors = sensorInfoSeq.length();
00097             cout << "num sensors: " << numSensors << "\n";
00098 
00099             for(int j=0; j < numSensors; ++j){
00100                 cout << "<<< SensorInfo >>>\n";
00101                 const SensorInfo& sensorInfo = sensorInfoSeq[j];
00102                 cout << "id: " << sensorInfo.id << "\n";
00103                 cout << "type: " << sensorInfo.type << "\n";
00104 
00105                 CORBA::String_var sensorName = sensorInfo.name;
00106                 cout << "name: \"" << sensorName << "\"\n";
00107 
00108                 const DblArray3& p = sensorInfo.translation;
00109                 cout << "translation: " << p[0] << ", " << p[1] << ", " << p[2] << "\n";
00110 
00111                 const DblArray4& r = sensorInfo.rotation;
00112                 cout << "rotation: " << r[0] << ", " << r[1] << ", " << r[2] << ", " << r[3] << "\n";
00113                         
00114             }
00115         }
00116 
00117         cout.flush();
00118     }
00119 
00120 
00121     inline double getLimitValue(DblSequence limitseq, double defaultValue)
00122     {
00123         return (limitseq.length() == 0) ? defaultValue : limitseq[0];
00124     }
00125     
00126     static Link *createNewLink() { return new Link(); }
00127     class ModelLoaderHelper
00128     {
00129     public:
00130         ModelLoaderHelper() {
00131             collisionDetectionModelLoading = false;
00132             createLinkFunc = createNewLink;
00133         }
00134 
00135         void enableCollisionDetectionModelLoading(bool isEnabled) {
00136             collisionDetectionModelLoading = isEnabled;
00137         };
00138         void setLinkFactory(Link *(*f)()) { createLinkFunc = f; }
00139 
00140         bool createBody(BodyPtr& body,  BodyInfo_ptr bodyInfo);
00141         
00142     private:
00143         BodyPtr body;
00144         LinkInfoSequence_var linkInfoSeq;
00145         ShapeInfoSequence_var shapeInfoSeq;
00146         ExtraJointInfoSequence_var extraJointInfoSeq;
00147         bool collisionDetectionModelLoading;
00148         Link *(*createLinkFunc)();
00149 
00150         Link* createLink(int index, const Matrix33& parentRs);
00151         void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs);
00152         void createLights(Link* link, const LightInfoSequence& lightInfoSeq, const Matrix33& Rs);
00153         void createColdetModel(Link* link, const LinkInfo& linkInfo);
00154         void addLinkPrimitiveInfo(ColdetModelPtr& coldetModel, 
00155                                   const double *R, const double *p,
00156                                   const ShapeInfo& shapeInfo);
00157         void addLinkVerticesAndTriangles(ColdetModelPtr& coldetModel, const LinkInfo& linkInfo);
00158         void addLinkVerticesAndTriangles(ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, ShapeInfoSequence_var& shapes, int& vertexIndex, int& triangleIndex);
00159                 void setExtraJoints();
00160     };
00161 }
00162 
00163 
00164 bool ModelLoaderHelper::createBody(BodyPtr& body, BodyInfo_ptr bodyInfo)
00165 {
00166     this->body = body;
00167 
00168     const char* name = bodyInfo->name();
00169     body->setModelName(name);
00170     body->setName(name);
00171 
00172     int n = bodyInfo->links()->length();
00173     linkInfoSeq = bodyInfo->links();
00174     shapeInfoSeq = bodyInfo->shapes();
00175         extraJointInfoSeq = bodyInfo->extraJoints();
00176 
00177     int rootIndex = -1;
00178 
00179     for(int i=0; i < n; ++i){
00180         if(linkInfoSeq[i].parentIndex < 0){
00181             if(rootIndex < 0){
00182                 rootIndex = i;
00183             } else {
00184                  // more than one root !
00185                 rootIndex = -1;
00186                 break;
00187             }
00188         }
00189     }
00190 
00191     if(rootIndex >= 0){ // root exists
00192 
00193         Matrix33 Rs(Matrix33::Identity());
00194         Link* rootLink = createLink(rootIndex, Rs);
00195         body->setRootLink(rootLink);
00196         body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
00197 
00198         body->installCustomizer();
00199         body->initializeConfiguration();
00200 
00201                 setExtraJoints();
00202 
00203         return true;
00204     }
00205 
00206     return false;
00207 }
00208 
00209 
00210 Link* ModelLoaderHelper::createLink(int index, const Matrix33& parentRs)
00211 {
00212     const LinkInfo& linkInfo = linkInfoSeq[index];
00213     int jointId = linkInfo.jointId;
00214         
00215     Link* link = (*createLinkFunc)();
00216         
00217     CORBA::String_var name0 = linkInfo.name;
00218     link->name = string( name0 );
00219     link->jointId = jointId;
00220         
00221     Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
00222     link->b = parentRs * relPos;
00223 
00224     Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
00225     Matrix33 R = rodrigues(rotAxis, linkInfo.rotation[3]);
00226     link->Rs = (parentRs * R);
00227     const Matrix33& Rs = link->Rs;
00228 
00229     CORBA::String_var jointType = linkInfo.jointType;
00230     const std::string jt( jointType );
00231 
00232     if(jt == "fixed" ){
00233         link->jointType = Link::FIXED_JOINT;
00234     } else if(jt == "free" ){
00235         link->jointType = Link::FREE_JOINT;
00236     } else if(jt == "rotate" ){
00237         link->jointType = Link::ROTATIONAL_JOINT;
00238     } else if(jt == "slide" ){
00239         link->jointType = Link::SLIDE_JOINT;
00240     } else if(jt == "crawler"){
00241         link->jointType = Link::FIXED_JOINT;
00242         link->isCrawler = true;
00243     } else {
00244         link->jointType = Link::FREE_JOINT;
00245     }
00246 
00247     if(jointId < 0){
00248         if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
00249             std::cerr << "Warning:  Joint ID is not given to joint " << link->name
00250                       << " of model " << body->modelName() << "." << std::endl;
00251         }
00252     }
00253 
00254     link->a.setZero();
00255     link->d.setZero();
00256 
00257     Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
00258 
00259     if(link->jointType == Link::ROTATIONAL_JOINT || jt == "crawler"){
00260         link->a = axis;
00261     } else if(link->jointType == Link::SLIDE_JOINT){
00262         link->d = axis;
00263     }
00264 
00265     link->m  = linkInfo.mass;
00266     link->Ir = linkInfo.rotorInertia;
00267 
00268     link->gearRatio     = linkInfo.gearRatio;
00269     link->rotorResistor = linkInfo.rotorResistor;
00270     link->torqueConst   = linkInfo.torqueConst;
00271     link->encoderPulse  = linkInfo.encoderPulse;
00272 
00273     link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
00274 
00275     DblSequence ulimit  = linkInfo.ulimit;
00276     DblSequence llimit  = linkInfo.llimit;
00277     DblSequence uvlimit = linkInfo.uvlimit;
00278     DblSequence lvlimit = linkInfo.lvlimit;
00279     DblSequence climit = linkInfo.climit;
00280 
00281     double maxlimit = (numeric_limits<double>::max)();
00282 
00283     link->ulimit  = getLimitValue(ulimit,  +maxlimit);
00284     link->llimit  = getLimitValue(llimit,  -maxlimit);
00285     link->uvlimit = getLimitValue(uvlimit, +maxlimit);
00286     link->lvlimit = getLimitValue(lvlimit, -maxlimit);
00287     link->climit  = getLimitValue(climit,  +maxlimit);
00288 
00289     link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
00290 
00291     Matrix33 Io;
00292     getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
00293     link->I = Rs * Io * Rs.transpose();
00294 
00295     // a stack is used for keeping the same order of children
00296     std::stack<Link*> children;
00297         
00298     //##### [Changed] Link Structure (convert NaryTree to BinaryTree).
00299     int childNum = linkInfo.childIndices.length();
00300     for(int i = 0 ; i < childNum ; i++) {
00301         int childIndex = linkInfo.childIndices[i];
00302         Link* childLink = createLink(childIndex, Rs);
00303         if(childLink) {
00304             children.push(childLink);
00305         }
00306     }
00307     while(!children.empty()){
00308         link->addChild(children.top());
00309         children.pop();
00310     }
00311         
00312     createSensors(link, linkInfo.sensors, Rs);
00313     createLights(link, linkInfo.lights, Rs);
00314 
00315     if(collisionDetectionModelLoading){
00316         createColdetModel(link, linkInfo);
00317     }
00318 
00319     return link;
00320 }
00321 
00322 
00323 void ModelLoaderHelper::createLights(Link* link, const LightInfoSequence& lightInfoSeq, const Matrix33& Rs)
00324 {
00325     int numLights = lightInfoSeq.length();
00326 
00327     for(int i=0 ; i < numLights ; ++i ) {
00328         const LightInfo& lightInfo = lightInfoSeq[i];
00329         std::string name(lightInfo.name);
00330         Light *light = body->createLight(link, lightInfo.type, name);
00331         const double *T = lightInfo.transformMatrix;
00332         light->localPos << T[3], T[7], T[11];
00333         light->localR << T[0], T[1], T[2], T[4], T[5], T[6], T[8], T[9], T[10]; 
00334         switch (lightInfo.type){
00335         case Light::POINT:
00336             light->ambientIntensity = lightInfo.ambientIntensity;
00337             getVector3(light->attenuation, lightInfo.attenuation);
00338             getVector3(light->color, lightInfo.color);
00339             light->intensity = lightInfo.intensity;
00340             getVector3(light->location, lightInfo.location);
00341             light->on = lightInfo.on;
00342             light->radius = lightInfo.radius;
00343             break;
00344         case Light::DIRECTIONAL:
00345             light->ambientIntensity = lightInfo.ambientIntensity;
00346             getVector3(light->color, lightInfo.color);
00347             light->intensity = lightInfo.intensity;
00348             light->on = lightInfo.on;
00349             getVector3(light->direction, lightInfo.color); 
00350             break;
00351         case Light::SPOT:
00352             light->ambientIntensity = lightInfo.ambientIntensity;
00353             getVector3(light->attenuation, lightInfo.attenuation);
00354             getVector3(light->color, lightInfo.color);
00355             light->intensity = lightInfo.intensity;
00356             getVector3(light->location, lightInfo.location);
00357             light->on = lightInfo.on;
00358             light->radius = lightInfo.radius;
00359             getVector3(light->direction, lightInfo.direction);
00360             light->beamWidth = lightInfo.beamWidth;
00361             light->cutOffAngle = lightInfo.cutOffAngle;
00362             break;
00363         default:
00364             std::cerr << "unknown light type" << std::endl;
00365         }
00366     }    
00367 }
00368 
00369 void ModelLoaderHelper::createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
00370 {
00371     int numSensors = sensorInfoSeq.length();
00372 
00373     for(int i=0 ; i < numSensors ; ++i ) {
00374         const SensorInfo& sensorInfo = sensorInfoSeq[i];
00375 
00376         int id = sensorInfo.id;
00377         if(id < 0) {
00378             std::cerr << "Warning:  sensor ID is not given to sensor " << sensorInfo.name
00379                       << "of model " << body->modelName() << "." << std::endl;
00380         } else {
00381             int sensorType = Sensor::COMMON;
00382 
00383             CORBA::String_var type0 = sensorInfo.type;
00384             string type(type0);
00385 
00386             if(type == "Force")             { sensorType = Sensor::FORCE; }
00387             else if(type == "RateGyro")     { sensorType = Sensor::RATE_GYRO; }
00388             else if(type == "Acceleration")     { sensorType = Sensor::ACCELERATION; }
00389             else if(type == "Vision")       { sensorType = Sensor::VISION; }
00390             else if(type == "Range")        { sensorType = Sensor::RANGE; }
00391 
00392             CORBA::String_var name0 = sensorInfo.name;
00393             string name(name0);
00394 
00395             Sensor* sensor = body->createSensor(link, sensorType, id, name);
00396 
00397             if(sensor) {
00398                 const DblArray3& p = sensorInfo.translation;
00399                 sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
00400 
00401                 const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
00402                 const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
00403                 sensor->localR = Rs * R;
00404             }
00405             
00406             if ( sensorType == Sensor::RANGE ) {
00407                 RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
00408                 range->scanAngle = sensorInfo.specValues[0];
00409                 range->scanStep = sensorInfo.specValues[1];
00410                 range->scanRate = sensorInfo.specValues[2];
00411                 range->maxDistance = sensorInfo.specValues[3];
00412             }else if (sensorType == Sensor::VISION) {
00413                 VisionSensor *vision = dynamic_cast<VisionSensor *>(sensor);
00414                 vision->near   = sensorInfo.specValues[0];
00415                 vision->far    = sensorInfo.specValues[1];
00416                 vision->fovy   = sensorInfo.specValues[2];
00417                 vision->width  = sensorInfo.specValues[4];
00418                 vision->height = sensorInfo.specValues[5];
00419                 int npixel = vision->width*vision->height;
00420                 switch((int)sensorInfo.specValues[3]){
00421                 case Camera::NONE: 
00422                     vision->imageType = VisionSensor::NONE; 
00423                     break;
00424                 case Camera::COLOR:
00425                     vision->imageType = VisionSensor::COLOR;
00426                     vision->image.resize(npixel*3);
00427                     break;
00428                 case Camera::MONO:
00429                     vision->imageType = VisionSensor::MONO;
00430                     vision->image.resize(npixel);
00431                     break;
00432                 case Camera::DEPTH:
00433                     vision->imageType = VisionSensor::DEPTH;
00434                     break;
00435                 case Camera::COLOR_DEPTH:
00436                     vision->imageType = VisionSensor::COLOR_DEPTH;
00437                     vision->image.resize(npixel*3);
00438                     break;
00439                 case Camera::MONO_DEPTH:
00440                     vision->imageType = VisionSensor::MONO_DEPTH;
00441                     vision->image.resize(npixel);
00442                     break;
00443                 }
00444                 vision->frameRate = sensorInfo.specValues[6];
00445             }
00446         }
00447     }
00448 }
00449 
00450 
00451 void ModelLoaderHelper::createColdetModel(Link* link, const LinkInfo& linkInfo)
00452 {
00453     int totalNumVertices = 0;
00454     int totalNumTriangles = 0;
00455     const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
00456     unsigned int nshape = shapeIndices.length();
00457     short shapeIndex;
00458     double R[9], p[3];
00459     for(unsigned int i=0; i < shapeIndices.length(); i++){
00460         shapeIndex = shapeIndices[i].shapeIndex;
00461         const DblArray12 &tform = shapeIndices[i].transformMatrix;
00462         R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
00463         R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
00464         R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
00465         const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
00466         totalNumVertices += shapeInfo.vertices.length() / 3;
00467         totalNumTriangles += shapeInfo.triangles.length() / 3;
00468     }
00469 
00470     const SensorInfoSequence& sensors = linkInfo.sensors;
00471     for (unsigned int i=0; i<sensors.length(); i++){
00472         const SensorInfo &sinfo = sensors[i];
00473         const TransformedShapeIndexSequence tsis = sinfo.shapeIndices;
00474         nshape += tsis.length();
00475         for (unsigned int j=0; j<tsis.length(); j++){
00476             short shapeIndex = tsis[j].shapeIndex;
00477             const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
00478             totalNumTriangles += shapeInfo.triangles.length() / 3;
00479             totalNumVertices += shapeInfo.vertices.length() / 3 ;
00480         }
00481     }
00482 
00483     ColdetModelPtr coldetModel(new ColdetModel());
00484     coldetModel->setName(std::string(linkInfo.name));
00485     if(totalNumTriangles > 0){
00486         coldetModel->setNumVertices(totalNumVertices);
00487         coldetModel->setNumTriangles(totalNumTriangles);
00488         if (nshape == 1){
00489             addLinkPrimitiveInfo(coldetModel, R, p, shapeInfoSeq[shapeIndex]);
00490         }
00491         addLinkVerticesAndTriangles(coldetModel, linkInfo);
00492         coldetModel->build();
00493     }
00494     link->coldetModel = coldetModel;
00495 }
00496 
00497 void ModelLoaderHelper::addLinkVerticesAndTriangles
00498 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, ShapeInfoSequence_var& shapes, int& vertexIndex, int& triangleIndex)
00499 {
00500     short shapeIndex = tsi.shapeIndex;
00501     const DblArray12& M = tsi.transformMatrix;;
00502     Matrix44 T, Tlocal;
00503     Tlocal << M[0], M[1], M[2],  M[3],
00504              M[4], M[5], M[6],  M[7],
00505              M[8], M[9], M[10], M[11],
00506              0.0,  0.0,  0.0,   1.0;
00507     T = Tparent * Tlocal;
00508     
00509     const ShapeInfo& shapeInfo = shapes[shapeIndex];
00510     int vertexIndexBase = vertexIndex;
00511     const FloatSequence& vertices = shapeInfo.vertices;
00512     const int numVertices = vertices.length() / 3;
00513     for(int j=0; j < numVertices; ++j){
00514         Vector4 v(T * Vector4(vertices[j*3], vertices[j*3+1], vertices[j*3+2], 1.0));
00515         coldetModel->setVertex(vertexIndex++, v[0], v[1], v[2]);
00516     }
00517 
00518     const LongSequence& triangles = shapeInfo.triangles;
00519     const int numTriangles = triangles.length() / 3;
00520     for(int j=0; j < numTriangles; ++j){
00521        int t0 = triangles[j*3] + vertexIndexBase;
00522        int t1 = triangles[j*3+1] + vertexIndexBase;
00523        int t2 = triangles[j*3+2] + vertexIndexBase;
00524        coldetModel->setTriangle( triangleIndex++, t0, t1, t2);
00525     }
00526     
00527 }
00528 
00529 void ModelLoaderHelper::addLinkVerticesAndTriangles(ColdetModelPtr& coldetModel, const LinkInfo& linkInfo)
00530 {
00531     int vertexIndex = 0;
00532     int triangleIndex = 0;
00533 
00534     const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
00535     
00536     Matrix44 E(Matrix44::Identity());
00537     for(unsigned int i=0; i < shapeIndices.length(); i++){
00538         addLinkVerticesAndTriangles(coldetModel, shapeIndices[i], E, shapeInfoSeq,
00539                                     vertexIndex, triangleIndex);
00540     }
00541 
00542     Matrix44 T(Matrix44::Identity());
00543     const SensorInfoSequence& sensors = linkInfo.sensors;
00544     for (unsigned int i=0; i<sensors.length(); i++){
00545         const SensorInfo& sensor = sensors[i]; 
00546         calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1], 
00547                                  sensor.rotation[2]), sensor.rotation[3]);
00548         T(0,3) = sensor.translation[0];
00549         T(1,3) = sensor.translation[1];
00550         T(2,3) = sensor.translation[2];
00551         const TransformedShapeIndexSequence& shapeIndices = sensor.shapeIndices;
00552         for (unsigned int j=0; j<shapeIndices.length(); j++){
00553             addLinkVerticesAndTriangles(coldetModel, shapeIndices[j], T, 
00554                                         shapeInfoSeq,
00555                                         vertexIndex, triangleIndex);
00556         }
00557     }
00558 }
00559 
00560 void ModelLoaderHelper::addLinkPrimitiveInfo(ColdetModelPtr& coldetModel, 
00561                                              const double *R, const double *p,
00562                                              const ShapeInfo& shapeInfo)
00563 {
00564     switch(shapeInfo.primitiveType){
00565     case SP_BOX:
00566         coldetModel->setPrimitiveType(ColdetModel::SP_BOX);
00567         break;
00568     case SP_CYLINDER:
00569         coldetModel->setPrimitiveType(ColdetModel::SP_CYLINDER);
00570         break;
00571     case SP_CONE:
00572         coldetModel->setPrimitiveType(ColdetModel::SP_CONE);
00573         break;
00574     case SP_SPHERE:
00575         coldetModel->setPrimitiveType(ColdetModel::SP_SPHERE);
00576         break;
00577     case SP_PLANE:
00578         coldetModel->setPrimitiveType(ColdetModel::SP_PLANE);
00579         break;
00580     default:
00581         break;
00582     }
00583     coldetModel->setNumPrimitiveParams(shapeInfo.primitiveParameters.length());
00584     for (unsigned int i=0; i<shapeInfo.primitiveParameters.length(); i++){
00585         coldetModel->setPrimitiveParam(i, shapeInfo.primitiveParameters[i]);
00586     }
00587     coldetModel->setPrimitivePosition(R, p);
00588 }
00589 
00590 void ModelLoaderHelper::setExtraJoints()
00591 {
00592     body->extraJoints.clear();
00593     int n = extraJointInfoSeq->length();
00594     
00595     for(int i=0; i < n; ++i){
00596                 const ExtraJointInfo& extraJointInfo = extraJointInfoSeq[i];
00597         Body::ExtraJoint joint;
00598                 joint.name = extraJointInfo.name;
00599                 joint.link[0] = body->link(string(extraJointInfo.link[0]));
00600         joint.link[1] = body->link(string(extraJointInfo.link[1]));
00601 
00602                 ExtraJointType jointType = extraJointInfo.jointType;
00603         if(jointType == OpenHRP::EJ_XY){
00604             joint.type = Body::EJ_XY;   
00605         }else if(jointType == OpenHRP::EJ_XYZ){
00606             joint.type = Body::EJ_XYZ;
00607         }else if(jointType == OpenHRP::EJ_Z){
00608             joint.type = Body::EJ_Z;
00609                 }
00610 
00611                 joint.axis = Vector3(extraJointInfo.axis[0], extraJointInfo.axis[1], extraJointInfo.axis[2] );
00612                 joint.point[0] = Vector3(extraJointInfo.point[0][0], extraJointInfo.point[0][1], extraJointInfo.point[0][2] );
00613                 joint.point[1] = Vector3(extraJointInfo.point[1][0], extraJointInfo.point[1][1], extraJointInfo.point[1][2] );
00614             
00615         body->extraJoints.push_back(joint);
00616     }
00617 }
00618 
00619 bool hrp::loadBodyFromBodyInfo(BodyPtr body, OpenHRP::BodyInfo_ptr bodyInfo, bool loadGeometryForCollisionDetection, Link *(*f)())
00620 {
00621     if(!CORBA::is_nil(bodyInfo)){
00622         ModelLoaderHelper helper;
00623         if (f) helper.setLinkFactory(f);
00624         if(loadGeometryForCollisionDetection){
00625             helper.enableCollisionDetectionModelLoading(true);
00626         }
00627         return helper.createBody(body, bodyInfo);
00628     }
00629     return false;
00630 }
00631 
00632 BodyInfo_var hrp::loadBodyInfo(const char* url, int& argc, char* argv[])
00633 {
00634     CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
00635     return loadBodyInfo(url, orb);
00636 }
00637 
00638 BodyInfo_var hrp::loadBodyInfo(const char* url, CORBA_ORB_var orb)
00639 {
00640     CosNaming::NamingContext_var cxt;
00641     try {
00642         CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00643         cxt = CosNaming::NamingContext::_narrow(nS);
00644     } catch(CORBA::SystemException& ex) {
00645         std::cerr << "NameService doesn't exist" << std::endl;
00646         return BodyInfo::_nil();
00647     }
00648     return loadBodyInfo(url, cxt);
00649 }
00650 
00651 ModelLoader_var hrp::getModelLoader(CORBA_ORB_var orb)
00652 {
00653     CosNaming::NamingContext_var cxt;
00654     try {
00655         CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00656         cxt = CosNaming::NamingContext::_narrow(nS);
00657     } catch(CORBA::SystemException& ex) {
00658         std::cerr << "NameService doesn't exist" << std::endl;
00659         return NULL;
00660     }
00661     return getModelLoader(cxt);
00662 }
00663 
00664 ModelLoader_var hrp::getModelLoader(CosNaming::NamingContext_var cxt)
00665 {
00666     CosNaming::Name ncName;
00667     ncName.length(1);
00668     ncName[0].id = CORBA::string_dup("ModelLoader");
00669     ncName[0].kind = CORBA::string_dup("");
00670     ModelLoader_var modelLoader = NULL;
00671     try {
00672         modelLoader = ModelLoader::_narrow(cxt->resolve(ncName));
00673         modelLoader->_non_existent();
00674     } catch(const CosNaming::NamingContext::NotFound &exc) {
00675         std::cerr << "ModelLoader not found: ";
00676         switch(exc.why) {
00677         case CosNaming::NamingContext::missing_node:
00678             std::cerr << "Missing Node" << std::endl;
00679         case CosNaming::NamingContext::not_context:
00680             std::cerr << "Not Context" << std::endl;
00681             break;
00682         case CosNaming::NamingContext::not_object:
00683             std::cerr << "Not Object" << std::endl;
00684             break;
00685         }
00686         modelLoader = ModelLoader::_nil();
00687     } catch(CosNaming::NamingContext::CannotProceed &exc) {
00688         std::cerr << "Resolve ModelLoader CannotProceed" << std::endl;
00689         modelLoader = ModelLoader::_nil();
00690     } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00691         std::cerr << "Resolve ModelLoader InvalidName" << std::endl;
00692         modelLoader = ModelLoader::_nil();
00693     } catch(...){
00694         modelLoader = ModelLoader::_nil();
00695     }
00696     return modelLoader;
00697 }
00698 
00699 BodyInfo_var hrp::loadBodyInfo(const char* url, CosNaming::NamingContext_var cxt)
00700 {
00701     ModelLoader_var modelLoader = getModelLoader(cxt);
00702 
00703     BodyInfo_var bodyInfo;
00704     try {        
00705         bodyInfo = modelLoader->getBodyInfo(url);
00706     } catch(CORBA::SystemException& ex) {
00707         std::cerr << "CORBA::SystemException raised by ModelLoader: " << ex._rep_id() << std::endl;
00708     } catch(ModelLoader::ModelLoaderException& ex){
00709         std::cerr << "ModelLoaderException : " << ex.description << std::endl;
00710     }
00711     return bodyInfo;
00712 }
00713 
00714 bool hrp::loadBodyFromModelLoader(BodyPtr body, const char* url, CosNaming::NamingContext_var cxt,  bool loadGeometryForCollisionDetection)
00715 {
00716     BodyInfo_var bodyInfo = loadBodyInfo(url, cxt);
00717 
00718     if(!CORBA::is_nil(bodyInfo)){
00719         ModelLoaderHelper helper;
00720         if(loadGeometryForCollisionDetection){
00721             helper.enableCollisionDetectionModelLoading(true);
00722         }
00723         return helper.createBody(body, bodyInfo);
00724     }
00725 
00726     return false;
00727 }
00728 
00729 
00730 bool hrp::loadBodyFromModelLoader(BodyPtr body, const char* url, CORBA_ORB_var orb, bool loadGeometryForCollisionDetection)
00731 {
00732     CosNaming::NamingContext_var cxt;
00733     try {
00734         CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00735         cxt = CosNaming::NamingContext::_narrow(nS);
00736     } catch(CORBA::SystemException& ex) {
00737         std::cerr << "NameService doesn't exist" << std::endl;
00738         return false;
00739     }
00740 
00741     return loadBodyFromModelLoader(body, url, cxt, loadGeometryForCollisionDetection);
00742 }
00743 
00744 
00745 bool hrp::loadBodyFromModelLoader(BodyPtr body, const char* url, int& argc, char* argv[], bool loadGeometryForCollisionDetection)
00746 {
00747     CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
00748     return loadBodyFromModelLoader(body, url,  orb, loadGeometryForCollisionDetection);
00749 }


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