Merge pull request #1025 from olegklimov/master

URDF loader improvement 2

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -30,12 +30,6 @@
 
 #include <vector>
 
-enum eMJCF_FILE_TYPE_ENUMS
-{
-	MJCF_FILE_STL = 1,
-	MJCF_FILE_OBJ = 2
-};
-
 enum ePARENT_LINK_ENUMS
 {
 	BASE_LINK_INDEX=-1,
@@ -137,9 +131,11 @@ struct MyMJCFAsset
 struct BulletMJCFImporterInternalData
 {
 	GUIHelperInterface* m_guiHelper;
+	struct LinkVisualShapesConverter* m_customVisualShapesConverter;
 	char m_pathPrefix[1024];
 
-	std::string m_fileModelName;
+	std::string m_sourceFileName; // with path
+	std::string m_fileModelName;  // without path
 	btHashMap<btHashString,MyMJCFAsset> m_assets;
 
 	btAlignedObjectArray<UrdfModel*>	m_models;
@@ -150,16 +146,24 @@ struct BulletMJCFImporterInternalData
 
 
 	int m_activeModel;
+	int m_activeBodyUniqueId;
 	//todo: for full MJCF compatibility, we would need a stack of default values
 	int m_defaultCollisionGroup;
 	int m_defaultCollisionMask;
 	btScalar m_defaultCollisionMargin;
 
+	// joint defaults
+	std::string m_defaultJointLimited;
+
+	// geom defaults
+	std::string m_defaultGeomRgba;
+
 	//those collision shapes are deleted by caller (todo: make sure this happens!)
 	btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
 
 	BulletMJCFImporterInternalData()
 		:m_activeModel(-1),
+		m_activeBodyUniqueId(-1),
 		m_defaultCollisionGroup(1),
 		m_defaultCollisionMask(1),
 		m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
@@ -167,6 +171,13 @@ struct BulletMJCFImporterInternalData
 		m_pathPrefix[0] = 0;
 	}
 
+	std::string sourceFileLocation(TiXmlElement* e)
+	{
+		char buf[1024];
+		snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row());
+		return buf;
+	}
+	
 	const UrdfLink* getLink(int modelIndex, int linkIndex) const
 	{
 		if (modelIndex>=0 && modelIndex<m_models.size())
@@ -238,6 +249,17 @@ struct BulletMJCFImporterInternalData
 			{
 				parseAssets(child_xml,logger);
 			}
+			if (n=="joint")
+			{
+				// Other attributes here:
+				// armature="1"
+				// damping="1"
+				// limited="true"
+				if (const char* conTypeStr = child_xml->Attribute("limited"))
+				{
+					m_defaultJointLimited = child_xml->Attribute("limited");
+				}
+			}
 			if (n=="geom")
 			{
 				//contype, conaffinity 
@@ -251,6 +273,11 @@ struct BulletMJCFImporterInternalData
 				{
 					m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
 				}
+				const char* rgba = child_xml->Attribute("rgba");
+				if (rgba)
+				{
+					m_defaultGeomRgba = rgba;
+				}
 			}
 		}
 		handled=true;
@@ -361,39 +388,57 @@ struct BulletMJCFImporterInternalData
 		bool isLimited = false;
 		double range[2] = {1,0};
 
+		std::string lim = m_defaultJointLimited;
 		if (limitedStr)
 		{
-			std::string lim = limitedStr;
-			if (lim=="true")
-			{
-				isLimited = true;
-				//parse the 'range' field
-				btArray<std::string> pieces;
-				btArray<float> sizes;
-				btAlignedObjectArray<std::string> strArray;
-				urdfIsAnyOf(" ", strArray);
-				urdfStringSplit(pieces, rangeStr, strArray);
-				for (int i = 0; i < pieces.size(); ++i)
-				{
-					if (!pieces[i].empty())
-					{
-						sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
-					}
-				}
-				if (sizes.size()==2)
-				{
-					range[0] = sizes[0];
-					range[1] = sizes[1];
-				} else
-				{
-					logger->reportWarning("Expected range[2] in joint with limits");
-				}
-
-			}
-		} else
-		{
-//			logger->reportWarning("joint without limited field");
+			lim = limitedStr;
 		}
+		if (lim=="true")
+		{
+			isLimited = true;
+			//parse the 'range' field
+			btArray<std::string> pieces;
+			btArray<float> sizes;
+			btAlignedObjectArray<std::string> strArray;
+			urdfIsAnyOf(" ", strArray);
+			urdfStringSplit(pieces, rangeStr, strArray);
+			for (int i = 0; i < pieces.size(); ++i)
+			{
+				if (!pieces[i].empty())
+				{
+					sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
+				}
+			}
+			if (sizes.size()==2)
+			{
+				// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
+				range[0] = sizes[0] * B3_PI / 180;
+				range[1] = sizes[1] * B3_PI / 180;
+			} else
+			{
+				logger->reportWarning("Expected range[2] in joint with limits");
+			}
+		}
+
+		// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
+		// degrees of freedom created by this joint. These are constants added to the
+		// diagonal of the inertia matrix in generalized coordinates. They make the
+		// simulation more stable, and often increase physical realism. This is because
+		// when a motor is attached to the system with a transmission that amplifies
+		// the motor force by c, the inertia of the rotor (i.e. the moving part of the
+		// motor) is amplified by c*c. The same holds for gears in the early stages of
+		// planetary gear boxes. These extra inertias often dominate the inertias of
+		// the robot parts that are represented explicitly in the model, and the
+		// armature attribute is the way to model them.
+
+		// TODO damping : real, "0" Damping applied to all degrees of
+		// freedom created by this joint. Unlike friction loss
+		// which is computed by the constraint solver, damping is
+		// simply a force linear in velocity. It is included in
+		// the passive forces. Despite this simplicity, larger
+		// damping values can make numerical integrators unstable,
+		// which is why our Euler integrator handles damping
+		// implicitly. See Integration in the Computation chapter.
 
 		bool jointHandled = false;
 		const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
@@ -489,10 +534,23 @@ struct BulletMJCFImporterInternalData
 
 		
 
-//		const char* rgba = link_xml->Attribute("rgba");
 		const char* gType = link_xml->Attribute("type");
 		const char* sz = link_xml->Attribute("size");
 		const char* posS = link_xml->Attribute("pos");
+
+		std::string rgba = m_defaultGeomRgba;
+		if (const char* rgbattr = link_xml->Attribute("rgba"))
+		{
+			rgba = rgbattr;
+		}
+		if (!rgba.empty())
+		{
+			// "0 0.7 0.7 1"
+			parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
+			geom.m_hasLocalMaterial = true;
+			geom.m_localMaterial.m_name = rgba;
+		}
+
 		if (posS)
 		{
 			btVector3 pos(0,0,0);
@@ -560,10 +618,10 @@ struct BulletMJCFImporterInternalData
 				handledGeomType = true;
 			}
 
-			//todo: capsule, cylinder, meshes or heightfields etc
-			if (geomType == "capsule")
+			if (geomType == "capsule" || geomType == "cylinder")
 			{
-				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;
@@ -621,9 +679,14 @@ struct BulletMJCFImporterInternalData
 					MyMJCFAsset* assetPtr = m_assets[meshStr];
 					if (assetPtr)
 					{
-						handledGeomType = true;
 						geom.m_type = URDF_GEOM_MESH;
 						geom.m_meshFileName = assetPtr->m_fileName;
+						bool exists = findExistingMeshFile(
+							m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
+							&geom.m_meshFileName,
+							&geom.m_meshFileType);
+						handledGeomType = exists;
+
 						geom.m_meshScale.setValue(1,1,1);
 						//todo: parse mesh scale
 						if (sz)
@@ -632,13 +695,6 @@ struct BulletMJCFImporterInternalData
 					}
 				}
 			}
-			#if 0
-			if (geomType == "cylinder")
-			{
-				geom.m_type = URDF_GEOM_CYLINDER;
-				handledGeomType = true;
-			}
-#endif
 			if (handledGeomType)
 			{
 						
@@ -803,6 +859,7 @@ struct BulletMJCFImporterInternalData
 
 		return orgChildLinkIndex;
 	}
+
 	bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
 	{
 		int newParentLinkIndex = orgParentLinkIndex;
@@ -964,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();
@@ -1113,10 +1166,11 @@ struct BulletMJCFImporterInternalData
 
 };
 
-BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper)
+BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
 {
 	m_data = new BulletMJCFImporterInternalData();
 	m_data->m_guiHelper = helper;
+	m_data->m_customVisualShapesConverter = customConverter;
 }
 
 BulletMJCFImporter::~BulletMJCFImporter()
@@ -1135,7 +1189,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
 	b3FileUtils fu;
 	
 	//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
-  	bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
+	bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
+	m_data->m_sourceFileName = relativeFileName;
 	
 	std::string xml_string;
 	m_data->m_pathPrefix[0] = 0;
@@ -1399,21 +1454,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
 	return false;
 }
 
-
-void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
+void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
 {
+	if (m_data->m_customVisualShapesConverter)
+	{
+		const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
+		m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex);
+	}
 }
+
 void BulletMJCFImporter::setBodyUniqueId(int bodyId)
 {
-
+	m_data->m_activeBodyUniqueId = bodyId;
 }
 
 int BulletMJCFImporter::getBodyUniqueId() const 
 { 
-	return 0;
+	b3Assert(m_data->m_activeBodyUniqueId != -1);
+	return m_data->m_activeBodyUniqueId;
 }
 
-
 static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
 {
 	btCompoundShape* compound = new btCompoundShape();
@@ -1496,132 +1556,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 			}
 			case URDF_GEOM_MESH:
 			{
-				//////////////////////
-				if (1)
-			{
-				if (col->m_geometry.m_meshFileName.length())
+				GLInstanceGraphicsShape* glmesh = 0;
+				switch (col->m_geometry.m_meshFileType)
 				{
-					const char* filename = col->m_geometry.m_meshFileName.c_str();
-					//b3Printf("mesh->filename=%s\n",filename);
-					char fullPath[1024];
-					int fileType = 0;
-					sprintf(fullPath,"%s%s",pathPrefix,filename);
-					b3FileUtils::toLower(fullPath);
-                    char tmpPathPrefix[1024];
-                    int maxPathLen = 1024;
-                    b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
-                    
-                    char collisionPathPrefix[1024];
-                    sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
-                    
-					if (strstr(fullPath,".stl"))
+				case UrdfGeometry::FILE_OBJ:
 					{
-						fileType = MJCF_FILE_STL;
+						if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+						{
+							glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
+						}
+						else
+						{
+							std::vector<tinyobj::shape_t> shapes;
+							std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
+							//create a convex hull for each shape, and store it in a btCompoundShape
+
+							childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
+
+						}
+						break;
 					}
-                    if (strstr(fullPath,".obj"))
-                   {
-                       fileType = MJCF_FILE_OBJ;
-                   }
-
-					sprintf(fullPath,"%s%s",pathPrefix,filename);
-					FILE* f = fopen(fullPath,"rb");
-					if (f)
+				case UrdfGeometry::FILE_STL:
 					{
-						fclose(f);
-						GLInstanceGraphicsShape* glmesh = 0;
+						glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
+						break;
+					}
+				default:
+					b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType);
+				}
 
+				if (childShape)
+				{
+					// okay!
+				}
+				else if (!glmesh || glmesh->m_numvertices<=0)
+				{
+					b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str());
+				}
+				else
+				{
+					//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
+					//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
+					//convex->setUserIndex(shapeId);
+					btAlignedObjectArray<btVector3> convertedVerts;
+					convertedVerts.reserve(glmesh->m_numvertices);
+					for (int i=0;i<glmesh->m_numvertices;i++)
+					{
+						convertedVerts.push_back(btVector3(
+							glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
+							glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
+							glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
+					}
 
-						switch (fileType)
+					if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+					{
+
+						btTriangleMesh* meshInterface = new btTriangleMesh();
+						for (int i=0;i<glmesh->m_numIndices/3;i++)
 						{
-                            case MJCF_FILE_OBJ:
-                            {
-								if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
-								{
-									glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
-								}
-								else
-								{
-									std::vector<tinyobj::shape_t> shapes;
-									std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
-									//create a convex hull for each shape, and store it in a btCompoundShape
-
-									childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
-									
-								}
-                                break;
-                            }
-						case MJCF_FILE_STL:
-							{
-								glmesh = LoadMeshFromSTL(fullPath);
-							break;
-							}
-						
-						default:
-							{
-                                b3Warning("Unsupported file type in Collision: %s\n",fullPath);
-                               
-							}
+							float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
+							float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
+							float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
+							meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
+														btVector3(v1[0],v1[1],v1[2]),
+													btVector3(v2[0],v2[1],v2[2]));
 						}
 
-
-						if (!childShape && glmesh && (glmesh->m_numvertices>0))
-						{
-							//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
-							//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
-							//convex->setUserIndex(shapeId);
-							btAlignedObjectArray<btVector3> convertedVerts;
-							convertedVerts.reserve(glmesh->m_numvertices);
-							for (int i=0;i<glmesh->m_numvertices;i++)
-							{
-								convertedVerts.push_back(btVector3(
-                                           glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
-                                           glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
-                                           glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
-							}
-							
-							if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
-							{
-								
-								btTriangleMesh* meshInterface = new btTriangleMesh();
-								for (int i=0;i<glmesh->m_numIndices/3;i++)
-								{
-									float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
-									float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
-									float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
-									meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
-																btVector3(v1[0],v1[1],v1[2]),
-															btVector3(v2[0],v2[1],v2[2]));
-								}
-								
-								btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
-								childShape = trimesh;
-							} else
-							{
-								btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
-								convexHull->optimizeConvexHull();
-								//convexHull->initializePolyhedralFeatures();
-								convexHull->setMargin(m_data->m_defaultCollisionMargin);
-								childShape = convexHull;
-							}
-						} else
-						{
-							b3Warning("issue extracting mesh from STL file %s\n", fullPath);
-						}
-
-                        delete glmesh;
-                       
+						btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+						childShape = trimesh;
 					} else
 					{
-						b3Warning("mesh geometry not found %s\n",fullPath);
+						btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+						convexHull->optimizeConvexHull();
+						//convexHull->initializePolyhedralFeatures();
+						convexHull->setMargin(m_data->m_defaultCollisionMargin);
+						childShape = convexHull;
 					}
-							
 				}
-			}
 
-				//////////////////////
+				delete glmesh;
 				break;
 			}
-			
 			case URDF_GEOM_CAPSULE:
 			{
 				//todo: convert fromto to btCapsuleShape + local btTransform
@@ -1647,11 +1662,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 				}
 				break;
 			}
-			default:
-			{
+			} // switch geom
 
-			}
-			}
 			if (childShape)
 			{
 				m_data->m_allocatedCollisionShapes.push_back(childShape);

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface
 {
 	struct BulletMJCFImporterInternalData* m_data;
 
-
 public:
-	BulletMJCFImporter(struct GUIHelperInterface* helper);
+	BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter);
 	virtual ~BulletMJCFImporter();
 	
 	virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
@@ -66,7 +65,7 @@ public:
     
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
     
-    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
     virtual void setBodyUniqueId(int bodyId);
     virtual int getBodyUniqueId() const;
     

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics()
 	m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
 	
 	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
-    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
-    btIDebugDraw::DBG_DrawConstraints
-    +btIDebugDraw::DBG_DrawContactPoints
-    +btIDebugDraw::DBG_DrawAabb
-        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+		btIDebugDraw::DBG_DrawConstraints
+		+btIDebugDraw::DBG_DrawContactPoints
+		+btIDebugDraw::DBG_DrawAabb
+	);//+btIDebugDraw::DBG_DrawConstraintLimits);
 
 
 	if (m_guiHelper->getParameterInterface())
@@ -204,18 +204,21 @@ void ImportMJCFSetup::initPhysics()
 		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
 
-	BulletMJCFImporter importer(m_guiHelper);
+	BulletMJCFImporter importer(m_guiHelper, 0);
 	MyMJCFLogger logger;
 	bool result = importer.loadMJCF(m_fileName,&logger);
 	if (result)
 	{
-		 btTransform rootTrans;
-        rootTrans.setIdentity();
+		btTransform rootTrans;
+		rootTrans.setIdentity();
 
-        for (int m =0; m<importer.getNumModels();m++)
+		for (int m =0; m<importer.getNumModels();m++)
 		{
+			importer.activateModel(m);
 
-            importer.activateModel(m);
+			// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
+			// emulate this behavior here:
+			importer.setBodyUniqueId(m);
 
 			btMultiBody* mb = 0;
 
@@ -226,18 +229,17 @@ void ImportMJCFSetup::initPhysics()
 			MyMultiBodyCreator creation(m_guiHelper);
 
 			rootTrans.setIdentity();
-           importer.getRootTransformInWorld(rootTrans);
+			importer.getRootTransformInWorld(rootTrans);
 
 			ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
 
 			mb = creation.getBulletMultiBody();
-                        if (mb)
-                        {
-                          printf("first MJCF file converted!\n");
-                          std::string* name =
-                              new std::string(importer.getLinkName(
-                                  importer.getRootLinkIndex()));
-                          m_nameMemory.push_back(name);
+			if (mb)
+			{
+				std::string* name =
+				new std::string(importer.getLinkName(
+					importer.getRootLinkIndex()));
+					m_nameMemory.push_back(name);
 #ifdef TEST_MULTIBODY_SERIALIZATION
 				s->registerNameForPointer(name->c_str(),name->c_str());
 #endif//TEST_MULTIBODY_SERIALIZATION
@@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics()
 							m_data->m_numMotors++;
 						}
 					}
-
 				}
+
 			} else
 			{
+				// not multibody
 				if (1)
 				{
 					//create motors for each generic joint

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -75,16 +75,6 @@ void BulletURDFImporter::printTree()
 //	btAssert(0);
 }
 
-
-enum MyFileType
-{
-	FILE_STL=1,
-	FILE_COLLADA=2,
-    FILE_OBJ=3,
-};
-
-
-    
 BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
 {
 	m_data = new BulletURDFInternalData;
@@ -572,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
         case URDF_GEOM_CYLINDER:
         {
-			btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
-			btScalar cylLength = collision->m_geometry.m_cylinderLength;
+			btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
+			btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
 			
             btAlignedObjectArray<btVector3> vertices;
             //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
@@ -624,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 		GLInstanceGraphicsShape* glmesh = 0;
 		switch (collision->m_geometry.m_meshFileType)
 		{
-		case FILE_OBJ:
+		case UrdfGeometry::FILE_OBJ:
 			if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
 			{
 				glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0);
@@ -640,11 +630,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 			}
 			break;
 
-		case FILE_STL:
+		case UrdfGeometry::FILE_STL:
 			glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
 			break;
 
-		case FILE_COLLADA:
+		case UrdfGeometry::FILE_COLLADA:
 			{
 				btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
 				btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -795,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 cylLength = visual->m_geometry.m_cylinderLength;
+				btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
+				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);
@@ -833,7 +823,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 		{
 			switch (visual->m_geometry.m_meshFileType)
 			{
-			case FILE_OBJ:
+			case UrdfGeometry::FILE_OBJ:
 				{
 					b3ImportMeshData meshData;
 					if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
@@ -852,13 +842,13 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 					break;
 				}
 
-			case FILE_STL:
+			case UrdfGeometry::FILE_STL:
 				{
 					glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
 					break;
 				}
 
-			case FILE_COLLADA:
+			case UrdfGeometry::FILE_COLLADA:
 				{
 					btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
 					btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -1128,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
 	return false;
 }
 
-void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
+void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
 {
-
   	if (m_data->m_customVisualShapesConverter)
 	{
 		const UrdfModel& model = m_data->m_urdfParser.getModel();
-		m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId);
+		UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
+		if (linkPtr)
+		{
+			m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
+		}
 	}
-
 }
 
 int BulletURDFImporter::getNumAllocatedCollisionShapes() const

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -51,7 +51,7 @@ public:
 
     virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
 
-    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
 
     ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
     

examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h

@@ -1,9 +1,14 @@
 #ifndef LINK_VISUAL_SHAPES_CONVERTER_H
 #define LINK_VISUAL_SHAPES_CONVERTER_H
 
+struct UrdfLink;
+struct UrdfModel;
+class btTransform;
+class btCollisionObject;
+
 struct LinkVisualShapesConverter
 {
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
+	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0;
 };
 
 #endif //LINK_VISUAL_SHAPES_CONVERTER_H

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -315,7 +315,7 @@ void ConvertURDF2BulletInternal(
             
 
 
-            //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
+            //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
         } else
         {
             if (cache.m_bulletMultiBody==0)
@@ -474,7 +474,7 @@ void ConvertURDF2BulletInternal(
 				u2b.getLinkColor(urdfLinkIndex,color);
                 creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
 
-                u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
+                u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());
 
 				URDFLinkContactInfo contactInfo;
 				u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
@@ -491,7 +491,7 @@ void ConvertURDF2BulletInternal(
             }
         } else
         {
-            //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
+            //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
         }
     }
 

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -49,7 +49,7 @@ public:
 	///quick hack: need to rethink the API/dependencies of this
     virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
     
-    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const  { }
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const  { }
     virtual void setBodyUniqueId(int bodyId) {}
     virtual int getBodyUniqueId() const { return 0;}
 

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_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
+		geom.m_hasFromTo = false;
+		geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
+		geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
 		
 	}
 	else if (type_name == "capsule")
@@ -575,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
   if (name_char)
 	  visual.m_name = name_char;
 
-	visual.m_hasLocalMaterial = false;
+	visual.m_geometry.m_hasLocalMaterial = false;
 	
   // Material
   TiXmlElement *mat = config->FirstChildElement("material");
@@ -597,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
             matPtr->m_rgbaColor = rgba;
             
             visual.m_materialName = matPtr->m_name;
-            visual.m_hasLocalMaterial = true;
+            visual.m_geometry.m_hasLocalMaterial = true;
         }
     } 
     else
@@ -616,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
           TiXmlElement *c = mat->FirstChildElement("color");
           if (t||c)
           {
-              if (parseMaterial(visual.m_localMaterial, mat,logger))
+              if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
               {
-                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
+                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
                   model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
-                  visual.m_hasLocalMaterial = true;
+                  visual.m_geometry.m_hasLocalMaterial = true;
               }
           }
       }
@@ -1415,12 +1416,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 				for (int i=0;i<link->m_visualArray.size();i++)
 				{
 					UrdfVisual& vis = link->m_visualArray.at(i);
-					if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
+					if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
 					{
 						UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
 						if (mat && *mat)
 						{
-							vis.m_localMaterial = **mat;
+							vis.m_geometry.m_localMaterial = **mat;
 						} else
 						{
 							//logger->reportError("Cannot find material with name:");
@@ -1610,12 +1611,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
                     for (int i=0;i<link->m_visualArray.size();i++)
                     {
                         UrdfVisual& vis = link->m_visualArray.at(i);
-                        if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
+                        if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
                         {
                             UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
                             if (mat && *mat)
                             {
-                                vis.m_localMaterial = **mat;
+                                vis.m_geometry.m_localMaterial = **mat;
                             } else
                             {
                                 //logger->reportError("Cannot find material with name:");

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -20,7 +20,11 @@ struct UrdfMaterial
 {
 	std::string m_name;
 	std::string m_textureFilename;
-	btVector4 m_rgbaColor;
+	btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
+	UrdfMaterial():
+		m_rgbaColor(0.8, 0.8, 0.8, 1)
+	{
+	}
 };
 
 struct UrdfInertia
@@ -66,9 +70,6 @@ struct UrdfGeometry
 	btVector3 m_capsuleFrom;
 	btVector3 m_capsuleTo;
 
-	double m_cylinderRadius;
-	double m_cylinderLength;
-
 	btVector3 m_planeNormal;
     
 	enum {
@@ -79,32 +80,30 @@ struct UrdfGeometry
 	int         m_meshFileType;
 	std::string m_meshFileName;
 	btVector3   m_meshScale;
+
+	UrdfMaterial m_localMaterial;
+	bool m_hasLocalMaterial;
 };
 
 bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
 	const std::string& error_message_prefix,
 	std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
 
-struct UrdfVisual
+struct UrdfShape
 {
 	std::string m_sourceFileLocation;
 	btTransform m_linkLocalFrame;
 	UrdfGeometry m_geometry;
 	std::string m_name;
-	std::string m_materialName;
-	bool m_hasLocalMaterial;
-	UrdfMaterial m_localMaterial;
 };
 
+struct UrdfVisual: UrdfShape
+{
+	std::string m_materialName;
+};
 
-
-
-struct UrdfCollision
+struct UrdfCollision: UrdfShape
 {
-	std::string m_sourceFileLocation;
-	btTransform m_linkLocalFrame;
-	UrdfGeometry m_geometry;
-	std::string m_name;
 	int m_flags;
 	int m_collisionGroup;
 	int m_collisionMask;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1499,7 +1499,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
 
 	m_data->m_sdfRecentLoadedBodies.clear();
 
-    BulletMJCFImporter u2b(m_data->m_guiHelper);	//, &m_data->m_visualConverter
+    BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
 
 	bool useFixedBase = false;
 	MyMJCFLogger2 logger;
@@ -4612,20 +4612,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
 					int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
 
-					m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
+					int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
 						shapeIndex,
 						visualShapeStoragePtr);
-
-
-                    //m_visualConverter
-					serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
-					serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
-					serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
-					serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
-					serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
-                    serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
-                    hasStatus = true;
-                    break;
+					if (success) {
+						serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
+						serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
+						serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
+						serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
+						serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
+						serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
+					}
+					hasStatus = true;
+					break;
                 }
                 case CMD_UPDATE_VISUAL_SHAPE:
                 {

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -170,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
     m_data->m_hasLightSpecularCoeff = true;
 }
 
-void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
+void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
 {
 
 	visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
@@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 	visualShapeOut.m_dimensions[1] = 0;
 	visualShapeOut.m_dimensions[2] = 0;
 	visualShapeOut.m_meshAssetFileName[0] = 0;
+	if (visual->m_geometry.m_hasLocalMaterial) {
+		visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
+		visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
+		visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
+		visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
+	}
 	
 	GLInstanceGraphicsShape* glmesh = 0;
 
@@ -186,25 +192,63 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 	switch (visual->m_geometry.m_type)
 	{
 		case URDF_GEOM_CYLINDER:
+		case URDF_GEOM_CAPSULE:
 		{
+			btVector3 p1 = visual->m_geometry.m_capsuleFrom;
+			btVector3 p2 = visual->m_geometry.m_capsuleTo;
+			btTransform tr;
+			tr.setIdentity();
+			btScalar rad, len;
+			if (visual->m_geometry.m_hasFromTo) {
+				btVector3 v      =  p2 - p1;
+				btVector3 center = (p2 + p1) * 0.5;
+				btVector3 up_vector(0,0,1);
+				btVector3 dir  = v.normalized();
+				btVector3 axis = dir.cross(up_vector);
+				if (axis.fuzzyZero())
+				{
+					axis = btVector3(0,0,1);
+				}
+				else
+				{
+					axis.normalize();
+				}
+				btQuaternion q(axis, -acos(dir.dot(up_vector)));
+				btTransform capsule_orient(q, center);
+				tr = visual->m_linkLocalFrame * capsule_orient;
+				len = v.length();
+				rad = visual->m_geometry.m_capsuleRadius;
+			} else {
+				tr = visual->m_linkLocalFrame;
+				len = visual->m_geometry.m_capsuleHalfHeight;
+				rad = visual->m_geometry.m_capsuleRadius;
+			}
+			visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
+			visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1];
+			visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2];
+			visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0];
+			visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1];
+			visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2];
+			visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3];
+			visualShapeOut.m_dimensions[0] = len;
+			visualShapeOut.m_dimensions[1] = rad;
+
 			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.);
+				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] = -cylLength / 2.;
+				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);
@@ -244,14 +288,6 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 			visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
 			visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
 
-			visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
-			visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
-			visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
-			visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
-			visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
-			visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
-			visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
-
 			switch (visual->m_geometry.m_meshFileType)
 			{
 			case UrdfGeometry::FILE_OBJ:
@@ -467,17 +503,30 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 
 
 
-void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
+void TinyRendererVisualShapeConverter::convertVisualShapes(
+	int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
+	const UrdfLink* linkPtr, const UrdfModel* model,
+	class btCollisionObject* colObj, int bodyUniqueId)
 {
-    
-	
-	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
+	btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
 	if (linkPtr)
 	{
+		const btArray<UrdfVisual>* shapeArray;
+		bool useVisual;
+		int cnt = 0;
+		if (linkPtr->m_visualArray.size() > 0)
+		{
+			useVisual = true;
+			cnt = linkPtr->m_visualArray.size();
+		}
+		else
+		{
+			// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
+			useVisual = false;
+			cnt = linkPtr->m_collisionArray.size();
+		}
 
-		const UrdfLink* link = *linkPtr;
-	
-		for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
+		for (int v1=0; v1<cnt; v1++)
 		{
 			btAlignedObjectArray<MyTexture2> textures;
 			btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -485,20 +534,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 			btTransform startTrans; startTrans.setIdentity();
 			//int graphicsIndex = -1;
 
-			const UrdfVisual& vis = link->m_visualArray[v1];
-			btTransform childTrans = vis.m_linkLocalFrame;
-			btHashString matName(vis.m_materialName.c_str());
-			UrdfMaterial *const * matPtr = model.m_materials[matName];
+			const UrdfShape* vis;
+			if (useVisual) {
+				vis = &linkPtr->m_visualArray[v1];
+			} else {
+				vis = &linkPtr->m_collisionArray[v1];
+			}
+			btTransform childTrans = vis->m_linkLocalFrame;
 
-            float rgbaColor[4] = {1,1,1,1};
-            
-			if (matPtr)
+			float rgbaColor[4] = {1,1,1,1};
+			if (model && useVisual)
 			{
-				UrdfMaterial *const  mat = *matPtr;
-                for (int i=0;i<4;i++)
-                    rgbaColor[i] = mat->m_rgbaColor[i];
-				//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
-				//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+				btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
+				UrdfMaterial*const* matPtr = model->m_materials[matName];
+				if (matPtr)
+				{
+					for (int i=0; i<4; i++)
+					{
+						rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
+					}
+					//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
+					//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+				}
 			}
 			
 			TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
@@ -513,19 +570,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 			b3VisualShapeData visualShape;
 			visualShape.m_objectUniqueId = bodyUniqueId;
 			visualShape.m_linkIndex = linkIndex;
-            visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0];
-            visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1];
-			visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2];
-			visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0];
-			visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1];
-			visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2];
-			visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3];
-            visualShape.m_rgbaColor[0] = rgbaColor[0];
-            visualShape.m_rgbaColor[1] = rgbaColor[1];
-            visualShape.m_rgbaColor[2] = rgbaColor[2];
-            visualShape.m_rgbaColor[3] = rgbaColor[3];
+			visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0];
+			visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1];
+			visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2];
+			visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0];
+			visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1];
+			visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2];
+			visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3];
+			visualShape.m_rgbaColor[0] = rgbaColor[0];
+			visualShape.m_rgbaColor[1] = rgbaColor[1];
+			visualShape.m_rgbaColor[2] = rgbaColor[2];
+			visualShape.m_rgbaColor[3] = rgbaColor[3];
             
-			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
+			convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
 			m_data->m_visualShapes.push_back(visualShape);
 
             if (vertices.size() && indices.size())

examples/SharedMemory/TinyRendererVisualShapeConverter.h

@@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
 	
 	virtual ~TinyRendererVisualShapeConverter();
 	
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
+	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex);
 	
 	virtual int getNumVisualShapes(int bodyUniqueId);