diff --git a/data/humanoid.urdf b/data/humanoid.urdf
new file mode 100644
index 000000000..b4f991241
--- /dev/null
+++ b/data/humanoid.urdf
@@ -0,0 +1,586 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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")
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 5e61b5c89..c025ee9cd 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -6464,7 +6464,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
{
pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation);
}
- b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
+ b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);