refactor of URDF importer (work-in-progress)

examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h

@@ -0,0 +1,29 @@
+#ifndef MULTIBODY_CREATION_INTERFACE_H
+#define MULTIBODY_CREATION_INTERFACE_H
+
+#include "LinearMath/btTransform.h"
+
+class MultiBodyCreationInterface
+{
+public:
+
+	virtual ~MultiBodyCreationInterface() {}
+
+    
+    virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
+  
+    ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
+    virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
+    
+    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)  =0;
+    
+    virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)  = 0;
+    
+    virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0;
+    
+    virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
+    
+    virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
+};
+
+#endif //MULTIBODY_CREATION_INTERFACE_H

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -0,0 +1,71 @@
+#include "MyMultiBodyCreator.h"
+
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+#include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+
+MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
+	:m_guiHelper(guiHelper),
+	m_bulletMultiBody(0)
+{
+}
+
+    
+ class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)
+{
+	m_urdf2mbLink.resize(totalNumJoints+1,-2);
+    m_mb2urdfLink.resize(totalNumJoints+1,-2);
+
+    m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
+    return m_bulletMultiBody;
+}
+
+class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) 
+{
+    btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
+    rbci.m_startWorldTransform = initialWorldTrans;
+    btRigidBody* body = new btRigidBody(rbci);
+    return body;
+}
+    
+class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) 
+{
+    btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
+    return mbCol;
+}
+    
+    
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) 
+{
+    btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
+    return c;
+}
+
+void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex) 
+{
+    m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
+    m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
+}
+
+void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) 
+{
+        
+    m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
+}
+    
+void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) 
+{
+    m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
+}
+
+btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
+{
+    return m_bulletMultiBody;
+}

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -0,0 +1,48 @@
+
+#ifndef MY_MULTIBODY_CREATOR
+#define MY_MULTIBODY_CREATOR
+
+#include "MultiBodyCreationInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class GUIHelperInterface;
+class btMultiBody;
+
+class MyMultiBodyCreator : public MultiBodyCreationInterface
+{
+
+	btMultiBody* m_bulletMultiBody;
+    
+	struct GUIHelperInterface* m_guiHelper;
+
+	
+
+public:
+
+	btAlignedObjectArray<int> m_urdf2mbLink;
+    btAlignedObjectArray<int> m_mb2urdfLink;
+  
+
+	MyMultiBodyCreator(GUIHelperInterface* guiHelper);
+
+	virtual ~MyMultiBodyCreator() {}
+
+    virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ;
+  
+    ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
+    virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
+    
+    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof);
+    
+    virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
+    
+    virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0);
+    
+    virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
+    
+    virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
+
+	btMultiBody* getBulletMultiBody();
+};
+
+#endif //MY_MULTIBODY_CREATOR

examples/Importers/ImportURDFDemo/MyURDFImporter.cpp

@@ -0,0 +1,762 @@
+#include "MyURDFImporter.h"
+
+
+#include "URDFImporterInterface.h"
+#include "btBulletCollisionCommon.h"
+#include "../ImportObjDemo/LoadMeshFromObj.h"
+#include "../ImportSTLDemo/LoadMeshFromSTL.h"
+#include "../ImportColladaDemo/LoadMeshFromCollada.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+using namespace urdf;
+
+void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
+btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
+
+
+struct MyURDFInternalData
+{
+	my_shared_ptr<ModelInterface> m_robot;
+    std::vector<my_shared_ptr<Link> > m_links;
+	struct GUIHelperInterface* m_guiHelper;
+	
+};
+    
+enum MyFileType
+{
+	FILE_STL=1,
+	FILE_COLLADA=2,
+    FILE_OBJ=3,
+};
+
+
+    
+MyURDFImporter::MyURDFImporter(my_shared_ptr<ModelInterface> robot,struct GUIHelperInterface* helper)
+{
+	m_data = new MyURDFInternalData;
+	m_data->m_robot = robot;
+	m_data->m_guiHelper = helper;
+    m_data->m_robot->getLinks(m_data->m_links);
+        
+    //initialize the 'index' of each link
+    for (int i=0;i<m_data->m_links.size();i++)
+    {
+        m_data->m_links[i]->m_link_index = i;
+    }
+  
+}
+
+MyURDFImporter::~MyURDFImporter()
+{
+	delete m_data;
+}
+
+    
+int MyURDFImporter::getRootLinkIndex() const
+{
+    if (m_data->m_links.size())
+    {
+        int rootLinkIndex = m_data->m_robot->getRoot()->m_link_index;
+        // btAssert(m_links[0]->m_link_index == rootLinkIndex);
+        return rootLinkIndex;
+    }
+    return -1;
+};
+    
+void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
+{
+    childLinkIndices.resize(0);
+    int numChildren = m_data->m_links[linkIndex]->child_links.size();
+        
+    for (int i=0;i<numChildren;i++)
+    {
+        int childIndex =m_data->m_links[linkIndex]->child_links[i]->m_link_index;
+        childLinkIndices.push_back(childIndex);
+    }
+}
+
+std::string MyURDFImporter::getLinkName(int linkIndex) const
+{
+    std::string n = m_data->m_links[linkIndex]->name;
+    return n;
+}
+    
+std::string MyURDFImporter::getJointName(int linkIndex) const
+{
+    return m_data->m_links[linkIndex]->parent_joint->name;
+}
+    
+
+void  MyURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
+{
+    if ((*m_data->m_links[linkIndex]).inertial)
+    {
+        mass = (*m_data->m_links[linkIndex]).inertial->mass;
+        localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
+        inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
+        inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
+    } else
+    {
+        mass = 1.f;
+        localInertiaDiagonal.setValue(1,1,1);
+        inertialFrame.setIdentity();
+    }
+}
+    
+bool MyURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
+{
+    jointLowerLimit = 0.f;
+    jointUpperLimit = 0.f;
+        
+    if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
+    {
+        my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
+            
+        const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
+        const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
+            
+        jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
+        parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
+        parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
+
+        switch (pj->type)
+        {
+            case Joint::REVOLUTE:
+                jointType = URDFRevoluteJoint;
+                break;
+            case Joint::FIXED:
+                jointType = URDFFixedJoint;
+                break;
+            case Joint::PRISMATIC:
+                jointType = URDFPrismaticJoint;
+                break;
+            case Joint::PLANAR:
+                jointType = URDFPlanarJoint;
+                break;
+            case Joint::CONTINUOUS:
+				jointType = URDFContinuousJoint;
+                break;
+            default:
+            {
+                printf("Error: unknown joint type %d\n", pj->type);
+                btAssert(0);
+            }
+                    
+        };
+            
+        if (pj->limits)
+        {
+            jointLowerLimit = pj->limits.get()->lower;
+            jointUpperLimit = pj->limits.get()->upper;
+        }
+        return true;
+    } else
+    {
+        parent2joint.setIdentity();
+        return false;
+    }
+}
+
+
+
+void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+{
+
+	
+	GLInstanceGraphicsShape* glmesh = 0;
+
+	btConvexShape* convexColShape = 0;
+
+	switch (visual->geometry->type)
+	{
+		case Geometry::CYLINDER:
+		{
+			printf("processing a cylinder\n");
+			urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
+			btAlignedObjectArray<btVector3> vertices;
+		
+			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+			int numSteps = 32;
+			for (int i = 0; i<numSteps; i++)
+			{
+
+				btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.);
+				vertices.push_back(vert);
+				vert[2] = -cyl->length / 2.;
+				vertices.push_back(vert);
+			}
+
+			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+			cylZShape->setMargin(0.001);
+			convexColShape = cylZShape;
+			break;
+		}
+		case Geometry::BOX:
+		{
+			printf("processing a box\n");
+			urdf::Box* box = (urdf::Box*)visual->geometry.get();
+			btVector3 extents(box->dim.x, box->dim.y, box->dim.z);
+			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
+			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
+			convexColShape = boxShape;
+			convexColShape->setMargin(0.001);
+			break;
+		}
+		case Geometry::SPHERE:
+		{
+			printf("processing a sphere\n");
+			urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
+			btScalar radius = sphere->radius;
+			btSphereShape* sphereShape = new btSphereShape(radius);
+			convexColShape = sphereShape;
+			convexColShape->setMargin(0.001);
+			break;
+
+			break;
+		}
+		case Geometry::MESH:
+		{
+			if (visual->name.length())
+			{
+				printf("visual->name=%s\n", visual->name.c_str());
+			}
+			if (visual->geometry)
+			{
+				const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
+				if (mesh->filename.length())
+				{
+					const char* filename = mesh->filename.c_str();
+					printf("mesh->filename=%s\n", filename);
+					char fullPath[1024];
+					int fileType = 0;
+					sprintf(fullPath, "%s%s", pathPrefix, filename);
+					b3FileUtils::toLower(fullPath);
+					if (strstr(fullPath, ".dae"))
+					{
+						fileType = FILE_COLLADA;
+					}
+					if (strstr(fullPath, ".stl"))
+					{
+						fileType = FILE_STL;
+					}
+                    if (strstr(fullPath,".obj"))
+                    {
+                        fileType = FILE_OBJ;
+                    }
+
+
+					sprintf(fullPath, "%s%s", pathPrefix, filename);
+					FILE* f = fopen(fullPath, "rb");
+					if (f)
+					{
+						fclose(f);
+						
+
+
+						switch (fileType)
+						{
+                            case FILE_OBJ:
+                            {
+                                glmesh = LoadMeshFromObj(fullPath,pathPrefix);
+                                break;
+                            }
+                           
+						case FILE_STL:
+						{
+							glmesh = LoadMeshFromSTL(fullPath);
+							break;
+						}
+						case FILE_COLLADA:
+						{
+
+							btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+							btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+							btTransform upAxisTrans; upAxisTrans.setIdentity();
+							float unitMeterScaling = 1;
+							int upAxis = 2;
+
+							LoadMeshFromCollada(fullPath,
+								visualShapes,
+								visualShapeInstances,
+								upAxisTrans,
+								unitMeterScaling,
+												upAxis);
+
+							glmesh = new GLInstanceGraphicsShape;
+							int index = 0;
+							glmesh->m_indices = new b3AlignedObjectArray<int>();
+							glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+							for (int i = 0; i<visualShapeInstances.size(); i++)
+							{
+								ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+								GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+
+								b3AlignedObjectArray<GLInstanceVertex> verts;
+								verts.resize(gfxShape->m_vertices->size());
+
+								int baseIndex = glmesh->m_vertices->size();
+
+								for (int i = 0; i<gfxShape->m_vertices->size(); i++)
+								{
+									verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+									verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+									verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+									verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+									verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+									verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+									verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+									verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+									verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+
+								}
+
+								int curNumIndices = glmesh->m_indices->size();
+								int additionalIndices = gfxShape->m_indices->size();
+								glmesh->m_indices->resize(curNumIndices + additionalIndices);
+								for (int k = 0; k<additionalIndices; k++)
+								{
+									glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
+								}
+
+								//compensate upAxisTrans and unitMeterScaling here
+								btMatrix4x4 upAxisMat;
+								upAxisMat.setIdentity();
+//								upAxisMat.setPureRotation(upAxisTrans.getRotation());
+								btMatrix4x4 unitMeterScalingMat;
+								unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
+								btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
+								//btMatrix4x4 worldMat = instance->m_worldTransform;
+								int curNumVertices = glmesh->m_vertices->size();
+								int additionalVertices = verts.size();
+								glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
+
+								for (int v = 0; v<verts.size(); v++)
+								{
+									btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
+									pos = worldMat*pos;
+									verts[v].xyzw[0] = float(pos[0]);
+									verts[v].xyzw[1] = float(pos[1]);
+									verts[v].xyzw[2] = float(pos[2]);
+									glmesh->m_vertices->push_back(verts[v]);
+								}
+							}
+							glmesh->m_numIndices = glmesh->m_indices->size();
+							glmesh->m_numvertices = glmesh->m_vertices->size();
+							//glmesh = LoadMeshFromCollada(fullPath);
+
+							break;
+						}
+						default:
+						{
+                            printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
+                            btAssert(0);
+						}
+						}
+
+
+						if (glmesh && (glmesh->m_numvertices>0))
+						{
+						}
+						else
+						{
+							printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
+						}
+
+					}
+					else
+					{
+						printf("mesh geometry not found %s\n", fullPath);
+					}
+
+
+				}
+			}
+
+
+			break;
+		}
+		default:
+		{
+			printf("Error: unknown visual geometry type\n");
+		}
+	}
+
+	//if we have a convex, tesselate into localVertices/localIndices
+	if (convexColShape)
+	{
+		btShapeHull* hull = new btShapeHull(convexColShape);
+		hull->buildHull(0.0);
+		{
+			//	int strideInBytes = 9*sizeof(float);
+			int numVertices = hull->numVertices();
+			int numIndices = hull->numIndices();
+
+			
+			glmesh = new GLInstanceGraphicsShape;
+			int index = 0;
+			glmesh->m_indices = new b3AlignedObjectArray<int>();
+			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+
+			for (int i = 0; i < numVertices; i++)
+			{
+				GLInstanceVertex vtx;
+				btVector3 pos = hull->getVertexPointer()[i];
+				vtx.xyzw[0] = pos.x();
+				vtx.xyzw[1] = pos.y();
+				vtx.xyzw[2] = pos.z();
+				vtx.xyzw[3] = 1.f;
+				pos.normalize();
+				vtx.normal[0] = pos.x();
+				vtx.normal[1] = pos.y();
+				vtx.normal[2] = pos.z();
+				vtx.uv[0] = 0.5f;
+				vtx.uv[1] = 0.5f;
+				glmesh->m_vertices->push_back(vtx);
+			}
+
+			btAlignedObjectArray<int> indices;
+			for (int i = 0; i < numIndices; i++)
+			{
+				glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
+			}
+			
+			glmesh->m_numvertices = glmesh->m_vertices->size();
+			glmesh->m_numIndices = glmesh->m_indices->size();
+		}
+		delete convexColShape;
+		convexColShape = 0;
+
+	}
+	
+	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
+	{
+
+		int baseIndex = verticesOut.size();
+
+
+
+		for (int i = 0; i < glmesh->m_indices->size(); i++)
+		{
+			indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
+		}
+
+		for (int i = 0; i < glmesh->m_vertices->size(); i++)
+		{
+			GLInstanceVertex& v = glmesh->m_vertices->at(i);
+			btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
+			btVector3 vt = visualTransform*vert;
+			v.xyzw[0] = vt[0];
+			v.xyzw[1] = vt[1];
+			v.xyzw[2] = vt[2];
+			btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
+			triNormal = visualTransform.getBasis()*triNormal;
+			v.normal[0] = triNormal[0];
+			v.normal[1] = triNormal[1];
+			v.normal[2] = triNormal[2];
+			verticesOut.push_back(v);
+		}
+	}
+}
+
+
+
+
+btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix)
+{
+	btCollisionShape* shape = 0;
+
+    switch (visual->geometry->type)
+    {
+        case Geometry::CYLINDER:
+        {
+            printf("processing a cylinder\n");
+            urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
+
+            btAlignedObjectArray<btVector3> vertices;
+            //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+            int numSteps = 32;
+            for (int i=0;i<numSteps;i++)
+            {
+
+                btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
+                vertices.push_back(vert);
+                vert[2] = -cyl->length/2.;
+                vertices.push_back(vert);
+
+            }
+            btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+            cylZShape->setMargin(0.001);
+			cylZShape->initializePolyhedralFeatures();
+			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+            
+            //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+            //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+            
+
+            shape = cylZShape;
+            break;
+        }
+        case Geometry::BOX:
+        {
+            printf("processing a box\n");
+            urdf::Box* box = (urdf::Box*)visual->geometry.get();
+            btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
+            btBoxShape* boxShape = new btBoxShape(extents*0.5f);
+			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
+            shape = boxShape;
+			shape ->setMargin(0.001);
+            break;
+        }
+        case Geometry::SPHERE:
+        {
+			printf("processing a sphere\n");
+            urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
+            btScalar radius = sphere->radius;
+			btSphereShape* sphereShape = new btSphereShape(radius);
+            shape = sphereShape;
+			shape ->setMargin(0.001);
+            break;
+
+            break;
+        }
+        case Geometry::MESH:
+        {
+			if (visual->name.length())
+			{
+				printf("visual->name=%s\n",visual->name.c_str());
+			}
+			if (visual->geometry)
+			{
+				const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
+				if (mesh->filename.length())
+				{
+					const char* filename = mesh->filename.c_str();
+					printf("mesh->filename=%s\n",filename);
+					char fullPath[1024];
+					int fileType = 0;
+					sprintf(fullPath,"%s%s",pathPrefix,filename);
+					b3FileUtils::toLower(fullPath);
+					if (strstr(fullPath,".dae"))
+					{
+						fileType = FILE_COLLADA;
+					}
+					if (strstr(fullPath,".stl"))
+					{
+						fileType = FILE_STL;
+					}
+                    if (strstr(fullPath,".obj"))
+                   {
+                       fileType = FILE_OBJ;
+                   }
+
+					sprintf(fullPath,"%s%s",pathPrefix,filename);
+					FILE* f = fopen(fullPath,"rb");
+					if (f)
+					{
+						fclose(f);
+						GLInstanceGraphicsShape* glmesh = 0;
+						
+						
+						switch (fileType)
+						{
+                            case FILE_OBJ:
+                            {
+                                glmesh = LoadMeshFromObj(fullPath,pathPrefix);
+                                break;
+                            }
+						case FILE_STL:
+							{
+								glmesh = LoadMeshFromSTL(fullPath);
+							break;
+							}
+						case FILE_COLLADA:
+							{
+								
+								btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+								btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+								btTransform upAxisTrans;upAxisTrans.setIdentity();
+								float unitMeterScaling=1;
+								int upAxis = 2;
+								LoadMeshFromCollada(fullPath,
+													visualShapes, 
+													visualShapeInstances,
+													upAxisTrans,
+													unitMeterScaling,
+													upAxis );
+								
+								glmesh = new GLInstanceGraphicsShape;
+								int index = 0;
+								glmesh->m_indices = new b3AlignedObjectArray<int>();
+								glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+								for (int i=0;i<visualShapeInstances.size();i++)
+								{
+									ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+									GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+		
+									b3AlignedObjectArray<GLInstanceVertex> verts;
+									verts.resize(gfxShape->m_vertices->size());
+
+									int baseIndex = glmesh->m_vertices->size();
+
+									for (int i=0;i<gfxShape->m_vertices->size();i++)
+									{
+										verts[i].normal[0] = 	gfxShape->m_vertices->at(i).normal[0];
+										verts[i].normal[1] = 	gfxShape->m_vertices->at(i).normal[1];
+										verts[i].normal[2] = 	gfxShape->m_vertices->at(i).normal[2];
+										verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+										verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+										verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+										verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+										verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+										verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+
+									}
+
+									int curNumIndices = glmesh->m_indices->size();
+									int additionalIndices = gfxShape->m_indices->size();
+									glmesh->m_indices->resize(curNumIndices+additionalIndices);
+									for (int k=0;k<additionalIndices;k++)
+									{
+										glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
+									}
+			
+									//compensate upAxisTrans and unitMeterScaling here
+									btMatrix4x4 upAxisMat;
+									upAxisMat.setPureRotation(upAxisTrans.getRotation());
+									btMatrix4x4 unitMeterScalingMat;
+									unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
+									btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
+									//btMatrix4x4 worldMat = instance->m_worldTransform;
+									int curNumVertices = glmesh->m_vertices->size();
+									int additionalVertices = verts.size();
+									glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
+									
+									for(int v=0;v<verts.size();v++)
+									{
+										btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
+										pos = worldMat*pos;
+										verts[v].xyzw[0] = float(pos[0]);
+										verts[v].xyzw[1] = float(pos[1]);
+										verts[v].xyzw[2] = float(pos[2]);
+										glmesh->m_vertices->push_back(verts[v]);
+									}
+								}
+								glmesh->m_numIndices = glmesh->m_indices->size();
+								glmesh->m_numvertices = glmesh->m_vertices->size();
+								//glmesh = LoadMeshFromCollada(fullPath);
+
+								break;
+							}
+						default:
+							{
+                                printf("Unsupported file type in Collision: %s\n",fullPath);
+                                btAssert(0);
+							}
+						}
+					
+
+						if (glmesh && (glmesh->m_numvertices>0))
+						{
+							printf("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],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
+							}
+							//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
+							btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+							//cylZShape->initializePolyhedralFeatures();
+							//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+							//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+							cylZShape->setMargin(0.001);
+							shape = cylZShape;
+						} else
+						{
+							printf("issue extracting mesh from STL file %s\n", fullPath);
+						}
+
+					} else
+					{
+						printf("mesh geometry not found %s\n",fullPath);
+					}
+							
+							
+				}
+			}
+
+					
+            break;
+        }
+        default:
+        {
+            printf("Error: unknown visual geometry type\n");
+        }
+    }
+	return shape;
+}
+
+
+
+
+int MyURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
+{
+    btAlignedObjectArray<GLInstanceVertex> vertices;
+    btAlignedObjectArray<int> indices;
+    btTransform startTrans; startTrans.setIdentity();
+    int graphicsIndex = -1;
+        
+    for (int v = 0; v < (int)m_data->m_links[linkIndex]->visual_array.size(); v++)
+    {
+        const Visual* vis = m_data->m_links[linkIndex]->visual_array[v].get();
+        btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
+        btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
+        btTransform childTrans;
+        childTrans.setOrigin(childPos);
+        childTrans.setRotation(childOrn);
+            
+        convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
+            
+    }
+        
+    if (vertices.size() && indices.size())
+    {
+        graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+    }
+        
+    return graphicsIndex;
+        
+}
+
+ class btCompoundShape* MyURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
+{
+        
+    btCompoundShape* compoundShape = new btCompoundShape();
+    compoundShape->setMargin(0.001);
+        
+    for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)
+    {
+        const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get();
+        btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
+            
+        if (childShape)
+        {
+            btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
+            btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
+            btTransform childTrans;
+            childTrans.setOrigin(childPos);
+            childTrans.setRotation(childOrn);
+            compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
+                
+        }
+    }
+        
+    return compoundShape;
+}

examples/Importers/ImportURDFDemo/MyURDFImporter.h

@@ -0,0 +1,41 @@
+#ifndef MY_URDF_IMPORTER_H
+#define MY_URDF_IMPORTER_H 
+
+#include "URDFImporterInterface.h"
+#include <vector> //temp, replace by btAlignedObjectArray
+
+#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
+
+class MyURDFImporter : public URDFImporterInterface
+{
+    
+	struct MyURDFInternalData* m_data;
+    
+
+public:
+    
+
+	MyURDFImporter(my_shared_ptr<urdf::ModelInterface> robot,struct GUIHelperInterface* helper);
+    
+	virtual ~MyURDFImporter();
+	
+    virtual int getRootLinkIndex() const;
+    
+    virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
+
+    virtual std::string getLinkName(int linkIndex) const;
+    
+    virtual std::string getJointName(int linkIndex) const;
+    
+    virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
+
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
+
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
+
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
+
+};
+
+
+#endif //MY_URDF_IMPORTER_H

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -1,4 +1,4 @@
-#include "URDF2Bullet.h"
+#include "URDFImporterInterface.h"
 #include <stdio.h>
 #include "LinearMath/btTransform.h"
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@@ -6,7 +6,9 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
-
+#include "URDFImporterInterface.h"
+#include "MultiBodyCreationInterface.h"
+#include <string>
 static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
 static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
 static bool enableConstraints = true;
@@ -30,7 +32,7 @@ static btVector3 selectColor2()
     return color;
 }
 
-void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
+void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
 {
     btAlignedObjectArray<int> childIndices;
     u2b.getLinkChildIndices(linkIndex,childIndices);
@@ -50,6 +52,7 @@ void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
     }
 }
 
+
 struct URDF2BulletCachedData
 {
     URDF2BulletCachedData()
@@ -104,7 +107,7 @@ struct URDF2BulletCachedData
     
 };
 
-void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex)
+void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
 {
     btAlignedObjectArray<int> childIndices;
     u2b.getLinkChildIndices(linkIndex,childIndices);
@@ -121,7 +124,7 @@ void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& c
     }
 }
 
-void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
+void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
 {
     cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
     cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
@@ -134,7 +137,7 @@ void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache,
     }
 }
 
-void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
+void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
 {
     //compute the number of links, and compute parent indices array (and possibly other cached data?)
     cache.m_totalNumJoints1 = 0;
@@ -156,7 +159,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
     
 }
 
-void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
+void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
 {
     printf("start converting/extracting data from URDF interface\n");
     
@@ -241,14 +244,14 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
         
         if (!createMultiBody)
         {
-            btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
+            btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
             linkRigidBody = body;
             
             world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
             
             compoundShape->setUserIndex(graphicsIndex);
             
-            u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
+            creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
             cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
         } else
         {
@@ -258,7 +261,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                 bool canSleep = false;
                 bool isFixedBase = (mass==0);//todo: figure out when base is fixed
                 int totalNumJoints = cache.m_totalNumJoints1;
-                cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
+                cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
                 
                 cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
             }
@@ -275,7 +278,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
             bool disableParentCollision = true;
             switch (jointType)
             {
-                case URDF2Bullet::FixedJoint:
+                case URDFFixedJoint:
                 {
                     if (createMultiBody)
                     {
@@ -285,7 +288,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                         btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
                         cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
                                                                rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
-                        u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+                        creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
                         
                         btMatrix3x3 rm(rot);
                         btScalar y,p,r;
@@ -302,7 +305,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                         printf("y=%f,p=%f,r=%f\n", y,p,r);
                         
                         //we could also use btFixedConstraint but it has some issues
-                        btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+                        btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
 
                         dof6->setLinearLowerLimit(btVector3(0,0,0));
                         dof6->setLinearUpperLimit(btVector3(0,0,0));
@@ -315,8 +318,8 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                     }
                     break;
                 }
-                case URDF2Bullet::ContinuousJoint:
+                case URDFContinuousJoint:
-                case URDF2Bullet::RevoluteJoint:
+                case URDFRevoluteJoint:
                 {
                     if (createMultiBody)
                     {
@@ -326,7 +329,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                                                                   offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
                                                                   -offsetInB.getOrigin(),
                                                                   disableParentCollision);
-                        u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+                        creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
                         
                     } else
                     {
@@ -337,7 +340,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                         {
                             case 0:
                             {
-                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
+                                btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
                                 dof6->setLinearLowerLimit(btVector3(0,0,0));
                                 dof6->setLinearUpperLimit(btVector3(0,0,0));
                                 
@@ -350,7 +353,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                             }
                             case 1:
                             {
-                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
+                                btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
                                 dof6->setLinearLowerLimit(btVector3(0,0,0));
                                 dof6->setLinearUpperLimit(btVector3(0,0,0));
                                 
@@ -364,7 +367,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                             case 2:
                             default:
                             {
-                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
+                                btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
                                 dof6->setLinearLowerLimit(btVector3(0,0,0));
                                 dof6->setLinearUpperLimit(btVector3(0,0,0));
                                 
@@ -379,7 +382,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                     }
                     break;
                 }
-                case URDF2Bullet::PrismaticJoint:
+                case URDFPrismaticJoint:
                 {
                     if (createMultiBody)
                     {
@@ -389,11 +392,11 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                                                                    -offsetInB.getOrigin(),
                                                                    disableParentCollision);
                         
-                        u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+                        creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
                         
                     } else
                     {
-                        btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+                        btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
                         //todo(erwincoumans) for now, we only support principle axis along X, Y or Z
                         int principleAxis = jointAxisInJointSpace.closestAxis();
                         switch (principleAxis)
@@ -440,7 +443,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
         {
             if (compoundShape->getNumChildShapes()>0)
             {
-                btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
+                btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
                                 
                 compoundShape->setUserIndex(graphicsIndex);
                 
@@ -462,7 +465,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
                 
                 btVector3 color = selectColor2();//(0.0,0.0,0.5);
                
-                u2b.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
+                creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
                 
                 btScalar friction = 0.5f;
                 
@@ -489,18 +492,18 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
     {
         int urdfChildLinkIndex = urdfChildIndices[i];
         
-        ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
+        ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
     }
     
 }
 
-void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
+void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
 {
     URDF2BulletCachedData cache;
 
     InitURDF2BulletCache(u2b,cache);
     int urdfLinkIndex = u2b.getRootLinkIndex();
-    ConvertURDF2BulletInternal(u2b, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
+    ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
     
 }
 

examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -8,64 +8,21 @@ class btTransform;
 class btMultiBodyDynamicsWorld;
 class btTransform;
 
-class URDF2Bullet
-{
 
-public:
+class URDFImporterInterface;
-
+class MultiBodyCreationInterface;
-    enum {
-        RevoluteJoint=1,
-        PrismaticJoint,
-        ContinuousJoint,
-        FloatingJoint,
-        PlanarJoint,
-        FixedJoint,
-    };
- 
-    ///return >=0 for the root link index, -1 if there is no root link
-    virtual int getRootLinkIndex() const = 0;
-    
-    ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
-    virtual std::string getLinkName(int linkIndex) const =0;
-    
-    virtual std::string getJointName(int linkIndex) const = 0;
-
-    //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
-    virtual void  getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0;
-    
-    ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
-    virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
-    
-    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
-    
-    virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
-    
-    ///create Bullet collision shapes from URDF 'Collision' objects, specified in inertial frame of the link.
-    virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
-    
-    virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0;
-  
-    ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
-    virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) const = 0;
-    
-    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const =0;
-    
-    virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const = 0;
-    
-    virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) const = 0;
-    
-    virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) const = 0;
-    
-    virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const = 0;
-};
 
 
-void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0);
+void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0);
 
 
 
-void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix);
+void ConvertURDF2Bullet(const URDFImporterInterface& u2b, 
-
+												MultiBodyCreationInterface& creationCallback, 
+												const btTransform& rootTransformInWorldSpace, 
+												btMultiBodyDynamicsWorld* world,
+												bool createMultiBody, 
+												const char* pathPrefix);
 
 #endif //_URDF2BULLET_H
 

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -0,0 +1,41 @@
+#ifndef URDF_IMPORTER_INTERFACE_H
+#define URDF_IMPORTER_INTERFACE_H
+
+#include <string>
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "URDFJointTypes.h"
+
+class URDFImporterInterface
+{
+
+public:
+
+
+	virtual ~URDFImporterInterface() {}
+	
+ 
+    ///return >=0 for the root link index, -1 if there is no root link
+    virtual int getRootLinkIndex() const = 0;
+    
+    ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
+    virtual std::string getLinkName(int linkIndex) const =0;
+    
+    virtual std::string getJointName(int linkIndex) const = 0;
+
+    //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
+    virtual void  getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0;
+    
+    ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
+    virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
+    
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
+    
+	///quick hack: need to rethink the API/dependencies of this
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;
+
+	 virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const  = 0;
+};
+
+#endif //URDF_IMPORTER_INTERFACE_H
+

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -0,0 +1,16 @@
+#ifndef URDF_JOINT_TYPES_H
+#define URDF_JOINT_TYPES_H
+
+
+enum 
+{
+		URDFRevoluteJoint=1,
+		URDFPrismaticJoint,
+		URDFContinuousJoint,
+		URDFFloatingJoint,
+		URDFPlanarJoint,
+		URDFFixedJoint,
+};
+
+
+#endif //URDF_JOINT_TYPES_H