Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add rudimentary MuJoCo mjcf xml to ROS URDF file, based on

Browse Source 
pybullet_utils.urdfEditor
This commit is contained in:
Erwin Coumans
2018-10-31 11:02:19 -07:00
parent 624212c641
commit 8e82de1b00
4 changed files with 74 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -2,8 +2,8 @@
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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); m_urdfParser.setGlobalScaling(scaling);
} }
}; };
void BulletURDFImporter::printTree() void BulletURDFImporter::printTree()
@@ -160,11 +160,11 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
//read file //read file
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
char destBuffer[8192]; char destBuffer[8192];
char* line = 0; char* line = 0;
do do
{ {
line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
if (line) if (line)
@@ -190,7 +190,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
BulletErrorLogger loggie; BulletErrorLogger loggie;
m_data->m_urdfParser.setParseSDF(false); m_data->m_urdfParser.setParseSDF(false);
bool result = false; bool result = false;
if (xml_string.length()) if (xml_string.length())
{ {
result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS)); 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 //read file
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
char destBuffer[8192]; char destBuffer[8192];
char* line = 0; char* line = 0;
do do
{ {
line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
if (line) if (line)
@@ -883,6 +883,15 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
switch (visual->m_geometry.m_type) 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: case URDF_GEOM_CYLINDER:
{ {
btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<btVector3> vertices;
@@ -932,7 +941,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
{ {
case UrdfGeometry::FILE_OBJ: case UrdfGeometry::FILE_OBJ:
{ {
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
{ {
if (meshData.m_textureImage1) 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)) (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
{ {
UrdfMaterialColor matCol; UrdfMaterialColor matCol;
if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{ {
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
@@ -1186,7 +1195,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
meshData.m_rgbaColor[2], meshData.m_rgbaColor[2],
1); 1);
} }
matCol.m_specularColor.setValue(meshData.m_specularColor[0], matCol.m_specularColor.setValue(meshData.m_specularColor[0],
meshData.m_specularColor[1], meshData.m_specularColor[1],
meshData.m_specularColor[2]); meshData.m_specularColor[2]);
@@ -1204,7 +1213,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
m_data->m_linkColors.insert(linkIndex, matCol); m_data->m_linkColors.insert(linkIndex, matCol);
} }
} }
} }
} }
if (vertices.size() && indices.size()) if (vertices.size() && indices.size())

34
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1652,7 +1652,7 @@ struct PhysicsServerCommandProcessorInternalData
std::string m_profileTimingFileName; std::string m_profileTimingFileName;
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey; int m_sharedMemoryKey;
bool m_enableTinyRenderer; bool m_enableTinyRenderer;
@@ -1777,7 +1777,7 @@ struct PhysicsServerCommandProcessorInternalData
} }
#endif #endif
m_vrControllerEvents.init(); m_vrControllerEvents.init();
@@ -2271,7 +2271,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
double globalScaling = 1.f; //todo! double globalScaling = 1.f; //todo!
int flags = 0; int flags = 0;
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(),fileIO, globalScaling, flags); BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(),fileIO, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
@@ -2755,7 +2755,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
SaveWorldObjectData sd; SaveWorldObjectData sd;
sd.m_fileName = fileName; sd.m_fileName = fileName;
for (int m = 0; m < u2b.getNumModels(); m++) for (int m = 0; m < u2b.getNumModels(); m++)
{ {
@@ -2971,7 +2971,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
} }
{ {
if (m_data->m_pluginManager.getRenderInterface()) if (m_data->m_pluginManager.getRenderInterface())
{ {
@@ -4267,7 +4267,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES; urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
break; break;
} }
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (foundFile) 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 batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays);
batchRayCaster.castRays(numThreads); batchRayCaster.castRays(numThreads);
@@ -6770,7 +6770,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
useMultiBody = false; useMultiBody = false;
} }
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); 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 = ""; const std::string& error_message_prefix = "";
std::string out_found_filename; std::string out_found_filename;
int out_type; int out_type;
bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
std::vector<tinyobj::shape_t> shapes; std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
@@ -9642,6 +9642,16 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape
switch (colShape->getShapeType()) 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: case CONVEX_HULL_SHAPE_PROXYTYPE:
{ {
UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape); UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
@@ -10054,7 +10064,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
{ {
int width, height, n; int width, height, n;
unsigned char* imageData = 0; unsigned char* imageData = 0;
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
if (fileIO) if (fileIO)
{ {
@@ -10083,7 +10093,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
{ {
imageData = stbi_load(relativeFileName, &width, &height, &n, 3); imageData = stbi_load(relativeFileName, &width, &height, &n, 3);
} }
if (imageData) if (imageData)
{ {
texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height); texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height);
@@ -10161,7 +10171,7 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
buffer.reserve(1024); buffer.reserve(1024);
if (fileIO) if (fileIO)
{ {
int fileId = -1; int fileId = -1;
found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
if (found) if (found)

21
examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py Normal file
View File

@@ -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")

12
examples/pybullet/gym/pybullet_utils/urdfEditor.py
View File

@@ -1,7 +1,6 @@
import pybullet as p import pybullet as p
import time import time
class UrdfInertial(object): class UrdfInertial(object):
def __init__(self): def __init__(self):
self.mass = 1 self.mass = 1
@@ -201,6 +200,10 @@ class UrdfEditor(object):
file.write("\t\t</inertial>\n") file.write("\t\t</inertial>\n")
def writeVisualShape(self,file,urdfVisual, precision=5): 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<visual>\n") file.write("\t\t<visual>\n")
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\ str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2], urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2],
@@ -276,8 +279,13 @@ class UrdfEditor(object):
file.write("\">\n") file.write("\">\n")
self.writeInertial(file,urdfLink.urdf_inertial) self.writeInertial(file,urdfLink.urdf_inertial)
hasCapsules = False
for v in urdfLink.urdf_visual_shapes: 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: for c in urdfLink.urdf_collision_shapes:
self.writeCollisionShape(file,c) self.writeCollisionShape(file,c)
file.write("\t</link>\n") file.write("\t</link>\n")