diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 4d225c05b..d1a051fc8 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -30,12 +30,6 @@ #include -enum eMJCF_FILE_TYPE_ENUMS -{ - MJCF_FILE_STL = 1, - MJCF_FILE_OBJ = 2 -}; - enum ePARENT_LINK_ENUMS { BASE_LINK_INDEX=-1, @@ -137,9 +131,11 @@ struct MyMJCFAsset struct BulletMJCFImporterInternalData { GUIHelperInterface* m_guiHelper; + struct LinkVisualShapesConverter* m_customVisualShapesConverter; char m_pathPrefix[1024]; - std::string m_fileModelName; + std::string m_sourceFileName; // with path + std::string m_fileModelName; // without path btHashMap m_assets; btAlignedObjectArray m_models; @@ -150,6 +146,7 @@ struct BulletMJCFImporterInternalData int m_activeModel; + int m_activeBodyUniqueId; //todo: for full MJCF compatibility, we would need a stack of default values int m_defaultCollisionGroup; int m_defaultCollisionMask; @@ -160,12 +157,20 @@ struct BulletMJCFImporterInternalData BulletMJCFImporterInternalData() :m_activeModel(-1), + m_activeBodyUniqueId(-1), m_defaultCollisionGroup(1), m_defaultCollisionMask(1), m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm { m_pathPrefix[0] = 0; } + + std::string sourceFileLocation(TiXmlElement* e) + { + char buf[1024]; + snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row()); + return buf; + } const UrdfLink* getLink(int modelIndex, int linkIndex) const { @@ -621,9 +626,14 @@ struct BulletMJCFImporterInternalData MyMJCFAsset* assetPtr = m_assets[meshStr]; if (assetPtr) { - handledGeomType = true; geom.m_type = URDF_GEOM_MESH; geom.m_meshFileName = assetPtr->m_fileName; + bool exists = findExistingMeshFile( + m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml), + &geom.m_meshFileName, + &geom.m_meshFileType); + handledGeomType = exists; + geom.m_meshScale.setValue(1,1,1); //todo: parse mesh scale if (sz) @@ -1113,10 +1123,11 @@ struct BulletMJCFImporterInternalData }; -BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper) +BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) { m_data = new BulletMJCFImporterInternalData(); m_data->m_guiHelper = helper; + m_data->m_customVisualShapesConverter = customConverter; } BulletMJCFImporter::~BulletMJCFImporter() @@ -1135,7 +1146,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger, b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); + bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); + m_data->m_sourceFileName = relativeFileName; std::string xml_string; m_data->m_pathPrefix[0] = 0; @@ -1399,21 +1411,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& return false; } - -void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const +void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { + if (m_data->m_customVisualShapesConverter) + { + const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex); + } } + void BulletMJCFImporter::setBodyUniqueId(int bodyId) { - + m_data->m_activeBodyUniqueId = bodyId; } int BulletMJCFImporter::getBodyUniqueId() const { - return 0; + b3Assert(m_data->m_activeBodyUniqueId != -1); + return m_data->m_activeBodyUniqueId; } - static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, btScalar collisionMargin) { btCompoundShape* compound = new btCompoundShape(); @@ -1496,132 +1513,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case URDF_GEOM_MESH: { - ////////////////////// - if (1) - { - if (col->m_geometry.m_meshFileName.length()) + GLInstanceGraphicsShape* glmesh = 0; + switch (col->m_geometry.m_meshFileType) { - const char* filename = col->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n",filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath,"%s%s",pathPrefix,filename); - b3FileUtils::toLower(fullPath); - char tmpPathPrefix[1024]; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char collisionPathPrefix[1024]; - sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix); - - if (strstr(fullPath,".stl")) + case UrdfGeometry::FILE_OBJ: { - fileType = MJCF_FILE_STL; + if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0); + } + else + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str()); + //create a convex hull for each shape, and store it in a btCompoundShape + + childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin); + + } + break; } - if (strstr(fullPath,".obj")) - { - fileType = MJCF_FILE_OBJ; - } - - sprintf(fullPath,"%s%s",pathPrefix,filename); - FILE* f = fopen(fullPath,"rb"); - if (f) + case UrdfGeometry::FILE_STL: { - fclose(f); - GLInstanceGraphicsShape* glmesh = 0; - - - switch (fileType) - { - case MJCF_FILE_OBJ: - { - if (col->m_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 + glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str()); + break; + } + default: + b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType); + } - childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin); - - } - break; - } - case MJCF_FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - - default: - { - b3Warning("Unsupported file type in Collision: %s\n",fullPath); - - } - } - + if (childShape) + { + // okay! + } + else if (!glmesh || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str()); + } + else + { + //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]*col->m_geometry.m_meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2])); + } - if (!childShape && glmesh && (glmesh->m_numvertices>0)) + if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + + btTriangleMesh* meshInterface = new btTriangleMesh(); + for (int i=0;im_numIndices/3;i++) { - //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]*col->m_geometry.m_meshScale[0], - glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1], - glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2])); - } - - if (col->m_flags & URDF_FORCE_CONCAVE_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); - childShape = trimesh; - } else - { - btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - convexHull->optimizeConvexHull(); - //convexHull->initializePolyhedralFeatures(); - convexHull->setMargin(m_data->m_defaultCollisionMargin); - childShape = convexHull; - } - } else - { - b3Warning("issue extracting mesh from STL file %s\n", fullPath); + 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])); } - delete glmesh; - + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + childShape = trimesh; } else { - b3Warning("mesh geometry not found %s\n",fullPath); + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + convexHull->optimizeConvexHull(); + //convexHull->initializePolyhedralFeatures(); + convexHull->setMargin(m_data->m_defaultCollisionMargin); + childShape = convexHull; } - } - } - ////////////////////// + delete glmesh; break; } - case URDF_GEOM_CAPSULE: { //todo: convert fromto to btCapsuleShape + local btTransform @@ -1636,7 +1608,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn btVector3 fromto[2] = {f,t}; btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) ,btScalar(col->m_geometry.m_capsuleRadius)}; - + btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2); childShape = ms; } else @@ -1647,11 +1619,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } break; } - default: - { + } // switch geom - } - } if (childShape) { m_data->m_allocatedCollisionShapes.push_back(childShape); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index f1b439690..a719c984e 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface { struct BulletMJCFImporterInternalData* m_data; - public: - BulletMJCFImporter(struct GUIHelperInterface* helper); + BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter); virtual ~BulletMJCFImporter(); virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger); @@ -66,7 +65,7 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; virtual void setBodyUniqueId(int bodyId); virtual int getBodyUniqueId() const; diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index 79fff963f..b5e8cda6c 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics() m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2; //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->getDebugDrawer()->setDebugMode( - btIDebugDraw::DBG_DrawConstraints - +btIDebugDraw::DBG_DrawContactPoints - +btIDebugDraw::DBG_DrawAabb - );//+btIDebugDraw::DBG_DrawConstraintLimits); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); if (m_guiHelper->getParameterInterface()) @@ -203,20 +203,23 @@ void ImportMJCFSetup::initPhysics() slider.m_maxVal = 10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - - BulletMJCFImporter importer(m_guiHelper); + + BulletMJCFImporter importer(m_guiHelper, 0); MyMJCFLogger logger; bool result = importer.loadMJCF(m_fileName,&logger); if (result) { - btTransform rootTrans; - rootTrans.setIdentity(); - - for (int m =0; mregisterNameForPointer(name->c_str(),name->c_str()); #endif//TEST_MULTIBODY_SERIALIZATION @@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics() m_data->m_numMotors++; } } - } + } else { + // not multibody if (1) { //create motors for each generic joint diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index ae3364334..2a60cb197 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -75,16 +75,6 @@ void BulletURDFImporter::printTree() // btAssert(0); } - -enum MyFileType -{ - FILE_STL=1, - FILE_COLLADA=2, - FILE_OBJ=3, -}; - - - BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) { m_data = new BulletURDFInternalData; @@ -624,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co GLInstanceGraphicsShape* glmesh = 0; switch (collision->m_geometry.m_meshFileType) { - case FILE_OBJ: + case UrdfGeometry::FILE_OBJ: if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); @@ -640,11 +630,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } break; - case FILE_STL: + case UrdfGeometry::FILE_STL: glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str()); break; - case FILE_COLLADA: + case UrdfGeometry::FILE_COLLADA: { btAlignedObjectArray visualShapes; btAlignedObjectArray visualShapeInstances; @@ -833,7 +823,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha { switch (visual->m_geometry.m_meshFileType) { - case FILE_OBJ: + case UrdfGeometry::FILE_OBJ: { b3ImportMeshData meshData; if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) @@ -852,13 +842,13 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha break; } - case FILE_STL: + case UrdfGeometry::FILE_STL: { glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); break; } - case FILE_COLLADA: + case UrdfGeometry::FILE_COLLADA: { btAlignedObjectArray visualShapes; btAlignedObjectArray visualShapeInstances; @@ -1128,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& return false; } -void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const +void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const { - if (m_data->m_customVisualShapesConverter) { const UrdfModel& model = m_data->m_urdfParser.getModel(); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId); + UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); + if (linkPtr) + { + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + } } - } int BulletURDFImporter::getNumAllocatedCollisionShapes() const diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 26b5a66e2..2ceb8e0d7 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -51,7 +51,7 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h index 814838ced..8f3d3c411 100644 --- a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h +++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h @@ -1,9 +1,14 @@ #ifndef LINK_VISUAL_SHAPES_CONVERTER_H #define LINK_VISUAL_SHAPES_CONVERTER_H +struct UrdfLink; +struct UrdfModel; +class btTransform; +class btCollisionObject; + struct LinkVisualShapesConverter { - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0; + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0; }; #endif //LINK_VISUAL_SHAPES_CONVERTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 30f35963a..a99477d93 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -311,7 +311,7 @@ void ConvertURDF2BulletInternal( - //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body); + //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody==0) @@ -469,8 +469,8 @@ void ConvertURDF2BulletInternal( btVector4 color = selectColor2();//(0.0,0.0,0.5); u2b.getLinkColor(urdfLinkIndex,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); - - u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId()); + + u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId()); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); @@ -487,7 +487,7 @@ void ConvertURDF2BulletInternal( } } else { - //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape); + //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape); } } diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 6d4c98e57..427f98b3b 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -49,7 +49,7 @@ public: ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { } + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { } virtual void setBodyUniqueId(int bodyId) {} virtual int getBodyUniqueId() const { return 0;} diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 505e2658f..6c2e0e090 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -85,26 +85,23 @@ 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 +struct UrdfShape { std::string m_sourceFileLocation; btTransform m_linkLocalFrame; UrdfGeometry m_geometry; std::string m_name; +}; + +struct UrdfVisual: UrdfShape +{ std::string m_materialName; bool m_hasLocalMaterial; UrdfMaterial m_localMaterial; }; - - - -struct UrdfCollision +struct UrdfCollision: UrdfShape { - std::string m_sourceFileLocation; - btTransform m_linkLocalFrame; - UrdfGeometry m_geometry; - std::string m_name; int m_flags; int m_collisionGroup; int m_collisionMask; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 0989a4962..5ca0f35a2 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds) double PhysicsDirect::getTimeOut() const { return m_data->m_timeOutInSeconds; -} \ No newline at end of file +} diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c5a48efaa..077dba179 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1497,7 +1497,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS m_data->m_sdfRecentLoadedBodies.clear(); - BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter + BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); bool useFixedBase = false; MyMJCFLogger2 logger; @@ -4610,20 +4610,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, + int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, shapeIndex, visualShapeStoragePtr); - - - //m_visualConverter - serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; - serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; - serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; - serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED; - hasStatus = true; - break; + if (success) { + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; + serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; + serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; + } + hasStatus = true; + break; } case CMD_UPDATE_VISUAL_SHAPE: { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index d2c3b79bd..58fd7f13e 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -170,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff m_data->m_hasLightSpecularCoeff = true; } -void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) +void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) { visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; @@ -211,6 +211,38 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref convexColShape = cylZShape; break; } + case URDF_GEOM_CAPSULE: + { + btVector3 p1 = visual->m_geometry.m_capsuleFrom; + btVector3 p2 = visual->m_geometry.m_capsuleTo; + btVector3 v = p2 - p1; + btVector3 center = (p2 + p1) * 0.5; + btScalar rad = visual->m_geometry.m_capsuleRadius; + btVector3 up_vector(0,0,1); + btVector3 dir = v.normalized(); + btVector3 axis = dir.cross(up_vector); + if (axis.fuzzyZero()) + { + axis = btVector3(0,0,1); + } + else + { + axis.normalize(); + } + btQuaternion q(axis, -acos(dir.dot(up_vector))); + btTransform capsule_orient(q, center); + btTransform tr = visual->m_linkLocalFrame * capsule_orient; + visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; + visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1]; + visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2]; + visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0]; + visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1]; + visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2]; + visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; + visualShapeOut.m_dimensions[0] = v.length(); + visualShapeOut.m_dimensions[1] = rad; + break; + } case URDF_GEOM_BOX: { visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0]; @@ -228,7 +260,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref case URDF_GEOM_SPHERE: { visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; - + btScalar radius = visual->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); convexColShape = sphereShape; @@ -244,14 +276,6 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref 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) { case UrdfGeometry::FILE_OBJ: @@ -467,17 +491,30 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref -void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId) +void TinyRendererVisualShapeConverter::convertVisualShapes( + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + const UrdfLink* linkPtr, const UrdfModel* model, + class btCollisionObject* colObj, int bodyUniqueId) { - - - UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); + btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) { + const btArray* shapeArray; + bool useVisual; + int cnt = 0; + if (linkPtr->m_visualArray.size() > 0) + { + useVisual = true; + cnt = linkPtr->m_visualArray.size(); + } + else + { + // We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape. + useVisual = false; + cnt = linkPtr->m_collisionArray.size(); + } - const UrdfLink* link = *linkPtr; - - for (int v1 = 0; v1 < link->m_visualArray.size();v1++) + for (int v1=0; v1 textures; btAlignedObjectArray vertices; @@ -485,20 +522,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const btTransform startTrans; startTrans.setIdentity(); //int graphicsIndex = -1; - const UrdfVisual& vis = link->m_visualArray[v1]; - btTransform childTrans = vis.m_linkLocalFrame; - btHashString matName(vis.m_materialName.c_str()); - UrdfMaterial *const * matPtr = model.m_materials[matName]; - - float rgbaColor[4] = {1,1,1,1}; - - if (matPtr) + const UrdfShape* vis; + if (useVisual) { + vis = &linkPtr->m_visualArray[v1]; + } else { + vis = &linkPtr->m_collisionArray[v1]; + } + btTransform childTrans = vis->m_linkLocalFrame; + + float rgbaColor[4] = {1,1,1,1}; + if (model && useVisual) { - UrdfMaterial *const mat = *matPtr; - for (int i=0;i<4;i++) - rgbaColor[i] = mat->m_rgbaColor[i]; - //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); - //m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); + btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); + UrdfMaterial*const* matPtr = model->m_materials[matName]; + if (matPtr) + { + for (int i=0; i<4; i++) + { + rgbaColor[i] = (*matPtr)->m_rgbaColor[i]; + } + //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); + //m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); + } } TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj]; @@ -513,19 +558,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const b3VisualShapeData visualShape; visualShape.m_objectUniqueId = bodyUniqueId; visualShape.m_linkIndex = linkIndex; - visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0]; - visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1]; - visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2]; - visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0]; - visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1]; - visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2]; - visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3]; - visualShape.m_rgbaColor[0] = rgbaColor[0]; - visualShape.m_rgbaColor[1] = rgbaColor[1]; - visualShape.m_rgbaColor[2] = rgbaColor[2]; - visualShape.m_rgbaColor[3] = rgbaColor[3]; + visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0]; + visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1]; + visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2]; + visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0]; + visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1]; + visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2]; + visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3]; + visualShape.m_rgbaColor[0] = rgbaColor[0]; + visualShape.m_rgbaColor[1] = rgbaColor[1]; + visualShape.m_rgbaColor[2] = rgbaColor[2]; + visualShape.m_rgbaColor[3] = rgbaColor[3]; - convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape); + convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape); m_data->m_visualShapes.push_back(visualShape); if (vertices.size() && indices.size()) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 78b4c62d6..e9e60948f 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter virtual ~TinyRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex); + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex); virtual int getNumVisualShapes(int bodyUniqueId);