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.
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<btVector3> 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())

34
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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<tinyobj::shape_t> 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)

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 time
class UrdfInertial(object):
def __init__(self):
self.mass = 1
@@ -201,6 +200,10 @@ class UrdfEditor(object):
file.write("\t\t</inertial>\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<visual>\n")
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],
@@ -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</link>\n")