From 5b2a9d1a01ef8a7810e19d87e26274d6f0bbfb5b Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Wed, 8 Mar 2017 14:49:39 +0300 Subject: [PATCH 1/4] URDF loader: resources path, "package://" removal, error messages, zero material path in .obj loader --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 776 ++++++++---------- .../Importers/ImportURDFDemo/UrdfParser.cpp | 13 +- .../Importers/ImportURDFDemo/UrdfParser.h | 12 +- .../Wavefront/tiny_obj_loader.cpp | 10 + 4 files changed, 389 insertions(+), 422 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index dfe49e8b6..3137af5ae 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -32,6 +32,7 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001; #include #include +#include #include "UrdfParser.h" struct MyTexture @@ -47,14 +48,27 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData UrdfParser m_urdfParser; struct GUIHelperInterface* m_guiHelper; + std::string m_sourceFile; char m_pathPrefix[1024]; int m_bodyId; btHashMap m_linkColors; btAlignedObjectArray m_allocatedCollisionShapes; LinkVisualShapesConverter* m_customVisualShapesConverter; -}; + void setSourceFile(const std::string& relativeFileName, const std::string& prefix) + { + m_sourceFile = relativeFileName; + m_urdfParser.setSourceFile(relativeFileName); + strncpy(m_pathPrefix, prefix.c_str(), sizeof(m_pathPrefix)); + m_pathPrefix[sizeof(m_pathPrefix)-1] = 0; // required, strncpy doesn't write zero on overflow + } + + BulletURDFInternalData() + { + m_pathPrefix[0] = 0; + } +}; void BulletURDFImporter::printTree() { @@ -76,7 +90,6 @@ BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVi m_data = new BulletURDFInternalData; m_data->m_guiHelper = helper; - m_data->m_pathPrefix[0]=0; m_data->m_customVisualShapesConverter = customConverter; @@ -111,7 +124,6 @@ struct BulletErrorLogger : public ErrorLogger bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { - if (strlen(fileName)==0) return false; @@ -124,17 +136,16 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0; std::string xml_string; - m_data->m_pathPrefix[0] = 0; - - if (!fileFound){ - std::cerr << "URDF file not found" << std::endl; - return false; - } else - { - - int maxPathLen = 1024; - fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen); + if (!fileFound){ + b3Warning("URDF file '%s' not found\n", fileName); + return false; + } else + { + + char path[1024]; + fu.extractPath(relativeFileName, path, sizeof(path)); + m_data->setSourceFile(relativeFileName, path); std::fstream xml_file(relativeFileName, std::fstream::in); while ( xml_file.good()) @@ -166,7 +177,7 @@ void BulletURDFImporter::activateModel(int modelIndex) bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) { - + //int argc=0; char relativeFileName[1024]; @@ -176,17 +187,16 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0; std::string xml_string; - m_data->m_pathPrefix[0] = 0; if (!fileFound){ - std::cerr << "SDF file not found" << std::endl; + b3Warning("SDF file '%s' not found\n", fileName); return false; } else { - int maxPathLen = 1024; - fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen); - + char path[1024]; + fu.extractPath(relativeFileName, path, sizeof(path)); + m_data->setSourceFile(relativeFileName, path); std::fstream xml_file(relativeFileName, std::fstream::in); while ( xml_file.good() ) @@ -447,6 +457,69 @@ static btCollisionShape* createConvexHullFromShapes(std::vector shorter; + int cnt = path_or_shorter.size(); + for (int i=0; i::iterator x=shorter.begin(); x!=shorter.end(); ++x) + { + std::string attempt = *x + "/" + fn; + FILE* f = fopen(attempt.c_str(), "rb"); + if (!f) { + //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); + continue; + } + fclose(f); + existing_file = attempt; + //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); + break; + } + + if (existing_file.empty()) + { + b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str()); + return false; + } else { + *out_found_filename = existing_file; + return true; + } +} + btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) { BT_PROFILE("convertURDFToCollisionShape"); @@ -507,239 +580,168 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } case URDF_GEOM_SPHERE: { - btScalar radius = collision->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); shape = sphereShape; shape ->setMargin(gUrdfDefaultCollisionMargin); break; + } - break; - } - case URDF_GEOM_MESH: - { - if (collision->m_name.length()) + case URDF_GEOM_MESH: + { + std::string existing_file; + int fileType; + bool success = findExistingMeshFile(urdfPathPrefix, collision->m_geometry.m_meshFileName, collision->m_sourceFileLocation, &existing_file, &fileType); + if (!success) break; // error message already printed + + GLInstanceGraphicsShape* glmesh = 0; + switch (fileType) { + case FILE_OBJ: + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { - //b3Printf("collision->name=%s\n",collision->m_name.c_str()); + glmesh = LoadMeshFromObj(existing_file.c_str(), 0); } - if (1) + else { - if (collision->m_geometry.m_meshFileName.length()) + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, existing_file.c_str()); + //create a convex hull for each shape, and store it in a btCompoundShape + + shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + return shape; + } + break; + + case FILE_STL: + glmesh = LoadMeshFromSTL(existing_file.c_str()); + break; + + case FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans;upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + LoadMeshFromCollada(existing_file.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2); + + glmesh = new GLInstanceGraphicsShape; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i=0;im_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n",filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath,"%s%s",urdfPathPrefix,filename); - b3FileUtils::toLower(fullPath); - char tmpPathPrefix[1024]; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char collisionPathPrefix[1024]; - sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - - if (strstr(fullPath,".dae")) + ColladaGraphicsInstance* instance = &visualShapeInstances[i]; + GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i=0;im_vertices->size();i++) { - fileType = FILE_COLLADA; + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + } - if (strstr(fullPath,".stl")) + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices+additionalIndices); + for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; } - if (strstr(fullPath,".obj")) - { - fileType = FILE_OBJ; - } - sprintf(fullPath,"%s%s",urdfPathPrefix,filename); - FILE* f = fopen(fullPath,"rb"); - if (f) + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; +upAxisMat.setIdentity(); + //upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices+additionalVertices); + + for(int v=0;vm_flags & URDF_FORCE_CONCAVE_TRIMESH) - { - glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix); - } - else - { - std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix); - //create a convex hull for each shape, and store it in a btCompoundShape - - shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); - return shape; - } - break; - } - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans;upAxisTrans.setIdentity(); - float unitMeterScaling=1; - int upAxis = 2; - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis ); - - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i=0;im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i=0;im_vertices->size();i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices+additionalIndices); - for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); - //upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices+additionalVertices); - - for(int v=0;vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: - { - b3Warning("Unsupported file type in Collision: %s\n",fullPath); - btAssert(0); - } - } - - - if (glmesh && (glmesh->m_numvertices>0)) - { - //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); - //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); - //convex->setUserIndex(shapeId); - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - for (int i=0;im_numvertices;i++) - { - convertedVerts.push_back(btVector3( - glmesh->m_vertices->at(i).xyzw[0]*collision->m_geometry.m_meshScale[0], - glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1], - glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2])); - } - - if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) - { - BT_PROFILE("convert trimesh"); - btTriangleMesh* meshInterface = new btTriangleMesh(); - for (int i=0;im_numIndices/3;i++) - { - float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; - float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; - float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; - meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]), - btVector3(v1[0],v1[1],v1[2]), - btVector3(v2[0],v2[1],v2[2])); - } - - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - trimesh->setLocalScaling(collision->m_geometry.m_meshScale); - shape = trimesh; - } else - { - BT_PROFILE("convert btConvexHullShape"); - - btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - convexHull->optimizeConvexHull(); - //convexHull->initializePolyhedralFeatures(); - convexHull->setMargin(gUrdfDefaultCollisionMargin); - convexHull->setLocalScaling(collision->m_geometry.m_meshScale); - shape = convexHull; - } - } else - { - b3Warning("issue extracting mesh from STL file %s\n", fullPath); - } - - delete glmesh; - - } else - { - b3Warning("mesh geometry not found %s\n",fullPath); + btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]); + pos = worldMat*pos; + verts[v].xyzw[0] = float(pos[0]); + verts[v].xyzw[1] = float(pos[1]); + verts[v].xyzw[2] = float(pos[2]); + glmesh->m_vertices->push_back(verts[v]); } - } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(success.c_str()); + break; } + } + + if (!glmesh || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, existing_file.c_str()); + delete glmesh; + break; + } + + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0; im_numvertices; i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*collision->m_geometry.m_meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2])); + } + + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + for (int i=0; im_numIndices/3; i++) + { + float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; + float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; + float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; + meshInterface->addTriangle( + btVector3(v0[0],v0[1],v0[2]), + btVector3(v1[0],v1[1],v1[2]), + btVector3(v2[0],v2[1],v2[2])); + } + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + + } else + { + BT_PROFILE("convert btConvexHullShape"); + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + convexHull->optimizeConvexHull(); + //convexHull->initializePolyhedralFeatures(); + convexHull->setMargin(gUrdfDefaultCollisionMargin); + convexHull->setLocalScaling(collision->m_geometry.m_meshScale); + shape = convexHull; + } + + delete glmesh; + break; + } // mesh case - - break; - } default: - { - b3Warning("Error: unknown visual geometry type\n"); - } - } + b3Warning("Error: unknown visual geometry type\n"); + } return shape; } @@ -778,17 +780,17 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha convexColShape = cylZShape; break; } + case URDF_GEOM_BOX: { - btVector3 extents = visual->m_geometry.m_boxSize; - btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); convexColShape = boxShape; convexColShape->setMargin(gUrdfDefaultCollisionMargin); break; } + case URDF_GEOM_SPHERE: { btScalar radius = visual->m_geometry.m_sphereRadius; @@ -796,206 +798,142 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha convexColShape = sphereShape; convexColShape->setMargin(gUrdfDefaultCollisionMargin); break; - - break; } + case URDF_GEOM_MESH: { - if (visual->m_name.length()) + std::string existing_file; + int fileType; + bool success = findExistingMeshFile(urdfPathPrefix, visual->m_geometry.m_meshFileName, visual->m_sourceFileLocation, &existing_file, &fileType); + if (!success) break; // error message already printed + + switch (fileType) { - //b3Printf("visual->name=%s\n", visual->m_name.c_str()); - } - if (1)//visual->m_geometry) - { - if (visual->m_geometry.m_meshFileName.length()) + case FILE_OBJ: { - const char* filename = visual->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - - char tmpPathPrefix[1024]; - std::string xml_string; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char visualPathPrefix[1024]; - sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".dae")) + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(existing_file, meshData)) { - fileType = FILE_COLLADA; + + if (meshData.m_textureImage) + { + MyTexture texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; } - if (strstr(fullPath, ".stl")) - { - fileType = FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = FILE_OBJ; - } - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - FILE* f = fopen(fullPath, "rb"); - if (f) - { - fclose(f); - - switch (fileType) - { - case FILE_OBJ: - { -// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) - { - - if (meshData.m_textureImage) - { - MyTexture texData; - texData.m_width = meshData.m_textureWidth; - texData.m_height = meshData.m_textureHeight; - texData.textureData = meshData.m_textureImage; - texturesOut.push_back(texData); - } - glmesh = meshData.m_gfxShape; - } - - break; - } - - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; - - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis); - - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_vertices->size(); i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices + additionalIndices); - for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); -// upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices + additionalVertices); - - for (int v = 0; vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: - { - b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath); - btAssert(0); - } - } - - - if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) - { - //apply the geometry scaling - for (int i=0;im_vertices->size();i++) - { - glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; - glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; - glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; - } - - } - else - { - b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); - } - - } - else - { - b3Warning("mesh geometry not found %s\n", fullPath); - } - - + break; } + + case FILE_STL: + { + glmesh = LoadMeshFromSTL(existing_file.c_str()); + break; + } + + case FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(existing_file.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); + // upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + + for (int v = 0; vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(existing_file); + + break; + } + } // switch file type + + if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, existing_file.c_str()); + break; } - + //apply the geometry scaling + for (int i=0;im_vertices->size();i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; + } break; } + default: - { b3Warning("Error: unknown visual geometry type\n"); - } } //if we have a convex, tesselate into localVertices/localIndices diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 37cf5eb70..e27794dde 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -8,9 +8,9 @@ UrdfParser::UrdfParser() :m_parseSDF(false), m_activeSdfModel(-1) { + m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real } - UrdfParser::~UrdfParser() { cleanModel(&m_urdf2Model); @@ -845,7 +845,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { UrdfVisual visual; - + visual.m_sourceFileLocation = sourceFileLocation(vis_xml); + if (parseVisual(model, visual, vis_xml,logger)) { link.m_visualArray.push_back(visual); @@ -864,6 +865,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { UrdfCollision col; + col.m_sourceFileLocation = sourceFileLocation(col_xml); + if (parseCollision(col, col_xml,logger)) { link.m_collisionArray.push_back(col); @@ -1657,3 +1660,9 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) return true; } +std::string UrdfParser::sourceFileLocation(TiXmlElement* e) +{ + char buf[1024]; + snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row()); + return buf; +} diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 64088a1d9..4e7484d23 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -77,6 +77,7 @@ struct UrdfGeometry struct UrdfVisual { + std::string m_sourceFileLocation; btTransform m_linkLocalFrame; UrdfGeometry m_geometry; std::string m_name; @@ -90,6 +91,7 @@ struct UrdfVisual struct UrdfCollision { + std::string m_sourceFileLocation; btTransform m_linkLocalFrame; UrdfGeometry m_geometry; std::string m_name; @@ -159,6 +161,7 @@ struct UrdfJoint struct UrdfModel { std::string m_name; + std::string m_sourceFile; btTransform m_rootTransformInWorld; btHashMap m_materials; btHashMap m_links; @@ -204,7 +207,7 @@ public: UrdfParser(); virtual ~UrdfParser(); - + void setParseSDF(bool useSDF) { m_parseSDF = useSDF; @@ -263,6 +266,13 @@ public: } return m_urdf2Model; } + + std::string sourceFileLocation(TiXmlElement* e); + + void setSourceFile(const std::string& sourceFile) + { + m_urdf2Model.m_sourceFile = sourceFile; + } }; #endif diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index ca4490aee..f69fa3dda 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -507,6 +507,16 @@ LoadObj( const char* filename, const char* mtl_basepath) { + std::string tmp = filename; + if (!mtl_basepath) { + int last_slash = 0; + for (int c=0; c<(int)tmp.size(); ++c) + if (tmp[c]=='/' || tmp[c]=='\\') + last_slash = c; + tmp = tmp.substr(0, last_slash); + mtl_basepath = tmp.c_str(); + //fprintf(stderr, "MTL PATH '%s' orig '%s'\n", mtl_basepath, filename); + } shapes.resize(0); std::vector allIndices; From 82495f3c84983bd2b2d4f1aa1bcd670d7c95b287 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Fri, 10 Mar 2017 18:17:38 +0300 Subject: [PATCH 2/4] URDF loader: reuse the same resources finder routine for TinyRendererVisualShapeConverter --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 54 ++- .../Importers/ImportURDFDemo/UrdfParser.cpp | 27 +- .../Importers/ImportURDFDemo/UrdfParser.h | 14 +- .../TinyRendererVisualShapeConverter.cpp | 322 +++++++----------- 4 files changed, 170 insertions(+), 247 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 3137af5ae..844b0adef 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -457,9 +457,8 @@ static btCollisionShape* createConvexHullFromShapes(std::vector shorter; - int cnt = path_or_shorter.size(); + shorter.push_back("../.."); + shorter.push_back(".."); + shorter.push_back("."); + int cnt = urdf_path.size(); for (int i=0; im_geometry.m_meshFileName, collision->m_sourceFileLocation, &existing_file, &fileType); - if (!success) break; // error message already printed - GLInstanceGraphicsShape* glmesh = 0; - switch (fileType) { + switch (collision->m_geometry.m_meshFileType) { case FILE_OBJ: if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { - glmesh = LoadMeshFromObj(existing_file.c_str(), 0); + glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); } else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, existing_file.c_str()); + std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str()); //create a convex hull for each shape, and store it in a btCompoundShape shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); @@ -613,7 +608,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co break; case FILE_STL: - glmesh = LoadMeshFromSTL(existing_file.c_str()); + glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str()); break; case FILE_COLLADA: @@ -622,7 +617,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co btAlignedObjectArray visualShapeInstances; btTransform upAxisTrans;upAxisTrans.setIdentity(); float unitMeterScaling = 1; - LoadMeshFromCollada(existing_file.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2); + LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2); glmesh = new GLInstanceGraphicsShape; glmesh->m_indices = new b3AlignedObjectArray(); @@ -691,7 +686,7 @@ upAxisMat.setIdentity(); if (!glmesh || glmesh->m_numvertices<=0) { - b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, existing_file.c_str()); + b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, collision->m_geometry.m_meshFileName.c_str()); delete glmesh; break; } @@ -802,17 +797,12 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha case URDF_GEOM_MESH: { - std::string existing_file; - int fileType; - bool success = findExistingMeshFile(urdfPathPrefix, visual->m_geometry.m_meshFileName, visual->m_sourceFileLocation, &existing_file, &fileType); - if (!success) break; // error message already printed - - switch (fileType) + switch (visual->m_geometry.m_meshFileType) { case FILE_OBJ: { b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(existing_file, meshData)) + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) { if (meshData.m_textureImage) @@ -830,7 +820,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha case FILE_STL: { - glmesh = LoadMeshFromSTL(existing_file.c_str()); + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); break; } @@ -842,7 +832,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha float unitMeterScaling = 1; int upAxis = 2; - LoadMeshFromCollada(existing_file.c_str(), + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, @@ -910,7 +900,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(existing_file); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName); break; } @@ -918,7 +908,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices<=0) { - b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, existing_file.c_str()); + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str()); break; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index e27794dde..4ccb9b14a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -444,21 +444,24 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* return false; } - geom.m_meshFileName = shape->Attribute("filename"); - geom.m_meshScale.setValue(1,1,1); + bool success = findExistingMeshFile( + m_urdf2Model.m_sourceFile, shape->Attribute("filename"), sourceFileLocation(shape), + &geom.m_meshFileName, &geom.m_meshFileType); + if (!success) return false; // warning printed + geom.m_meshScale.setValue(1,1,1); - if (shape->Attribute("scale")) + if (shape->Attribute("scale")) { if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger)) - { - logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n"); - std::string scalar_str = shape->Attribute("scale"); - double scaleFactor = urdfLexicalCast(scalar_str.c_str()); - if (scaleFactor) - { - geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); - } - } + { + logger->reportWarning("%s: scale should be a vector3, not single scalar. Workaround activated.\n"); + std::string scalar_str = shape->Attribute("scale"); + double scaleFactor = urdfLexicalCast(scalar_str.c_str()); + if (scaleFactor) + { + geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); + } + } } else { } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 4e7484d23..505e2658f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -69,12 +69,22 @@ struct UrdfGeometry double m_cylinderRadius; double m_cylinderLength; - btVector3 m_planeNormal; + btVector3 m_planeNormal; + enum { + FILE_STL =1, + FILE_COLLADA =2, + FILE_OBJ =3, + }; + int m_meshFileType; std::string m_meshFileName; - btVector3 m_meshScale; + btVector3 m_meshScale; }; +bool findExistingMeshFile(const std::string& urdf_path, std::string fn, + const std::string& error_message_prefix, + std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere + struct UrdfVisual { std::string m_sourceFileLocation; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index cfe25c938..1876e9623 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -36,13 +36,6 @@ subject to the following restrictions: #include "../TinyRenderer/model.h" #include "../ThirdPartyLibs/stb_image/stb_image.h" -enum MyFileType -{ - MY_FILE_STL=1, - MY_FILE_COLLADA=2, - MY_FILE_OBJ=3, -}; - struct MyTexture2 { unsigned char* textureData; @@ -241,223 +234,150 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref convexColShape = sphereShape; convexColShape->setMargin(0.001); break; - - break; } case URDF_GEOM_MESH: { - if (visual->m_name.length()) + strncpy(visualShapeOut.m_meshAssetFileName, visual->m_geometry.m_meshFileName.c_str(), VISUAL_SHAPE_MAX_PATH_LEN); + visualShapeOut.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN-1] = 0; + + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; + + visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0]; + visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1]; + visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2]; + visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0]; + visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1]; + visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2]; + visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3]; + + switch (visual->m_geometry.m_meshFileType) { - //b3Printf("visual->name=%s\n", visual->m_name.c_str()); - } - if (1)//visual->m_geometry) - { - if (visual->m_geometry.m_meshFileName.length()) + case UrdfGeometry::FILE_OBJ: { - const char* filename = visual->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - - char tmpPathPrefix[1024]; - std::string xml_string; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char visualPathPrefix[1024]; - sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".dae")) + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) { - fileType = MY_FILE_COLLADA; - } - if (strstr(fullPath, ".stl")) - { - fileType = MY_FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = MY_FILE_OBJ; - } - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; - visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; - visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; - visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0]; - visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1]; - visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2]; - visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0]; - visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1]; - visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2]; - visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3]; - - int sl = strlen(fullPath); - if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1)) - { - memcpy(visualShapeOut.m_meshAssetFileName, fullPath, sl); - visualShapeOut.m_meshAssetFileName[sl] = 0; - } - - FILE* f = fopen(fullPath, "rb"); - if (f) - { - fclose(f); - - - - switch (fileType) + if (meshData.m_textureImage) { - case MY_FILE_OBJ: - { - //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) - { - - if (meshData.m_textureImage) - { - MyTexture2 texData; - texData.m_width = meshData.m_textureWidth; - texData.m_height = meshData.m_textureHeight; - texData.textureData = meshData.m_textureImage; - texturesOut.push_back(texData); - } - glmesh = meshData.m_gfxShape; - } - - - break; - } - - case MY_FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); } - case MY_FILE_COLLADA: + glmesh = meshData.m_gfxShape; + } + break; + } + case UrdfGeometry::FILE_STL: + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + break; + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_vertices->size(); i++) { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; + } - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis); + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_vertices->size(); i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices + additionalIndices); - for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); // upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); - for (int v = 0; vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: + for (int v = 0; vm_vertices->push_back(verts[v]); } - } - - - if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) - { - //apply the geometry scaling - for (int i=0;im_vertices->size();i++) - { - glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; - glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; - glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; - } - - } - else - { - b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); - } - - } - else - { - b3Warning("mesh geometry not found %s\n", fullPath); } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str()); + break; + } + default: + // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) + btAssert(0); + } + if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) + { + //apply the geometry scaling + for (int i=0;im_vertices->size();i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; } } - - + else + { + b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + } break; - } + } // case mesh + default: { b3Warning("Error: unknown visual geometry type\n"); From 3a8199ec2867c8c38751a323641ce972c8fee572 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Fri, 10 Mar 2017 19:46:46 +0300 Subject: [PATCH 3/4] Fix formatting --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 34 +++++++++++++++---- .../Importers/ImportURDFDemo/UrdfParser.cpp | 9 +++-- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 844b0adef..553c34b12 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -467,13 +467,26 @@ bool findExistingMeshFile( b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str()); return false; } + std::string ext; std::string ext_ = fn.substr(fn.size()-4); for (std::string::iterator i=ext_.begin(); i!=ext_.end(); ++i) + { ext += char(tolower(*i)); - if (ext==".dae") *out_type = UrdfGeometry::FILE_COLLADA; - else if (ext==".stl") *out_type = UrdfGeometry::FILE_STL; - else if (ext==".obj") *out_type = UrdfGeometry::FILE_OBJ; + } + + if (ext==".dae") + { + *out_type = UrdfGeometry::FILE_COLLADA; + } + else if (ext==".stl") + { + *out_type = UrdfGeometry::FILE_STL; + } + else if (ext==".obj") + { + *out_type = UrdfGeometry::FILE_OBJ; + } else { b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str()); @@ -489,9 +502,12 @@ bool findExistingMeshFile( shorter.push_back(".."); shorter.push_back("."); int cnt = urdf_path.size(); - for (int i=0; im_geometry.m_meshFileType) { + switch (collision->m_geometry.m_meshFileType) + { case FILE_OBJ: if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 4ccb9b14a..cd0a19acc 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -439,7 +439,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* } else { - if (!shape->Attribute("filename")) { + if (!shape->Attribute("filename")) + { logger->reportError("Mesh must contain a filename attribute"); return false; } @@ -447,7 +448,11 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* bool success = findExistingMeshFile( m_urdf2Model.m_sourceFile, shape->Attribute("filename"), sourceFileLocation(shape), &geom.m_meshFileName, &geom.m_meshFileType); - if (!success) return false; // warning printed + if (!success) + { + // warning printed + return false; + } geom.m_meshScale.setValue(1,1,1); if (shape->Attribute("scale")) From fa60cc5f567efb516ff6fc02a2220abeae44ddb7 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Tue, 14 Mar 2017 02:32:02 +0300 Subject: [PATCH 4/4] URDF loader: fix SDF branch, warn about unsupported geometry --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 5 +- .../Importers/ImportURDFDemo/UrdfParser.cpp | 113 +++++++++--------- .../TinyRendererVisualShapeConverter.cpp | 2 +- 3 files changed, 61 insertions(+), 59 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 553c34b12..0820d7e5e 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -755,7 +755,8 @@ upAxisMat.setIdentity(); } // mesh case default: - b3Warning("Error: unknown visual geometry type\n"); + b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type); + // for example, URDF_GEOM_PLANE } return shape; } @@ -943,7 +944,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha } default: - b3Warning("Error: unknown visual geometry type\n"); + b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type); } //if we have a convex, tesselate into localVertices/localIndices diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index cd0a19acc..ad5247710 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -410,67 +410,68 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* geom.m_type = URDF_GEOM_CAPSULE; if (!shape->Attribute("length") || !shape->Attribute("radius")) - { - logger->reportError("Capsule shape must have both length and radius attributes"); - return false; - } + { + logger->reportError("Capsule shape must have both length and radius attributes"); + return false; + } geom.m_hasFromTo = false; geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast(shape->Attribute("length")); - } - else if (type_name == "mesh") - { - geom.m_type = URDF_GEOM_MESH; - if (m_parseSDF) - { - TiXmlElement* scale = shape->FirstChildElement("scale"); - if (0==scale) - { - geom.m_meshScale.setValue(1,1,1); - } - else - { - parseVector3(geom.m_meshScale,scale->GetText(),logger); - } - - TiXmlElement* filename = shape->FirstChildElement("uri"); - geom.m_meshFileName = filename->GetText(); - } - else - { - if (!shape->Attribute("filename")) - { - logger->reportError("Mesh must contain a filename attribute"); - return false; - } - - bool success = findExistingMeshFile( - m_urdf2Model.m_sourceFile, shape->Attribute("filename"), sourceFileLocation(shape), - &geom.m_meshFileName, &geom.m_meshFileType); - if (!success) - { - // warning printed - return false; - } - geom.m_meshScale.setValue(1,1,1); + else if (type_name == "mesh") + { + geom.m_type = URDF_GEOM_MESH; + geom.m_meshScale.setValue(1,1,1); + std::string fn; - if (shape->Attribute("scale")) - { - if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger)) - { - logger->reportWarning("%s: scale should be a vector3, not single scalar. Workaround activated.\n"); - std::string scalar_str = shape->Attribute("scale"); - double scaleFactor = urdfLexicalCast(scalar_str.c_str()); - if (scaleFactor) - { - geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); - } - } - } else - { - } - } + if (m_parseSDF) + { + if (TiXmlElement* scale = shape->FirstChildElement("scale")) + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + } + if (TiXmlElement* filename = shape->FirstChildElement("uri")) + { + fn = filename->GetText(); + } + } + else + { + // URDF + if (shape->Attribute("filename")) + { + fn = shape->Attribute("filename"); + } + if (shape->Attribute("scale")) + { + if (!parseVector3(geom.m_meshScale, shape->Attribute("scale"), logger)) + { + logger->reportWarning("Scale should be a vector3, not single scalar. Workaround activated.\n"); + std::string scalar_str = shape->Attribute("scale"); + double scaleFactor = urdfLexicalCast(scalar_str.c_str()); + if (scaleFactor) + { + geom.m_meshScale.setValue(scaleFactor, scaleFactor, scaleFactor); + } + } + } + } + + if (fn.empty()) + { + logger->reportError("Mesh filename is empty"); + return false; + } + + geom.m_meshFileName = fn; + bool success = findExistingMeshFile( + m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape), + &geom.m_meshFileName, &geom.m_meshFileType); + if (!success) + { + // warning already printed + return false; + } } else { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 1876e9623..d2c3b79bd 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -380,7 +380,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref default: { - b3Warning("Error: unknown visual geometry type\n"); + b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); } }