From e8da7bb6f8de7ac464548f69b918b2cbf2ed2aaa Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Fri, 17 Mar 2017 02:09:55 +0300 Subject: [PATCH 1/9] URDF loader: fix MuJoCo xml load, also closes #993 --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 229 ++++++++---------- .../ImportMJCFDemo/BulletMJCFImporter.h | 5 +- .../ImportMJCFDemo/ImportMJCFSetup.cpp | 51 ++-- .../ImportURDFDemo/BulletUrdfImporter.cpp | 32 +-- .../ImportURDFDemo/BulletUrdfImporter.h | 2 +- .../LinkVisualShapesConverter.h | 7 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 8 +- .../ImportURDFDemo/URDFImporterInterface.h | 2 +- .../Importers/ImportURDFDemo/UrdfParser.h | 15 +- examples/SharedMemory/PhysicsDirect.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 25 +- .../TinyRendererVisualShapeConverter.cpp | 129 ++++++---- .../TinyRendererVisualShapeConverter.h | 2 +- 13 files changed, 259 insertions(+), 250 deletions(-) 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); From fa7397cc922e3901fe3163333abcb77474cab237 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Fri, 17 Mar 2017 02:10:27 +0300 Subject: [PATCH 2/9] Rename *.STL to *.stl for case-sensitive filesystems --- data/MPL/mesh/{index0.STL => index0.stl} | Bin .../{index0_collision.STL => index0_collision.stl} | Bin data/MPL/mesh/{index1.STL => index1.stl} | Bin .../{index1_collision.STL => index1_collision.stl} | Bin data/MPL/mesh/{index2.STL => index2.stl} | Bin .../{index2_collision.STL => index2_collision.stl} | Bin data/MPL/mesh/{index3.STL => index3.stl} | Bin .../{index3_collision.STL => index3_collision.stl} | Bin data/MPL/mesh/{middle0.STL => middle0.stl} | Bin ...{middle0_collision.STL => middle0_collision.stl} | Bin data/MPL/mesh/{middle1.STL => middle1.stl} | Bin ...{middle1_collision.STL => middle1_collision.stl} | Bin data/MPL/mesh/{middle2.STL => middle2.stl} | Bin ...{middle2_collision.STL => middle2_collision.stl} | Bin data/MPL/mesh/{middle3.STL => middle3.stl} | Bin ...{middle3_collision.STL => middle3_collision.stl} | Bin data/MPL/mesh/{palm.STL => palm.stl} | Bin .../mesh/{palm_collision.STL => palm_collision.stl} | Bin data/MPL/mesh/{pinky0.STL => pinky0.stl} | Bin .../{pinky0_collision.STL => pinky0_collision.stl} | Bin data/MPL/mesh/{pinky1.STL => pinky1.stl} | Bin .../{pinky1_collision.STL => pinky1_collision.stl} | Bin data/MPL/mesh/{pinky2.STL => pinky2.stl} | Bin .../{pinky2_collision.STL => pinky2_collision.stl} | Bin data/MPL/mesh/{pinky3.STL => pinky3.stl} | Bin .../{pinky3_collision.STL => pinky3_collision.stl} | Bin data/MPL/mesh/{ring0.STL => ring0.stl} | Bin .../{ring0_collision.STL => ring0_collision.stl} | Bin data/MPL/mesh/{ring1.STL => ring1.stl} | Bin .../{ring1_collision.STL => ring1_collision.stl} | Bin data/MPL/mesh/{ring2.STL => ring2.stl} | Bin .../{ring2_collision.STL => ring2_collision.stl} | Bin data/MPL/mesh/{ring3.STL => ring3.stl} | Bin .../{ring3_collision.STL => ring3_collision.stl} | Bin data/MPL/mesh/{thumb0.STL => thumb0.stl} | Bin .../{thumb0_collision.STL => thumb0_collision.stl} | Bin data/MPL/mesh/{thumb1.STL => thumb1.stl} | Bin .../{thumb1_collision.STL => thumb1_collision.stl} | Bin data/MPL/mesh/{thumb2.STL => thumb2.stl} | Bin .../{thumb2_collision.STL => thumb2_collision.stl} | Bin data/MPL/mesh/{thumb3.STL => thumb3.stl} | Bin .../{thumb3_collision.STL => thumb3_collision.stl} | Bin data/MPL/mesh/{wristx.STL => wristx.stl} | Bin .../{wristx_collision.STL => wristx_collision.stl} | Bin data/MPL/mesh/{wristy.STL => wristy.stl} | Bin .../{wristy_collision.STL => wristy_collision.stl} | Bin data/MPL/mesh/{wristz.STL => wristz.stl} | Bin .../{wristz_collision.STL => wristz_collision.stl} | Bin 48 files changed, 0 insertions(+), 0 deletions(-) rename data/MPL/mesh/{index0.STL => index0.stl} (100%) rename data/MPL/mesh/{index0_collision.STL => index0_collision.stl} (100%) rename data/MPL/mesh/{index1.STL => index1.stl} (100%) rename data/MPL/mesh/{index1_collision.STL => index1_collision.stl} (100%) rename data/MPL/mesh/{index2.STL => index2.stl} (100%) rename data/MPL/mesh/{index2_collision.STL => index2_collision.stl} (100%) rename data/MPL/mesh/{index3.STL => index3.stl} (100%) rename data/MPL/mesh/{index3_collision.STL => index3_collision.stl} (100%) rename data/MPL/mesh/{middle0.STL => middle0.stl} (100%) rename data/MPL/mesh/{middle0_collision.STL => middle0_collision.stl} (100%) rename data/MPL/mesh/{middle1.STL => middle1.stl} (100%) rename data/MPL/mesh/{middle1_collision.STL => middle1_collision.stl} (100%) rename data/MPL/mesh/{middle2.STL => middle2.stl} (100%) rename data/MPL/mesh/{middle2_collision.STL => middle2_collision.stl} (100%) rename data/MPL/mesh/{middle3.STL => middle3.stl} (100%) rename data/MPL/mesh/{middle3_collision.STL => middle3_collision.stl} (100%) rename data/MPL/mesh/{palm.STL => palm.stl} (100%) rename data/MPL/mesh/{palm_collision.STL => palm_collision.stl} (100%) rename data/MPL/mesh/{pinky0.STL => pinky0.stl} (100%) rename data/MPL/mesh/{pinky0_collision.STL => pinky0_collision.stl} (100%) rename data/MPL/mesh/{pinky1.STL => pinky1.stl} (100%) rename data/MPL/mesh/{pinky1_collision.STL => pinky1_collision.stl} (100%) rename data/MPL/mesh/{pinky2.STL => pinky2.stl} (100%) rename data/MPL/mesh/{pinky2_collision.STL => pinky2_collision.stl} (100%) rename data/MPL/mesh/{pinky3.STL => pinky3.stl} (100%) rename data/MPL/mesh/{pinky3_collision.STL => pinky3_collision.stl} (100%) rename data/MPL/mesh/{ring0.STL => ring0.stl} (100%) rename data/MPL/mesh/{ring0_collision.STL => ring0_collision.stl} (100%) rename data/MPL/mesh/{ring1.STL => ring1.stl} (100%) rename data/MPL/mesh/{ring1_collision.STL => ring1_collision.stl} (100%) rename data/MPL/mesh/{ring2.STL => ring2.stl} (100%) rename data/MPL/mesh/{ring2_collision.STL => ring2_collision.stl} (100%) rename data/MPL/mesh/{ring3.STL => ring3.stl} (100%) rename data/MPL/mesh/{ring3_collision.STL => ring3_collision.stl} (100%) rename data/MPL/mesh/{thumb0.STL => thumb0.stl} (100%) rename data/MPL/mesh/{thumb0_collision.STL => thumb0_collision.stl} (100%) rename data/MPL/mesh/{thumb1.STL => thumb1.stl} (100%) rename data/MPL/mesh/{thumb1_collision.STL => thumb1_collision.stl} (100%) rename data/MPL/mesh/{thumb2.STL => thumb2.stl} (100%) rename data/MPL/mesh/{thumb2_collision.STL => thumb2_collision.stl} (100%) rename data/MPL/mesh/{thumb3.STL => thumb3.stl} (100%) rename data/MPL/mesh/{thumb3_collision.STL => thumb3_collision.stl} (100%) rename data/MPL/mesh/{wristx.STL => wristx.stl} (100%) rename data/MPL/mesh/{wristx_collision.STL => wristx_collision.stl} (100%) rename data/MPL/mesh/{wristy.STL => wristy.stl} (100%) rename data/MPL/mesh/{wristy_collision.STL => wristy_collision.stl} (100%) rename data/MPL/mesh/{wristz.STL => wristz.stl} (100%) rename data/MPL/mesh/{wristz_collision.STL => wristz_collision.stl} (100%) diff --git a/data/MPL/mesh/index0.STL b/data/MPL/mesh/index0.stl similarity index 100% rename from data/MPL/mesh/index0.STL rename to data/MPL/mesh/index0.stl diff --git a/data/MPL/mesh/index0_collision.STL b/data/MPL/mesh/index0_collision.stl similarity index 100% rename from data/MPL/mesh/index0_collision.STL rename to data/MPL/mesh/index0_collision.stl diff --git a/data/MPL/mesh/index1.STL b/data/MPL/mesh/index1.stl similarity index 100% rename from data/MPL/mesh/index1.STL rename to data/MPL/mesh/index1.stl diff --git a/data/MPL/mesh/index1_collision.STL b/data/MPL/mesh/index1_collision.stl similarity index 100% rename from data/MPL/mesh/index1_collision.STL rename to data/MPL/mesh/index1_collision.stl diff --git a/data/MPL/mesh/index2.STL b/data/MPL/mesh/index2.stl similarity index 100% rename from data/MPL/mesh/index2.STL rename to data/MPL/mesh/index2.stl diff --git a/data/MPL/mesh/index2_collision.STL b/data/MPL/mesh/index2_collision.stl similarity index 100% rename from data/MPL/mesh/index2_collision.STL rename to data/MPL/mesh/index2_collision.stl diff --git a/data/MPL/mesh/index3.STL b/data/MPL/mesh/index3.stl similarity index 100% rename from data/MPL/mesh/index3.STL rename to data/MPL/mesh/index3.stl diff --git a/data/MPL/mesh/index3_collision.STL b/data/MPL/mesh/index3_collision.stl similarity index 100% rename from data/MPL/mesh/index3_collision.STL rename to data/MPL/mesh/index3_collision.stl diff --git a/data/MPL/mesh/middle0.STL b/data/MPL/mesh/middle0.stl similarity index 100% rename from data/MPL/mesh/middle0.STL rename to data/MPL/mesh/middle0.stl diff --git a/data/MPL/mesh/middle0_collision.STL b/data/MPL/mesh/middle0_collision.stl similarity index 100% rename from data/MPL/mesh/middle0_collision.STL rename to data/MPL/mesh/middle0_collision.stl diff --git a/data/MPL/mesh/middle1.STL b/data/MPL/mesh/middle1.stl similarity index 100% rename from data/MPL/mesh/middle1.STL rename to data/MPL/mesh/middle1.stl diff --git a/data/MPL/mesh/middle1_collision.STL b/data/MPL/mesh/middle1_collision.stl similarity index 100% rename from data/MPL/mesh/middle1_collision.STL rename to data/MPL/mesh/middle1_collision.stl diff --git a/data/MPL/mesh/middle2.STL b/data/MPL/mesh/middle2.stl similarity index 100% rename from data/MPL/mesh/middle2.STL rename to data/MPL/mesh/middle2.stl diff --git a/data/MPL/mesh/middle2_collision.STL b/data/MPL/mesh/middle2_collision.stl similarity index 100% rename from data/MPL/mesh/middle2_collision.STL rename to data/MPL/mesh/middle2_collision.stl diff --git a/data/MPL/mesh/middle3.STL b/data/MPL/mesh/middle3.stl similarity index 100% rename from data/MPL/mesh/middle3.STL rename to data/MPL/mesh/middle3.stl diff --git a/data/MPL/mesh/middle3_collision.STL b/data/MPL/mesh/middle3_collision.stl similarity index 100% rename from data/MPL/mesh/middle3_collision.STL rename to data/MPL/mesh/middle3_collision.stl diff --git a/data/MPL/mesh/palm.STL b/data/MPL/mesh/palm.stl similarity index 100% rename from data/MPL/mesh/palm.STL rename to data/MPL/mesh/palm.stl diff --git a/data/MPL/mesh/palm_collision.STL b/data/MPL/mesh/palm_collision.stl similarity index 100% rename from data/MPL/mesh/palm_collision.STL rename to data/MPL/mesh/palm_collision.stl diff --git a/data/MPL/mesh/pinky0.STL b/data/MPL/mesh/pinky0.stl similarity index 100% rename from data/MPL/mesh/pinky0.STL rename to data/MPL/mesh/pinky0.stl diff --git a/data/MPL/mesh/pinky0_collision.STL b/data/MPL/mesh/pinky0_collision.stl similarity index 100% rename from data/MPL/mesh/pinky0_collision.STL rename to data/MPL/mesh/pinky0_collision.stl diff --git a/data/MPL/mesh/pinky1.STL b/data/MPL/mesh/pinky1.stl similarity index 100% rename from data/MPL/mesh/pinky1.STL rename to data/MPL/mesh/pinky1.stl diff --git a/data/MPL/mesh/pinky1_collision.STL b/data/MPL/mesh/pinky1_collision.stl similarity index 100% rename from data/MPL/mesh/pinky1_collision.STL rename to data/MPL/mesh/pinky1_collision.stl diff --git a/data/MPL/mesh/pinky2.STL b/data/MPL/mesh/pinky2.stl similarity index 100% rename from data/MPL/mesh/pinky2.STL rename to data/MPL/mesh/pinky2.stl diff --git a/data/MPL/mesh/pinky2_collision.STL b/data/MPL/mesh/pinky2_collision.stl similarity index 100% rename from data/MPL/mesh/pinky2_collision.STL rename to data/MPL/mesh/pinky2_collision.stl diff --git a/data/MPL/mesh/pinky3.STL b/data/MPL/mesh/pinky3.stl similarity index 100% rename from data/MPL/mesh/pinky3.STL rename to data/MPL/mesh/pinky3.stl diff --git a/data/MPL/mesh/pinky3_collision.STL b/data/MPL/mesh/pinky3_collision.stl similarity index 100% rename from data/MPL/mesh/pinky3_collision.STL rename to data/MPL/mesh/pinky3_collision.stl diff --git a/data/MPL/mesh/ring0.STL b/data/MPL/mesh/ring0.stl similarity index 100% rename from data/MPL/mesh/ring0.STL rename to data/MPL/mesh/ring0.stl diff --git a/data/MPL/mesh/ring0_collision.STL b/data/MPL/mesh/ring0_collision.stl similarity index 100% rename from data/MPL/mesh/ring0_collision.STL rename to data/MPL/mesh/ring0_collision.stl diff --git a/data/MPL/mesh/ring1.STL b/data/MPL/mesh/ring1.stl similarity index 100% rename from data/MPL/mesh/ring1.STL rename to data/MPL/mesh/ring1.stl diff --git a/data/MPL/mesh/ring1_collision.STL b/data/MPL/mesh/ring1_collision.stl similarity index 100% rename from data/MPL/mesh/ring1_collision.STL rename to data/MPL/mesh/ring1_collision.stl diff --git a/data/MPL/mesh/ring2.STL b/data/MPL/mesh/ring2.stl similarity index 100% rename from data/MPL/mesh/ring2.STL rename to data/MPL/mesh/ring2.stl diff --git a/data/MPL/mesh/ring2_collision.STL b/data/MPL/mesh/ring2_collision.stl similarity index 100% rename from data/MPL/mesh/ring2_collision.STL rename to data/MPL/mesh/ring2_collision.stl diff --git a/data/MPL/mesh/ring3.STL b/data/MPL/mesh/ring3.stl similarity index 100% rename from data/MPL/mesh/ring3.STL rename to data/MPL/mesh/ring3.stl diff --git a/data/MPL/mesh/ring3_collision.STL b/data/MPL/mesh/ring3_collision.stl similarity index 100% rename from data/MPL/mesh/ring3_collision.STL rename to data/MPL/mesh/ring3_collision.stl diff --git a/data/MPL/mesh/thumb0.STL b/data/MPL/mesh/thumb0.stl similarity index 100% rename from data/MPL/mesh/thumb0.STL rename to data/MPL/mesh/thumb0.stl diff --git a/data/MPL/mesh/thumb0_collision.STL b/data/MPL/mesh/thumb0_collision.stl similarity index 100% rename from data/MPL/mesh/thumb0_collision.STL rename to data/MPL/mesh/thumb0_collision.stl diff --git a/data/MPL/mesh/thumb1.STL b/data/MPL/mesh/thumb1.stl similarity index 100% rename from data/MPL/mesh/thumb1.STL rename to data/MPL/mesh/thumb1.stl diff --git a/data/MPL/mesh/thumb1_collision.STL b/data/MPL/mesh/thumb1_collision.stl similarity index 100% rename from data/MPL/mesh/thumb1_collision.STL rename to data/MPL/mesh/thumb1_collision.stl diff --git a/data/MPL/mesh/thumb2.STL b/data/MPL/mesh/thumb2.stl similarity index 100% rename from data/MPL/mesh/thumb2.STL rename to data/MPL/mesh/thumb2.stl diff --git a/data/MPL/mesh/thumb2_collision.STL b/data/MPL/mesh/thumb2_collision.stl similarity index 100% rename from data/MPL/mesh/thumb2_collision.STL rename to data/MPL/mesh/thumb2_collision.stl diff --git a/data/MPL/mesh/thumb3.STL b/data/MPL/mesh/thumb3.stl similarity index 100% rename from data/MPL/mesh/thumb3.STL rename to data/MPL/mesh/thumb3.stl diff --git a/data/MPL/mesh/thumb3_collision.STL b/data/MPL/mesh/thumb3_collision.stl similarity index 100% rename from data/MPL/mesh/thumb3_collision.STL rename to data/MPL/mesh/thumb3_collision.stl diff --git a/data/MPL/mesh/wristx.STL b/data/MPL/mesh/wristx.stl similarity index 100% rename from data/MPL/mesh/wristx.STL rename to data/MPL/mesh/wristx.stl diff --git a/data/MPL/mesh/wristx_collision.STL b/data/MPL/mesh/wristx_collision.stl similarity index 100% rename from data/MPL/mesh/wristx_collision.STL rename to data/MPL/mesh/wristx_collision.stl diff --git a/data/MPL/mesh/wristy.STL b/data/MPL/mesh/wristy.stl similarity index 100% rename from data/MPL/mesh/wristy.STL rename to data/MPL/mesh/wristy.stl diff --git a/data/MPL/mesh/wristy_collision.STL b/data/MPL/mesh/wristy_collision.stl similarity index 100% rename from data/MPL/mesh/wristy_collision.STL rename to data/MPL/mesh/wristy_collision.stl diff --git a/data/MPL/mesh/wristz.STL b/data/MPL/mesh/wristz.stl similarity index 100% rename from data/MPL/mesh/wristz.STL rename to data/MPL/mesh/wristz.stl diff --git a/data/MPL/mesh/wristz_collision.STL b/data/MPL/mesh/wristz_collision.stl similarity index 100% rename from data/MPL/mesh/wristz_collision.STL rename to data/MPL/mesh/wristz_collision.stl From 4526b0a94ab8df6d19d590929645c5a97f4d24ac Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sat, 18 Mar 2017 04:10:07 +0300 Subject: [PATCH 3/9] MJCF: support for default joint limited="true" --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 96 ++++++++++++------- 1 file changed, 64 insertions(+), 32 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index d1a051fc8..0fb20ad28 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -152,6 +152,9 @@ struct BulletMJCFImporterInternalData int m_defaultCollisionMask; btScalar m_defaultCollisionMargin; + // joint defaults + std::string m_defaultJointLimited; + //those collision shapes are deleted by caller (todo: make sure this happens!) btAlignedObjectArray m_allocatedCollisionShapes; @@ -243,6 +246,17 @@ struct BulletMJCFImporterInternalData { parseAssets(child_xml,logger); } + if (n=="joint") + { + // Other attributes here: + // armature="1" + // damping="1" + // limited="true" + if (const char* conTypeStr = child_xml->Attribute("limited")) + { + m_defaultJointLimited = child_xml->Attribute("limited"); + } + } if (n=="geom") { //contype, conaffinity @@ -366,39 +380,57 @@ struct BulletMJCFImporterInternalData bool isLimited = false; double range[2] = {1,0}; + std::string lim = m_defaultJointLimited; if (limitedStr) { - std::string lim = limitedStr; - if (lim=="true") - { - isLimited = true; - //parse the 'range' field - btArray pieces; - btArray sizes; - btAlignedObjectArray strArray; - urdfIsAnyOf(" ", strArray); - urdfStringSplit(pieces, rangeStr, strArray); - for (int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - sizes.push_back(urdfLexicalCast(pieces[i].c_str())); - } - } - if (sizes.size()==2) - { - range[0] = sizes[0]; - range[1] = sizes[1]; - } else - { - logger->reportWarning("Expected range[2] in joint with limits"); - } - - } - } else - { -// logger->reportWarning("joint without limited field"); + lim = limitedStr; } + if (lim=="true") + { + isLimited = true; + //parse the 'range' field + btArray pieces; + btArray sizes; + btAlignedObjectArray strArray; + urdfIsAnyOf(" ", strArray); + urdfStringSplit(pieces, rangeStr, strArray); + for (int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + sizes.push_back(urdfLexicalCast(pieces[i].c_str())); + } + } + if (sizes.size()==2) + { + // TODO angle units are in " + range[0] = sizes[0] * M_PI / 180; + range[1] = sizes[1] * M_PI / 180; + } else + { + logger->reportWarning("Expected range[2] in joint with limits"); + } + } + + // TODO armature : real, "0" Armature inertia (or rotor inertia) of all + // degrees of freedom created by this joint. These are constants added to the + // diagonal of the inertia matrix in generalized coordinates. They make the + // simulation more stable, and often increase physical realism. This is because + // when a motor is attached to the system with a transmission that amplifies + // the motor force by c, the inertia of the rotor (i.e. the moving part of the + // motor) is amplified by c*c. The same holds for gears in the early stages of + // planetary gear boxes. These extra inertias often dominate the inertias of + // the robot parts that are represented explicitly in the model, and the + // armature attribute is the way to model them. + + // TODO damping : real, "0" Damping applied to all degrees of + // freedom created by this joint. Unlike friction loss + // which is computed by the constraint solver, damping is + // simply a force linear in velocity. It is included in + // the passive forces. Despite this simplicity, larger + // damping values can make numerical integrators unstable, + // which is why our Euler integrator handles damping + // implicitly. See Integration in the Computation chapter. bool jointHandled = false; const UrdfLink* linkPtr = getLink(modelIndex,linkIndex); @@ -448,11 +480,11 @@ struct BulletMJCFImporterInternalData jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform; jointPtr->m_type = ejtype; int numJoints = m_models[modelIndex]->m_joints.size(); - + //range jointPtr->m_lowerLimit = range[0]; jointPtr->m_upperLimit = range[1]; - + if (nameStr) { jointPtr->m_name =nameStr; From 41df15a46401a53772bcb651288312ae463c9020 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sun, 19 Mar 2017 00:19:04 +0300 Subject: [PATCH 4/9] MJCF: fix B3_PI, fix colors, fix capsule without 'fromto' --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 27 ++++++++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 18 +++---- .../Importers/ImportURDFDemo/UrdfParser.h | 13 +++-- .../TinyRendererVisualShapeConverter.cpp | 50 ++++++++++++------- 4 files changed, 75 insertions(+), 33 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 0fb20ad28..b1cbdb5d3 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -155,6 +155,9 @@ struct BulletMJCFImporterInternalData // joint defaults std::string m_defaultJointLimited; + // geom defaults + std::string m_defaultGeomRgba; + //those collision shapes are deleted by caller (todo: make sure this happens!) btAlignedObjectArray m_allocatedCollisionShapes; @@ -270,6 +273,11 @@ struct BulletMJCFImporterInternalData { m_defaultCollisionMask = urdfLexicalCast(conAffinityStr); } + const char* rgba = child_xml->Attribute("rgba"); + if (rgba) + { + m_defaultGeomRgba = rgba; + } } } handled=true; @@ -404,8 +412,8 @@ struct BulletMJCFImporterInternalData if (sizes.size()==2) { // TODO angle units are in " - range[0] = sizes[0] * M_PI / 180; - range[1] = sizes[1] * M_PI / 180; + range[0] = sizes[0] * B3_PI / 180; + range[1] = sizes[1] * B3_PI / 180; } else { logger->reportWarning("Expected range[2] in joint with limits"); @@ -526,10 +534,23 @@ struct BulletMJCFImporterInternalData -// const char* rgba = link_xml->Attribute("rgba"); const char* gType = link_xml->Attribute("type"); const char* sz = link_xml->Attribute("size"); const char* posS = link_xml->Attribute("pos"); + + std::string rgba = m_defaultGeomRgba; + if (const char* rgbattr = link_xml->Attribute("rgba")) + { + rgba = rgbattr; + } + if (!rgba.empty()) + { + // "0 0.7 0.7 1" + parseVector4(geom.m_localMaterial.m_rgbaColor, rgba); + geom.m_hasLocalMaterial = true; + geom.m_localMaterial.m_name = rgba; + } + if (posS) { btVector3 pos(0,0,0); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index ad5247710..cc188e74d 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -575,7 +575,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* if (name_char) visual.m_name = name_char; - visual.m_hasLocalMaterial = false; + visual.m_geometry.m_hasLocalMaterial = false; // Material TiXmlElement *mat = config->FirstChildElement("material"); @@ -597,7 +597,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* matPtr->m_rgbaColor = rgba; visual.m_materialName = matPtr->m_name; - visual.m_hasLocalMaterial = true; + visual.m_geometry.m_hasLocalMaterial = true; } } else @@ -616,11 +616,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* TiXmlElement *c = mat->FirstChildElement("color"); if (t||c) { - if (parseMaterial(visual.m_localMaterial, mat,logger)) + if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger)) { - UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); + UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial); model.m_materials.insert(matPtr->m_name.c_str(),matPtr); - visual.m_hasLocalMaterial = true; + visual.m_geometry.m_hasLocalMaterial = true; } } } @@ -1408,12 +1408,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) + if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) { - vis.m_localMaterial = **mat; + vis.m_geometry.m_localMaterial = **mat; } else { //logger->reportError("Cannot find material with name:"); @@ -1603,12 +1603,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) + if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) { - vis.m_localMaterial = **mat; + vis.m_geometry.m_localMaterial = **mat; } else { //logger->reportError("Cannot find material with name:"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 6c2e0e090..1967cd729 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -18,9 +18,13 @@ struct ErrorLogger struct UrdfMaterial { - std::string m_name; + std::string m_name; std::string m_textureFilename; - btVector4 m_rgbaColor; + btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a + UrdfMaterial(): + m_rgbaColor(0.8, 0.8, 0.8, 1) + { + } }; struct UrdfInertia @@ -79,6 +83,9 @@ struct UrdfGeometry int m_meshFileType; std::string m_meshFileName; btVector3 m_meshScale; + + UrdfMaterial m_localMaterial; + bool m_hasLocalMaterial; }; bool findExistingMeshFile(const std::string& urdf_path, std::string fn, @@ -96,8 +103,6 @@ struct UrdfShape struct UrdfVisual: UrdfShape { std::string m_materialName; - bool m_hasLocalMaterial; - UrdfMaterial m_localMaterial; }; struct UrdfCollision: UrdfShape diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 58fd7f13e..a3631e9e2 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi visualShapeOut.m_dimensions[1] = 0; visualShapeOut.m_dimensions[2] = 0; visualShapeOut.m_meshAssetFileName[0] = 0; + if (visual->m_geometry.m_hasLocalMaterial) { + visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0]; + visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1]; + visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2]; + visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3]; + } GLInstanceGraphicsShape* glmesh = 0; @@ -215,23 +221,33 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi { 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); + btTransform tr; + tr.setIdentity(); + btScalar rad, len; + if (visual->m_geometry.m_hasFromTo) { + btVector3 v = p2 - p1; + btVector3 center = (p2 + p1) * 0.5; + 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); + tr = visual->m_linkLocalFrame * capsule_orient; + len = v.length(); + rad = visual->m_geometry.m_capsuleRadius; + } else { + tr = visual->m_linkLocalFrame; + len = visual->m_geometry.m_capsuleHalfHeight; + rad = visual->m_geometry.m_capsuleRadius; } - 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]; @@ -239,7 +255,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi 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[0] = len; visualShapeOut.m_dimensions[1] = rad; break; } From 6695268fbaa5f987d2c44bf43f988c3749ea3820 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 20 Mar 2017 09:52:24 -0700 Subject: [PATCH 5/9] Fix the issue with indentation in pr2 setup. --- examples/pybullet/vr_kuka_pr2_move.py | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/examples/pybullet/vr_kuka_pr2_move.py b/examples/pybullet/vr_kuka_pr2_move.py index 22e5730c1..53dbe844d 100644 --- a/examples/pybullet/vr_kuka_pr2_move.py +++ b/examples/pybullet/vr_kuka_pr2_move.py @@ -15,14 +15,8 @@ BUTTONS=6 gripper_max_joint = 0.550569 while True: events = p.getVREvents() - - for e in (events): - if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes - p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) - p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL, - targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint, - force=1.0) - p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL, - targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint, - force=1.1) - \ No newline at end of file + for e in (events): + if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes + p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) + p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.0) + p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.1) \ No newline at end of file From 753f1d3d2f447ba2435edbf677fb1e4e04338d23 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 20 Mar 2017 10:16:15 -0700 Subject: [PATCH 6/9] Add lateral and spinning friction coefficients to gripper sdf model. --- data/gripper/wsg50_with_r2d2_gripper.sdf | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/data/gripper/wsg50_with_r2d2_gripper.sdf b/data/gripper/wsg50_with_r2d2_gripper.sdf index b022faf00..d67959f19 100644 --- a/data/gripper/wsg50_with_r2d2_gripper.sdf +++ b/data/gripper/wsg50_with_r2d2_gripper.sdf @@ -215,6 +215,10 @@ + + 1.0 + 1.5 + 0.062 0 0.145 0 0 1.5708 0.2 @@ -255,6 +259,10 @@ + + 1.0 + 1.5 + -0.062 0 0.145 0 0 4.71239 0.2 From 80a0b51a1de630eed2da187a0c2ca76551b7b774 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Wed, 22 Mar 2017 00:36:28 +0300 Subject: [PATCH 7/9] MJCF: fix cylinders in MJCF, also (possibly) fixes capsules in tiny renderer --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 18 ++------ .../ImportURDFDemo/BulletUrdfImporter.cpp | 8 ++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 5 +- .../Importers/ImportURDFDemo/UrdfParser.h | 3 -- .../TinyRendererVisualShapeConverter.cpp | 46 +++++++++---------- 5 files changed, 32 insertions(+), 48 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index b1cbdb5d3..563c657a4 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -618,10 +618,10 @@ struct BulletMJCFImporterInternalData handledGeomType = true; } - //todo: capsule, cylinder, meshes or heightfields etc - if (geomType == "capsule") + if (geomType == "capsule" || geomType == "cylinder") { - geom.m_type = URDF_GEOM_CAPSULE; + // + geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE; btArray pieces; btArray sizes; @@ -695,13 +695,6 @@ struct BulletMJCFImporterInternalData } } } - #if 0 - if (geomType == "cylinder") - { - geom.m_type = URDF_GEOM_CYLINDER; - handledGeomType = true; - } -#endif if (handledGeomType) { @@ -866,6 +859,7 @@ struct BulletMJCFImporterInternalData return orgChildLinkIndex; } + bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger) { int newParentLinkIndex = orgParentLinkIndex; @@ -1027,10 +1021,6 @@ struct BulletMJCFImporterInternalData } linkPtr->m_linkTransformInWorld = linkTransform; - if (bodyN == "cart1")//front_left_leg") - { - printf("found!\n"); - } if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint) { //linkPtr->m_linkTransformInWorld.setIdentity(); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 2a60cb197..5ba817b51 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -562,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co case URDF_GEOM_CYLINDER: { - btScalar cylRadius = collision->m_geometry.m_cylinderRadius; - btScalar cylLength = collision->m_geometry.m_cylinderLength; + btScalar cylRadius = collision->m_geometry.m_capsuleRadius; + btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight; btAlignedObjectArray vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); @@ -785,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha for (int i = 0; im_geometry.m_cylinderRadius; - btScalar cylLength = visual->m_geometry.m_cylinderLength; + btScalar cylRadius = visual->m_geometry.m_capsuleRadius; + btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight; btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); vertices.push_back(vert); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index cc188e74d..cbbc481c2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger->reportError("Cylinder shape must have both length and radius attributes"); return false; } - geom.m_cylinderRadius = urdfLexicalCast(shape->Attribute("radius")); - geom.m_cylinderLength = urdfLexicalCast(shape->Attribute("length")); + geom.m_hasFromTo = false; + geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHalfHeight = urdfLexicalCast(shape->Attribute("length")); } else if (type_name == "capsule") diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1967cd729..015342a0a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -70,9 +70,6 @@ struct UrdfGeometry btVector3 m_capsuleFrom; btVector3 m_capsuleTo; - double m_cylinderRadius; - double m_cylinderLength; - btVector3 m_planeNormal; enum { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index a3631e9e2..520ca6710 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -192,31 +192,6 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi switch (visual->m_geometry.m_type) { case URDF_GEOM_CYLINDER: - { - btAlignedObjectArray vertices; - - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength; - visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius; - - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i = 0; im_geometry.m_cylinderRadius; - btScalar cylLength = visual->m_geometry.m_cylinderLength; - - btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); - vertices.push_back(vert); - vert[2] = -cylLength / 2.; - vertices.push_back(vert); - } - - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); - convexColShape = cylZShape; - break; - } case URDF_GEOM_CAPSULE: { btVector3 p1 = visual->m_geometry.m_capsuleFrom; @@ -257,6 +232,27 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; visualShapeOut.m_dimensions[0] = len; visualShapeOut.m_dimensions[1] = rad; + + btAlignedObjectArray vertices; + int numSteps = 32; + for (int i = 0; im_geometry.m_type==URDF_GEOM_CAPSULE) { + // TODO: check if tiny renderer works with that, didn't check -- Oleg + btVector3 pole1(0, 0, + len / 2. + rad); + btVector3 pole2(0, 0, - len / 2. - rad); + vertices.push_back(pole1); + vertices.push_back(pole2); + } + + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(0.001); + convexColShape = cylZShape; break; } case URDF_GEOM_BOX: From 049d9a5ca66c75cb25d22972b11bd9fdca7047cd Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 21 Mar 2017 17:41:11 -0700 Subject: [PATCH 8/9] Update OpenGLExampleBrowser.cpp revert original width/height to 1024x768 --- examples/ExampleBrowser/OpenGLExampleBrowser.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index df2a83021..42250d589 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -869,8 +869,8 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) } - int width = 1280; - int height=640; + int width = 1024; + int height=768; #ifndef NO_OPENGL3 SimpleOpenGL3App* simpleApp=0; sUseOpenGL2 =args.CheckCmdLineFlag("opengl2"); From f4986707989817a70973fb43f6976c8b98cb1890 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 21 Mar 2017 18:10:50 -0700 Subject: [PATCH 9/9] :%s/STL/stl/g for Bullet/data/MPL/MPL.xml --- data/MPL/MPL.xml | 48 ++++++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/data/MPL/MPL.xml b/data/MPL/MPL.xml index 7ed5170ff..2b8408323 100644 --- a/data/MPL/MPL.xml +++ b/data/MPL/MPL.xml @@ -49,30 +49,30 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + +