make Bullet URDF parser more similar to the ROS URDF parser. There is still a difference in ordering of links,

due to the use of different hash-map implementations, with a difference in iterator order
(btHashMap versus std::hashmap)

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -607,6 +607,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 	///most OpenCL drivers and OpenCL compilers have issues with our kernels.
 	///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers
 	///you could give it a try
+	///Note that several old OpenCL physics examples still have to be ported over to this new Example Browser
 	if (args.CheckCmdLineFlag("enable_experimental_opencl"))
 	{
 		gAllExamples->initOpenCLExampleEntries();

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -177,7 +177,7 @@ std::string BulletURDFImporter::getLinkName(int linkIndex) const
 	if (linkPtr)
 	{
 		UrdfLink* link = *linkPtr;
-		std::string n = link->m_name;
+		return link->m_name;
 	}
 	return "";
 }

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -10,7 +10,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
 #include "ROSURDFImporter.h"
-//#include "BulletURDFImporter.h"
+#include "BulletURDFImporter.h"
 
 
 #include "URDF2Bullet.h"
@@ -195,15 +195,28 @@ void ImportUrdfSetup::initPhysics()
 
 
     //now print the tree using the new interface
+    URDFImporterInterface* bla=0;
 	
-	//BulletURDFImporter u2b(m_guiHelper);
+    static bool newURDF = false;
-	ROSURDFImporter u2b(m_guiHelper);
+	newURDF = !newURDF;
+	if (newURDF)
+	{
+		b3Printf("using new URDF\n");
+		bla = new  BulletURDFImporter(m_guiHelper);
+	} else
+	{
+		b3Printf("using ROS URDF\n");
+		bla = new ROSURDFImporter(m_guiHelper);
+	}
 
+	URDFImporterInterface& u2b = *bla;
 	bool loadOk =  u2b.loadURDF(m_fileName);
 
 	if (loadOk)
 	{
-		u2b.printTree();
+		printTree(u2b,u2b.getRootLinkIndex());
+
+		//u2b.printTree();
 
 		btTransform identityTrans;
 		identityTrans.setIdentity();
@@ -227,8 +240,8 @@ void ImportUrdfSetup::initPhysics()
 			{
 
 				//create motors for each btMultiBody joint
-
+				int numLinks = mb->getNumLinks();
-				for (int i=0;i<mb->getNumLinks();i++)
+				for (int i=0;i<numLinks;i++)
 				{
 					int mbLinkIndex = i;
 					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
@@ -263,7 +276,8 @@ void ImportUrdfSetup::initPhysics()
 				if (1)
 				{
 					//create motors for each generic joint
-					for (int i=0;i<creation.getNum6DofConstraints();i++)
+					int num6Dof = creation.getNum6DofConstraints();
+					for (int i=0;i<num6Dof;i++)
 					{
 						btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
 						if (c->getUserConstraintPtr())

examples/Importers/ImportURDFDemo/ROSURDFImporter.h

@@ -18,7 +18,7 @@ public:
 
 	virtual bool loadURDF(const char* fileName);
 
-	const char* getPathPrefix();
+	virtual const char* getPathPrefix();
 
 	void printTree(); //for debugging
 	

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -35,6 +35,20 @@ static btVector3 selectColor2()
 
 void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
 {
+	  btScalar mass;
+        btVector3 localInertia;
+        btTransform inertialFrame;
+        u2b.getMassAndInertia(linkIndex,mass,localInertia,inertialFrame);
+		 std::string name = u2b.getLinkName(linkIndex);
+		 for(int j=0;j<indentationLevel;j++) 
+			 printf("  "); //indent
+		 printf("link %s mass=%f\n",name.c_str(),mass);
+		 for(int j=0;j<indentationLevel;j++) 
+			 printf("  "); //indent
+		 printf("local inertia:%f,%f,%f\n",localInertia[0],
+               localInertia[1],
+               localInertia[2]);
+
     btAlignedObjectArray<int> childIndices;
     u2b.getLinkChildIndices(linkIndex,childIndices);
 
@@ -46,6 +60,8 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL
     {
         int childLinkIndex = childIndices[i];
         std::string name = u2b.getLinkName(childLinkIndex);
+      
+        
         for(int j=0;j<indentationLevel;j++) printf("  "); //indent
         printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
         // first grandchild

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -15,6 +15,10 @@ public:
 	virtual ~URDFImporterInterface() {}
 	
  
+    virtual bool loadURDF(const char* fileName)=0;
+
+    virtual const char* getPathPrefix()=0;
+    
     ///return >=0 for the root link index, -1 if there is no root link
     virtual int getRootLinkIndex() const = 0;
     

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -377,7 +377,13 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
 	  }
   } else
   {
-	  logger->reportWarning("No inertial data for link");
+	  logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
+	  link.m_inertia.m_mass = 1.f;
+	  link.m_inertia.m_linkLocalFrame.setIdentity();
+	  link.m_inertia.m_ixx = 1.f;
+	  link.m_inertia.m_iyy = 1.f;
+	  link.m_inertia.m_izz= 1.f;
+
 	  logger->reportWarning(link.m_name.c_str());
   }