From 8e82de1b008cd6275b8800243da5a06bd1a1425b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 31 Oct 2018 11:02:19 -0700 Subject: [PATCH] add rudimentary MuJoCo mjcf xml to ROS URDF file, based on pybullet_utils.urdfEditor --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 33 +++++++++++------- .../PhysicsServerCommandProcessor.cpp | 34 ++++++++++++------- .../gym/pybullet_utils/examples/mjcf2urdf.py | 21 ++++++++++++ .../pybullet/gym/pybullet_utils/urdfEditor.py | 12 +++++-- 4 files changed, 74 insertions(+), 26 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 943b26ddd..9add4bddc 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -2,8 +2,8 @@ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -85,7 +85,7 @@ BulletURDFInternalData m_urdfParser.setGlobalScaling(scaling); } - + }; void BulletURDFImporter::printTree() @@ -160,11 +160,11 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) //read file int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); - + char destBuffer[8192]; char* line = 0; - do + do { line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); if (line) @@ -190,7 +190,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) BulletErrorLogger loggie; m_data->m_urdfParser.setParseSDF(false); bool result = false; - + if (xml_string.length()) { result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS)); @@ -235,10 +235,10 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) //read file int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); - + char destBuffer[8192]; char* line = 0; - do + do { line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); if (line) @@ -883,6 +883,15 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu switch (visual->m_geometry.m_type) { + case URDF_GEOM_CAPSULE: + { + btScalar radius = visual->m_geometry.m_capsuleRadius; + btScalar height = visual->m_geometry.m_capsuleHeight; + btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height); + convexColShape = capsuleShape; + convexColShape->setMargin(gUrdfDefaultCollisionMargin); + break; + } case URDF_GEOM_CYLINDER: { btAlignedObjectArray vertices; @@ -932,7 +941,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu { case UrdfGeometry::FILE_OBJ: { - + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) { if (meshData.m_textureImage1) @@ -1172,7 +1181,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) { UrdfMaterialColor matCol; - + if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) { matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], @@ -1186,7 +1195,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP meshData.m_rgbaColor[2], 1); } - + matCol.m_specularColor.setValue(meshData.m_specularColor[0], meshData.m_specularColor[1], meshData.m_specularColor[2]); @@ -1204,7 +1213,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP m_data->m_linkColors.insert(linkIndex, matCol); } } - + } } if (vertices.size() && indices.size()) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d251ca53e..537f3e555 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1652,7 +1652,7 @@ struct PhysicsServerCommandProcessorInternalData std::string m_profileTimingFileName; struct GUIHelperInterface* m_guiHelper; - + int m_sharedMemoryKey; bool m_enableTinyRenderer; @@ -1777,7 +1777,7 @@ struct PhysicsServerCommandProcessorInternalData } #endif - + m_vrControllerEvents.init(); @@ -2271,7 +2271,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface double globalScaling = 1.f; //todo! int flags = 0; CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); - + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(),fileIO, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); @@ -2755,7 +2755,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, SaveWorldObjectData sd; sd.m_fileName = fileName; - + for (int m = 0; m < u2b.getNumModels(); m++) { @@ -2971,7 +2971,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, } { - + if (m_data->m_pluginManager.getRenderInterface()) { @@ -4267,7 +4267,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES; break; } - + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); if (foundFile) { @@ -5210,7 +5210,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co } } } - + BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); batchRayCaster.castRays(numThreads); @@ -6770,7 +6770,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S useMultiBody = false; } - + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); @@ -6912,7 +6912,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar const std::string& error_message_prefix = ""; std::string out_found_filename; int out_type; - + bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); std::vector shapes; std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); @@ -9642,6 +9642,16 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape switch (colShape->getShapeType()) { + case STATIC_PLANE_PROXYTYPE: + { + btStaticPlaneShape* plane = (btStaticPlaneShape*) colShape; + collisionShapeBuffer[0].m_collisionGeometryType = GEOM_PLANE; + collisionShapeBuffer[0].m_dimensions[0] = plane->getPlaneNormal()[0]; + collisionShapeBuffer[0].m_dimensions[1] = plane->getPlaneNormal()[1]; + collisionShapeBuffer[0].m_dimensions[2] = plane->getPlaneNormal()[2]; + numConverted += 1; + break; + } case CONVEX_HULL_SHAPE_PROXYTYPE: { UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape); @@ -10054,7 +10064,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share { int width, height, n; unsigned char* imageData = 0; - + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); if (fileIO) { @@ -10083,7 +10093,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share { imageData = stbi_load(relativeFileName, &width, &height, &n, 3); } - + if (imageData) { texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height); @@ -10161,7 +10171,7 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar buffer.reserve(1024); if (fileIO) { - + int fileId = -1; found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); if (found) diff --git a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py new file mode 100644 index 000000000..a4fc7ca0e --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py @@ -0,0 +1,21 @@ +#rudimentary MuJoCo mjcf to ROS URDF converter using the UrdfEditor + +import pybullet_utils.bullet_client as bc +import pybullet_data as pd + +import pybullet_utils.urdfEditor as ed +import argparse +parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) +parser.add_argument('--mjcf', help='MuJoCo xml file to be converted to URDF', default='mjcf/humanoid.xml') +args = parser.parse_args() + +p = bc.BulletClient() +p.setAdditionalSearchPath(pd.getDataPath()) +objs = p.loadMJCF(args.mjcf, flags=p.URDF_USE_IMPLICIT_CYLINDER) + +for o in objs: + print("o=",o, p.getBodyInfo(o), p.getNumJoints(o)) + humanoid = objs[o] + ed0 = ed.UrdfEditor() + ed0.initializeFromBulletBody(humanoid, p._client) + ed0.saveUrdf(p.getBodyInfo(0)[1]+"_"+p.getBodyInfo(o)[0]+".urdf") diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index b29ce6c60..6b05ce72c 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -1,7 +1,6 @@ import pybullet as p import time - class UrdfInertial(object): def __init__(self): self.mass = 1 @@ -201,6 +200,10 @@ class UrdfEditor(object): file.write("\t\t\n") def writeVisualShape(self,file,urdfVisual, precision=5): + #we don't support loading capsule types from visuals, so auto-convert from collision shape + if urdfVisual.geom_type == p.GEOM_CAPSULE: + return + file.write("\t\t\n") str = '\t\t\t\n'.format(\ urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2], @@ -276,8 +279,13 @@ class UrdfEditor(object): file.write("\">\n") self.writeInertial(file,urdfLink.urdf_inertial) + hasCapsules = False for v in urdfLink.urdf_visual_shapes: - self.writeVisualShape(file,v) + if (v.geom_type == p.GEOM_CAPSULE): + hasCapsules = True + if (not hasCapsules): + for v in urdfLink.urdf_visual_shapes: + self.writeVisualShape(file,v) for c in urdfLink.urdf_collision_shapes: self.writeCollisionShape(file,c) file.write("\t\n")