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

pybullet_utils.urdfEditor

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -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;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

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

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

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