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
00057 rootIndex = -1;
00058 break;
00059 }
00060 }
00061 }
00062
00063 if(rootIndex >= 0){
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
00199 stack<Link*> children;
00200
00201
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 }