ODE_ModelLoaderUtil.cpp
Go to the documentation of this file.
00001 #include "ODE_ModelLoaderUtil.h"
00002 #include <stack>
00003 
00004 using namespace hrp;
00005 using namespace OpenHRP;
00006 
00007 namespace ODESim
00008 {
00009     class ModelLoaderHelper
00010     {
00011     public:
00012         bool createBody(BodyPtr body, ODE_World* worldId, BodyInfo_ptr bodyInfo);
00013         
00014     private:
00015         BodyPtr body;
00016         dWorldID worldId;
00017         dSpaceID spaceId;
00018         LinkInfoSequence_var linkInfoSeq;
00019         ShapeInfoSequence_var shapeInfoSeq;
00020 
00021         ODE_Link* createLink(int index, dBodyID parentBodyId, const Matrix44& parentT);
00022         void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs);
00023         void createGeometry(ODE_Link* link, const LinkInfo& linkInfo);
00024         void addLinkVerticesAndTriangles(ODE_Link* link, const LinkInfo& linkInfo);
00025     };
00026 };
00027 
00028     inline double getLimitValue(DblSequence limitseq, double defaultValue)
00029     {
00030         return (limitseq.length() == 0) ? defaultValue : limitseq[0];
00031     }
00032     
00033 
00034 using namespace ODESim;
00035 using namespace std;
00036 bool ModelLoaderHelper::createBody(BodyPtr body, ODE_World* world, BodyInfo_ptr bodyInfo)
00037 {
00038     worldId = world->getWorldID();
00039     spaceId = world->getSpaceID();
00040     this->body = body;
00041     const char* name = bodyInfo->name();
00042     body->setModelName(name);
00043     body->setName(name);
00044 
00045     int n = bodyInfo->links()->length();
00046     linkInfoSeq = bodyInfo->links();
00047     shapeInfoSeq = bodyInfo->shapes();
00048 
00049     int rootIndex = -1;
00050 
00051     for(int i=0; i < n; ++i){
00052         if(linkInfoSeq[i].parentIndex < 0){
00053             if(rootIndex < 0){
00054                 rootIndex = i;
00055             } else {
00056                  // more than one root !
00057                 rootIndex = -1;
00058                 break;
00059             }
00060         }
00061     }
00062 
00063     if(rootIndex >= 0){ // root exists
00064         Matrix44 T(Matrix44::Identity());
00065         ODE_Link* rootLink = createLink(rootIndex, 0, T);
00066         body->setRootLink(rootLink);
00067 
00068         body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
00069 
00070         body->initializeConfiguration();
00071 
00072         return true;
00073     }
00074 
00075     return false;
00076 }
00077 
00078 ODE_Link* ModelLoaderHelper::createLink(int index, dBodyID parentBodyId, const Matrix44& parentT)
00079 {
00080     const LinkInfo& linkInfo = linkInfoSeq[index];
00081     int jointId = linkInfo.jointId;
00082         
00083     ODE_Link* link = new ODE_Link();
00084     dBodyID bodyId = dBodyCreate(worldId);
00085     link->bodyId = bodyId;
00086 
00087     CORBA::String_var name0 = linkInfo.name;
00088     link->name = string( name0 );
00089     link->jointId = jointId;
00090     
00091     Matrix33 parentRs;
00092     parentRs << parentT(0,0), parentT(0,1), parentT(0,2),
00093                 parentT(1,0), parentT(1,1), parentT(1,2),
00094                 parentT(2,0), parentT(2,1), parentT(2,2);
00095     Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
00096     link->b = parentRs * relPos;
00097 
00098     Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
00099     Matrix44 localT;
00100     calcRodrigues(localT, rotAxis,linkInfo.rotation[3]);
00101     localT(0,3) = linkInfo.translation[0];
00102     localT(1,3) = linkInfo.translation[1];
00103     localT(2,3) = linkInfo.translation[2];
00104     Matrix44 T(parentT*localT);
00105     
00106     link->Rs << T(0,0), T(0,1), T(0,2),
00107                 T(1,0), T(1,1), T(1,2),
00108                 T(2,0), T(2,1), T(2,2);
00109     Vector3 p(T(0,3), T(1,3), T(2,3));
00110     const Matrix33& Rs = link->Rs;
00111     link->C = Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
00112 
00113     link->setTransform(p, Rs);
00114 
00115     CORBA::String_var jointType = linkInfo.jointType;
00116     const string jt( jointType );
00117     if(jt == "fixed" ){
00118         link->jointType = ODE_Link::FIXED_JOINT;
00119         dJointID djointId = dJointCreateFixed(worldId, 0);
00120         dJointAttach(djointId, bodyId, 0);
00121         link->odeJointId = djointId;
00122     } else if(jt == "free" ){
00123         link->jointType = ODE_Link::FREE_JOINT;
00124     } else if(jt == "rotate" ){
00125         link->jointType = ODE_Link::ROTATIONAL_JOINT;
00126         dJointID djointId = dJointCreateHinge(worldId, 0);
00127         dJointAttach(djointId, bodyId, parentBodyId);
00128         dJointSetHingeAnchor(djointId, T(0,3), T(1,3), T(2,3));
00129         Vector4 axis( T * Vector4(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2], 0));
00130         dJointSetHingeAxis(djointId, axis(0), axis(1), axis(2));
00131         link->odeJointId = djointId;
00132     } else if(jt == "slide" ){
00133         link->jointType = ODE_Link::SLIDE_JOINT;
00134         dJointID djointId = dJointCreateSlider(worldId, 0);
00135         dJointAttach(djointId, bodyId, parentBodyId);
00136         Vector4 axis( T * Vector4(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2], 0));
00137         dJointSetSliderAxis(djointId, axis(0), axis(1), axis(2));
00138         link->odeJointId = djointId;
00139     } else {
00140         link->jointType = ODE_Link::FREE_JOINT;
00141     }
00142 
00143     link->a.setZero();
00144     link->d.setZero();
00145     Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
00146 
00147     if(link->jointType == Link::ROTATIONAL_JOINT){
00148         link->a = axis;
00149     } else if(link->jointType == Link::SLIDE_JOINT){
00150         link->d = axis;
00151     }
00152 
00153     if(jointId < 0){
00154         if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
00155             cerr << "Warning:  Joint ID is not given to joint " << link->name
00156                       << " of model " << body->modelName() << "." << endl;
00157         }
00158     }
00159 
00160     link->m  = linkInfo.mass;
00161     link->Ir = linkInfo.rotorInertia;
00162 
00163     if(jt != "fixed" ){
00164         dMass mass;
00165         dMassSetZero(&mass);
00166         dMassSetParameters(&mass, linkInfo.mass,
00167             0,0,0,
00168             linkInfo.inertia[0], linkInfo.inertia[4], linkInfo.inertia[8],
00169             linkInfo.inertia[1], linkInfo.inertia[2], linkInfo.inertia[5] );
00170         dBodySetMass(bodyId, &mass);
00171     }
00172 
00173     link->gearRatio     = linkInfo.gearRatio;
00174     link->rotorResistor = linkInfo.rotorResistor;
00175     link->torqueConst   = linkInfo.torqueConst;
00176     link->encoderPulse  = linkInfo.encoderPulse;
00177 
00178     link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
00179 
00180     DblSequence ulimit  = linkInfo.ulimit;
00181     DblSequence llimit  = linkInfo.llimit;
00182     DblSequence uvlimit = linkInfo.uvlimit;
00183     DblSequence lvlimit = linkInfo.lvlimit;
00184 
00185     double maxlimit = (numeric_limits<double>::max)();
00186 
00187     link->ulimit  = getLimitValue(ulimit,  +maxlimit);
00188     link->llimit  = getLimitValue(llimit,  -maxlimit);
00189     link->uvlimit = getLimitValue(uvlimit, +maxlimit);
00190     link->lvlimit = getLimitValue(lvlimit, -maxlimit);
00191 
00192     link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
00193 
00194     Matrix33 Io;
00195     getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
00196     link->I = Rs * Io * Rs.transpose();
00197 
00198     // a stack is used for keeping the same order of children
00199     stack<Link*> children;
00200         
00201     //##### [Changed] Link Structure (convert NaryTree to BinaryTree).
00202     int childNum = linkInfo.childIndices.length();
00203     for(int i = 0 ; i < childNum ; i++) {
00204         int childIndex = linkInfo.childIndices[i];
00205         Link* childLink = createLink(childIndex, bodyId, T);
00206         if(childLink) {
00207             children.push(childLink);
00208         }
00209     }
00210     while(!children.empty()){
00211         link->addChild(children.top());
00212         children.pop();
00213     }
00214         
00215     createSensors(link, linkInfo.sensors, Rs);
00216 
00217     createGeometry(link, linkInfo);
00218 
00219     return link;
00220 }
00221 
00222 void ModelLoaderHelper::createGeometry(ODE_Link* link, const LinkInfo& linkInfo)
00223 {
00224     int totalNumTriangles = 0;
00225     const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
00226     int numofGeom = 0;
00227     for(unsigned int i=0; i < shapeIndices.length(); i++){
00228         const TransformedShapeIndex& tsi = shapeIndices[i];
00229         const DblArray12& M = tsi.transformMatrix;
00230         dMatrix3 R = {  M[0], M[1], M[2],  0,
00231                         M[4], M[5], M[6],  0,
00232                         M[8], M[9], M[10], 0 };
00233         short shapeIndex = shapeIndices[i].shapeIndex;
00234         const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
00235         Matrix33 R0;
00236         R0 << M[0],M[1],M[2],
00237               M[4],M[5],M[6],
00238               M[8],M[9],M[10];
00239         if(isOrthogonalMatrix(R0)){
00240             switch(shapeInfo.primitiveType){
00241                 case OpenHRP::SP_BOX :{
00242                         dReal x = shapeInfo.primitiveParameters[0];
00243                         dReal y = shapeInfo.primitiveParameters[1];
00244                         dReal z = shapeInfo.primitiveParameters[2];
00245                         link->geomIds.push_back(dCreateBox( spaceId, x, y, z));
00246                         dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
00247                         dGeomSetOffsetRotation(link->geomIds.at(numofGeom), R);
00248                         dGeomSetOffsetPosition(link->geomIds.at(numofGeom), M[3]-link->C(0), M[7]-link->C(1), M[11]-link->C(2));
00249                         numofGeom++;
00250                     }
00251                     break;
00252                 case OpenHRP::SP_CYLINDER :{
00253                         dReal radius = shapeInfo.primitiveParameters[0];
00254                         dReal height = shapeInfo.primitiveParameters[1];
00255                         link->geomIds.push_back(dCreateCylinder( spaceId, radius, height));
00256                         dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
00257                         dGeomSetOffsetRotation(link->geomIds.at(numofGeom), R);
00258                         dGeomSetOffsetPosition(link->geomIds.at(numofGeom), M[3]-link->C(0), M[7]-link->C(1), M[11]-link->C(2));
00259                         numofGeom++;
00260                                                            }
00261                     break;
00262                 case OpenHRP::SP_SPHERE :{
00263                         dReal radius = shapeInfo.primitiveParameters[0];
00264                         link->geomIds.push_back(dCreateSphere( spaceId, radius ));
00265                         dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
00266                         dGeomSetOffsetRotation(link->geomIds.at(numofGeom), R);
00267                         dGeomSetOffsetPosition(link->geomIds.at(numofGeom), M[3]-link->C(0), M[7]-link->C(1), M[11]-link->C(2));
00268                         numofGeom++;
00269                                                            }
00270                     break;
00271                 default :
00272                     totalNumTriangles += shapeInfo.triangles.length() / 3;
00273             }
00274         }else
00275             totalNumTriangles += shapeInfo.triangles.length() / 3;
00276     }
00277     int totalNumVertices = totalNumTriangles * 3;
00278 
00279     link->vertices.resize(totalNumVertices*3);
00280     link->indices.resize(totalNumTriangles*3);
00281     addLinkVerticesAndTriangles( link, linkInfo );
00282     if(totalNumTriangles){
00283         link->triMeshDataId = dGeomTriMeshDataCreate();
00284         dGeomTriMeshDataBuildSingle(link->triMeshDataId, &link->vertices[0], 3 * sizeof(dReal), 
00285             totalNumVertices, &link->indices[0], totalNumTriangles*3, 3*sizeof(int));
00286         link->geomIds.push_back( dCreateTriMesh( spaceId, link->triMeshDataId, 0, 0, 0) );
00287         dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
00288         dGeomSetOffsetPosition (link->geomIds.at(numofGeom), -link->C(0), -link->C(1), -link->C(2));
00289     }else{
00290        link->triMeshDataId = 0;
00291     }
00292 }
00293 
00294 void ModelLoaderHelper::addLinkVerticesAndTriangles(ODE_Link* link, const LinkInfo& linkInfo)
00295 {
00296     int vertexIndex = 0;
00297     int triangleIndex = 0;
00298 
00299     const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
00300     
00301     for(unsigned int i=0; i < shapeIndices.length(); i++){
00302         const TransformedShapeIndex& tsi = shapeIndices[i];
00303         short shapeIndex = tsi.shapeIndex;
00304         const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
00305         const DblArray12& M = tsi.transformMatrix;
00306         Matrix33 R0;
00307         R0 << M[0],M[1],M[2],
00308               M[4],M[5],M[6],
00309               M[8],M[9],M[10];
00310         if(isOrthogonalMatrix(R0) && 
00311             (shapeInfo.primitiveType == OpenHRP::SP_BOX ||
00312             shapeInfo.primitiveType == OpenHRP::SP_CYLINDER ||
00313             shapeInfo.primitiveType == OpenHRP::SP_SPHERE ) )
00314             continue;
00315 
00316         Matrix44 T;
00317         T << M[0], M[1], M[2],  M[3],
00318              M[4], M[5], M[6],  M[7],
00319              M[8], M[9], M[10], M[11],
00320              0.0,  0.0,  0.0,   1.0;
00321         const FloatSequence& vertices = shapeInfo.vertices;
00322         const LongSequence& triangles = shapeInfo.triangles;
00323         const int numTriangles = triangles.length() / 3;
00324 
00325         for(int j=0; j < numTriangles; ++j){
00326            int vertexIndexTop = vertexIndex;
00327            for(int k=0; k < 3; ++k){
00328                 long orgVertexIndex = shapeInfo.triangles[j * 3 + k];
00329                 int p = orgVertexIndex * 3;
00330                 Vector4 v(T * Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
00331                 link->vertices[vertexIndex*3] = v[0];
00332                 link->vertices[vertexIndex*3+1] = v[1];
00333                 link->vertices[vertexIndex*3+2] = v[2];
00334                 vertexIndex++;
00335             }
00336             link->indices[triangleIndex++] = vertexIndexTop;
00337             link->indices[triangleIndex++] = vertexIndexTop+1;
00338             link->indices[triangleIndex++] = vertexIndexTop+2;
00339         }
00340     }
00341 }
00342 
00343 void ModelLoaderHelper::createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
00344 {
00345     int numSensors = sensorInfoSeq.length();
00346 
00347     for(int i=0 ; i < numSensors ; ++i ) {
00348         const SensorInfo& sensorInfo = sensorInfoSeq[i];
00349 
00350         int id = sensorInfo.id;
00351         if(id < 0) {
00352             cerr << "Warning:  sensor ID is not given to sensor " << sensorInfo.name
00353                       << "of model " << body->modelName() << "." << endl;
00354         } else {
00355             int sensorType = Sensor::COMMON;
00356 
00357             CORBA::String_var type0 = sensorInfo.type;
00358             string type(type0);
00359 
00360             if(type == "Force")             { sensorType = Sensor::FORCE; }
00361             else if(type == "RateGyro")     { sensorType = Sensor::RATE_GYRO; }
00362             else if(type == "Acceleration")     { sensorType = Sensor::ACCELERATION; }
00363             else if(type == "Vision")       { sensorType = Sensor::VISION; }
00364             else if(type == "Range")        { sensorType = Sensor::RANGE; }
00365 
00366             CORBA::String_var name0 = sensorInfo.name;
00367             string name(name0);
00368 
00369             Sensor* sensor = 0;
00370             if(sensorType == Sensor::FORCE){
00371                 sensor = new ODE_ForceSensor();
00372             }else
00373                 sensor = Sensor::create(sensorType);
00374             if(sensor){
00375                 sensor->id = id;
00376                 sensor->link = link;
00377                 sensor->name = name;
00378                 body->addSensor(sensor, sensorType, id);
00379 
00380                 
00381                 const DblArray3& p = sensorInfo.translation;
00382                 sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
00383 
00384                 const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
00385                 const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
00386                 sensor->localR = Rs * R;
00387 
00388                 if(sensorType == Sensor::FORCE){
00389                     dJointSetFeedback(((ODE_Link*)link)->odeJointId, &((ODE_ForceSensor*)sensor)->feedback);
00390                 }
00391             }
00392             
00393             if ( sensorType == Sensor::RANGE ) {
00394                 RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
00395                 range->scanAngle = sensorInfo.specValues[0];
00396                 range->scanStep = sensorInfo.specValues[1];
00397                 range->scanRate = sensorInfo.specValues[2];
00398                 range->maxDistance = sensorInfo.specValues[3];
00399             } 
00400         }
00401     }
00402 }
00403 
00404 bool ODE_loadBodyFromBodyInfo(BodyPtr body, ODE_World* world, OpenHRP::BodyInfo_ptr bodyInfo)
00405 {
00406     if(!CORBA::is_nil(bodyInfo)){
00407         ODESim::ModelLoaderHelper helper;
00408         return helper.createBody(body, world, bodyInfo);
00409     }
00410     return false;
00411 }


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