MJCF: fix cylinders in MJCF, also (possibly) fixes capsules in tiny renderer

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -618,10 +618,10 @@ struct BulletMJCFImporterInternalData
 				handledGeomType = true;
 			}
 
-			//todo: capsule, cylinder, meshes or heightfields etc
+			if (geomType == "capsule" || geomType == "cylinder")
-			if (geomType == "capsule")
 			{
-				geom.m_type = URDF_GEOM_CAPSULE;
+				// <geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
+				geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE;
 
 				btArray<std::string> pieces;
 				btArray<float> sizes;
@@ -695,13 +695,6 @@ struct BulletMJCFImporterInternalData
 					}
 				}
 			}
-			#if 0
-			if (geomType == "cylinder")
-			{
-				geom.m_type = URDF_GEOM_CYLINDER;
-				handledGeomType = true;
-			}
-#endif
 			if (handledGeomType)
 			{
 						
@@ -866,6 +859,7 @@ struct BulletMJCFImporterInternalData
 
 		return orgChildLinkIndex;
 	}
+
 	bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
 	{
 		int newParentLinkIndex = orgParentLinkIndex;
@@ -1027,10 +1021,6 @@ struct BulletMJCFImporterInternalData
 		}
 
 		linkPtr->m_linkTransformInWorld = linkTransform;
-		if (bodyN == "cart1")//front_left_leg")
-		{
-			printf("found!\n");
-		}
 		if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
 		{
 			//linkPtr->m_linkTransformInWorld.setIdentity();

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -562,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
         case URDF_GEOM_CYLINDER:
         {
-			btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
+			btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
-			btScalar cylLength = collision->m_geometry.m_cylinderLength;
+			btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
 			
             btAlignedObjectArray<btVector3> vertices;
             //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
@@ -785,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 			for (int i = 0; i<numSteps; i++)
 			{
 
-				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
+				btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
-				btScalar cylLength = visual->m_geometry.m_cylinderLength;
+				btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
 				
 				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
 				vertices.push_back(vert);

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
 		  logger->reportError("Cylinder shape must have both length and radius attributes");
 		  return false;
 	  }
-		geom.m_cylinderRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
+		geom.m_hasFromTo = false;
-		geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
+		geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
+		geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
 		
 	}
 	else if (type_name == "capsule")

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -70,9 +70,6 @@ struct UrdfGeometry
 	btVector3 m_capsuleFrom;
 	btVector3 m_capsuleTo;
 
-	double m_cylinderRadius;
-	double m_cylinderLength;
-
 	btVector3 m_planeNormal;
     
 	enum {

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -192,31 +192,6 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 	switch (visual->m_geometry.m_type)
 	{
 		case URDF_GEOM_CYLINDER:
-		{
-			btAlignedObjectArray<btVector3> vertices;
-		
-			visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
-			visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
-			
-			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
-			int numSteps = 32;
-			for (int i = 0; i<numSteps; i++)
-			{
-
-				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
-				btScalar cylLength = visual->m_geometry.m_cylinderLength;
-				
-				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
-				vertices.push_back(vert);
-				vert[2] = -cylLength / 2.;
-				vertices.push_back(vert);
-			}
-
-			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-			cylZShape->setMargin(0.001);
-			convexColShape = cylZShape;
-			break;
-		}
 		case URDF_GEOM_CAPSULE:
 		{
 			btVector3 p1 = visual->m_geometry.m_capsuleFrom;
@@ -257,6 +232,27 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 			visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3];
 			visualShapeOut.m_dimensions[0] = len;
 			visualShapeOut.m_dimensions[1] = rad;
+
+			btAlignedObjectArray<btVector3> vertices;
+			int numSteps = 32;
+			for (int i = 0; i<numSteps; i++)
+			{
+				btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
+				vertices.push_back(vert);
+				vert[2] = -len / 2.;
+				vertices.push_back(vert);
+			}
+			if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) {
+				// TODO: check if tiny renderer works with that, didn't check -- Oleg
+				btVector3 pole1(0, 0, + len / 2. + rad);
+				btVector3 pole2(0, 0, - len / 2. - rad);
+				vertices.push_back(pole1);
+				vertices.push_back(pole2);
+			}
+
+			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+			cylZShape->setMargin(0.001);
+			convexColShape = cylZShape;
 			break;
 		}
 		case URDF_GEOM_BOX: