Merge pull request #342 from erwincoumans/master

small URDF2Bullet refactor to make URDF import a little bit more re-useable

Demos3/AllBullet2Demos/CMakeLists.txt

@@ -46,6 +46,7 @@ SET(App_AllBullet2Demos_SRCS
 #	../bullet2/RagdollDemo/RagdollDemo.h
 	../bullet2/LuaDemo/LuaPhysicsSetup.cpp
 	../bullet2/LuaDemo/LuaPhysicsSetup.h
+	../ImportURDFDemo/URDF2Bullet.cpp
 	../ImportURDFDemo/ImportURDFSetup.cpp
 	../ImportURDFDemo/ImportURDFSetup.h
 	../ImportObjDemo/ImportObjSetup.cpp

Demos3/AllBullet2Demos/premake4.lua

@@ -69,6 +69,7 @@
 --		"../bullet2/SoftDemo/SoftDemo.cpp",
 		"../ImportColladaDemo/LoadMeshFromCollada.cpp",
 		"../ImportColladaDemo/ImportColladaSetup.cpp",
+		"../ImportURDFDemo/URDF2Bullet.cpp",	
 		"../ImportURDFDemo/ImportURDFSetup.cpp",
 		"../ImportObjDemo/ImportObjSetup.cpp",
 		"../ImportObjDemo/LoadMeshFromObj.cpp",

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -2,60 +2,296 @@
 #include "ImportURDFSetup.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "Bullet3Common/b3FileUtils.h"
-#include "../ImportSTLDemo/LoadMeshFromSTL.h"
 #include "../ImportObjDemo/LoadMeshFromObj.h"
+#include "../ImportSTLDemo/LoadMeshFromSTL.h"
 #include "../ImportColladaDemo/LoadMeshFromCollada.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
-#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
 #include "Bullet3Common/b3FileUtils.h"
 #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
 
 static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
 static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
 static bool enableConstraints = true;//false;
+#include "URDF2Bullet.h"
 
+#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
 
-const char* fileNames[] = 
+#include "urdf_samples.h"
+
+//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+//#define USE_BARREL_VERTICES
+//#include "OpenGLWindow/ShapeData.h"
+
+#include <iostream>
+#include <fstream>
+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);
+
+class MyURDF2Bullet : public URDF2Bullet
 {
-	"r2d2.urdf",
+    my_shared_ptr<ModelInterface> m_robot;
-	"r2d2.urdf",
+    std::vector<my_shared_ptr<Link> > m_links;
+    GraphicsPhysicsBridge& m_gfxBridge;
+    mutable btMultiBody* m_bulletMultiBody;
     
-};
+public:
+    MyURDF2Bullet(my_shared_ptr<ModelInterface> robot,GraphicsPhysicsBridge& gfxBridge)
+    :m_robot(robot),
+    m_gfxBridge(gfxBridge),
+    m_bulletMultiBody(0)
+    {
+        m_robot->getLinks(m_links);
         
-#define MAX_NUM_MOTORS 1024
+        //initialize the 'index' of each link
+        for (int i=0;i<m_links.size();i++)
+        {
+            m_links[i]->m_link_index = i;
+        }
+    }
     
-struct ImportUrdfInternalData
+    virtual int getRootLinkIndex() const
-{
+    {
-	ImportUrdfInternalData()
+        if (m_links.size())
-		:m_numMotors(0)
+        {
-	{
+            int rootLinkIndex = m_robot->getRoot()->m_link_index;
-	}
+           // btAssert(m_links[0]->m_link_index == rootLinkIndex);
+            return rootLinkIndex;
+        }
+        return -1;
+    };
+    
+    virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
+    {
+        childLinkIndices.resize(0);
+        int numChildren = m_links[linkIndex]->child_links.size();
+        
+        for (int i=0;i<numChildren;i++)
+        {
+            int childIndex =m_links[linkIndex]->child_links[i]->m_link_index;
+            childLinkIndices.push_back(childIndex);
+        }
+    }
+    virtual std::string getLinkName(int linkIndex) const
+    {
+        std::string n = m_links[linkIndex]->name;
+        return n;
+    }
+    
+    virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
+    {
+        if ((*m_links[linkIndex]).inertial)
+        {
+            mass = (*m_links[linkIndex]).inertial->mass;
+            localInertiaDiagonal.setValue((*m_links[linkIndex]).inertial->ixx,(*m_links[linkIndex]).inertial->iyy,(*m_links[linkIndex]).inertial->izz);
+            inertialFrame.setOrigin(btVector3((*m_links[linkIndex]).inertial->origin.position.x,(*m_links[linkIndex]).inertial->origin.position.y,(*m_links[linkIndex]).inertial->origin.position.z));
+            inertialFrame.setRotation(btQuaternion((*m_links[linkIndex]).inertial->origin.rotation.x,(*m_links[linkIndex]).inertial->origin.rotation.y,(*m_links[linkIndex]).inertial->origin.rotation.z,(*m_links[linkIndex]).inertial->origin.rotation.w));
+        } else
+        {
+            mass = 1.f;
+            localInertiaDiagonal.setValue(1,1,1);
+            inertialFrame.setIdentity();
+        }
+    }
+    
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
+    {
+        jointLowerLimit = 0.f;
+        jointUpperLimit = 0.f;
+        
+        if ((*m_links[urdfLinkIndex]).parent_joint)
+        {
+            my_shared_ptr<Joint> pj =(*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 = URDF2Bullet::RevoluteJoint;
+                    break;
+                case Joint::FIXED:
+                    jointType = URDF2Bullet::FixedJoint;
+                    break;
+                case Joint::PRISMATIC:
+                    jointType = URDF2Bullet::PrismaticJoint;
+                    break;
+                case Joint::PLANAR:
+                    jointType = URDF2Bullet::PlanarJoint;
+                    break;
+                case Joint::CONTINUOUS:
+					jointType = URDF2Bullet::ContinuousJoint;
+                    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;
+        }
+    }
+
+    virtual int 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_links[linkIndex]->visual_array.size(); v++)
+        {
+            const Visual* vis = 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_gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+        }
+        
+        return graphicsIndex;
+        
+    }
+    
+    virtual class btCompoundShape* 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_links[linkIndex]->collision_array.size();v++)
+        {
+            const Collision* col = 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;
+    }
+    
+    virtual class btMultiBody* allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const
+    {
+        m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
+        return m_bulletMultiBody;
+    }
+
+    virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const
+    {
+        btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
+        rbci.m_startWorldTransform = initialWorldTrans;
+        btRigidBody* body = new btRigidBody(rbci);
+        return body;
+    }
+    
+    virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) const
+    {
+        btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
+        return mbCol;
+    }
+    
+    
+    virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) const
+    {
+        btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
+        return c;
+    }
+
+
+    virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const
+    {
+        
+        m_gfxBridge.createRigidBodyGraphicsObject(body, colorRgba);
+    }
+    
+    virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) const
+    {
+        m_gfxBridge.createCollisionObjectGraphicsObject(colObj,colorRgba);
+    }
+
+    btMultiBody* getBulletMultiBody()
+    {
+        return m_bulletMultiBody;
+    }
     
-	btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
-	btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
-	int m_numMotors;
 };
 
 
 
+btAlignedObjectArray<std::string> gFileNameArray;
 
 ImportUrdfSetup::ImportUrdfSetup()
 {
-	m_data = new ImportUrdfInternalData;
-
 	static int count = 0;
+    gFileNameArray.clear();
+    gFileNameArray.push_back("r2d2.urdf");
 
-	sprintf(m_fileName,fileNames[count++]);
+    //load additional urdf file names from file
-	int sz = sizeof(fileNames)/sizeof(char*);
+    
-	if (count>=sz)
+    FILE* f = fopen("urdf_files.txt","r");
+    if (f)
+    {
+        int result;
+        //warning: we don't avoid string buffer overflow in this basic example in fscanf
+        char fileName[1024];
+        do
+        {
+            result = fscanf(f,"%s",fileName);
+            if (result==1)
+            {
+                gFileNameArray.push_back(fileName);
+            }
+        } while (result==1);
+        
+        fclose(f);
+    }
+    
+    int numFileNames = gFileNameArray.size();
+
+    if (count>=numFileNames)
 	{
 		count=0;
 	}
+    sprintf(m_fileName,gFileNameArray[count++].c_str());
 }
 
 ImportUrdfSetup::~ImportUrdfSetup()
 {
-	delete m_data;
+
 }
 
 static btVector4 colors[4] =
@@ -82,18 +318,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
     memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
 }
 
-#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
 
-#include "urdf_samples.h"
 
-//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
-//#define USE_BARREL_VERTICES
-//#include "OpenGLWindow/ShapeData.h"
 
-#include <iostream>
-#include <fstream>
-
-using namespace urdf;
 
 void printTree(my_shared_ptr<const Link> link,int level = 0)
 {
@@ -157,24 +384,13 @@ struct URDF_JointInformation
 struct URDF2BulletMappings
 {
     btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
-    //btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
-
-	//btAlignedObjectArray<btTransform>		m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
 	btAlignedObjectArray<btScalar>			m_linkMasses;
-	//btAlignedObjectArray<btVector3>			m_linkLocalDiagonalInertiaTensors;
-	
-	//btAlignedObjectArray<int>				m_parentIndices;//for root, it is identity
-	//btAlignedObjectArray<btVector3>			m_jointAxisArray;
-	//btAlignedObjectArray<btTransform>			m_jointOffsetInParent;
-	//btAlignedObjectArray<btTransform>			m_jointOffsetInChild;
-	//btAlignedObjectArray<int>				m_jointTypeArray;
-
-
 	
 	bool m_createMultiBody;
 	int m_totalNumJoints;
 	btMultiBody*	m_bulletMultiBody;
 
+    btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
 	URDF2BulletMappings()
 		:m_createMultiBody(false),
 		m_totalNumJoints(0),
@@ -187,7 +403,7 @@ enum MyFileType
 {
 	FILE_STL=1,
 	FILE_COLLADA=2,
-	FILE_OBJ=3,
+    FILE_OBJ=3,
 };
 
 
@@ -272,10 +488,10 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
 					{
 						fileType = FILE_STL;
 					}
-					if (strstr(fullPath,".obj"))
+                    if (strstr(fullPath,".obj"))
-					{
+                    {
-						fileType = FILE_OBJ;
+                        fileType = FILE_OBJ;
-					}
+                    }
 
 
 					sprintf(fullPath, "%s%s", pathPrefix, filename);
@@ -288,11 +504,12 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
 
 						switch (fileType)
 						{
-						case FILE_OBJ:
+                            case FILE_OBJ:
-						{
+                            {
-							glmesh = LoadMeshFromObj(fullPath, pathPrefix);
+                                glmesh = LoadMeshFromObj(fullPath,pathPrefix);
-							break;
+                                break;
-						}
+                            }
+                           
 						case FILE_STL:
 						{
 							glmesh = LoadMeshFromSTL(fullPath);
@@ -381,6 +598,8 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
 						}
 						default:
 						{
+                            printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
+                            btAssert(0);
 						}
 						}
 
@@ -390,7 +609,7 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
 						}
 						else
 						{
-							printf("issue extracting mesh from COLLADA/STL/obj file %s\n", fullPath);
+							printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
 						}
 
 					}
@@ -573,10 +792,10 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
 					{
 						fileType = FILE_STL;
 					}
-					if (strstr(fullPath,".obj"))
+                    if (strstr(fullPath,".obj"))
-					{
+                   {
-						fileType = FILE_OBJ;
+                       fileType = FILE_OBJ;
-					}
+                   }
 
 					sprintf(fullPath,"%s%s",pathPrefix,filename);
 					FILE* f = fopen(fullPath,"rb");
@@ -588,11 +807,11 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
 						
 						switch (fileType)
 						{
-						case FILE_OBJ:
+                            case FILE_OBJ:
-						{
+                            {
-							glmesh = LoadMeshFromObj(fullPath,pathPrefix);
+                                glmesh = LoadMeshFromObj(fullPath,pathPrefix);
-							break;
+                                break;
-						}
+                            }
 						case FILE_STL:
 							{
 								glmesh = LoadMeshFromSTL(fullPath);
@@ -679,6 +898,8 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
 							}
 						default:
 							{
+                                printf("Unsupported file type in Collision: %s\n",fullPath);
+                                btAssert(0);
 							}
 						}
 					
@@ -725,27 +946,30 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
     }
 	return shape;
 }
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix, ImportUrdfInternalData* data)
+void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
 {
     //btCollisionShape* shape = 0;
 
 	btTransform linkTransformInWorldSpace;
 	linkTransformInWorldSpace.setIdentity();
 
-	btScalar mass = 1;
+	btScalar mass = 0;
 	btTransform inertialFrame;
 	inertialFrame.setIdentity();
     const Link* parentLink = (*link).getParent();
 	URDF_LinkInformation* pp = 0;
-	int linkIndex =  mappings.m_linkMasses.size();
+	
-	btVector3 localInertiaDiagonal(1,1,1);
+    int linkIndex =  mappings.m_linkMasses.size();//assuming root == 0, child links use contiguous numbering > 0
+    
+	btVector3 localInertiaDiagonal(0,0,0);
 
 	int parentIndex = -1;
 
 	
 	if (parentLink)
 	{
-		parentIndex = parentLink->m_link_index;
+		parentIndex = mappings.m_urdfLinkIndices2BulletLinkIndices[parentLink->m_link_index];
+        
 		btAssert(parentIndex>=0);
 	}
 
@@ -759,7 +983,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 		}
 	}
 
-	(*link).m_link_index = linkIndex;
+    mappings.m_urdfLinkIndices2BulletLinkIndices[(*link).m_link_index] = linkIndex;
     
 	if ((*link).inertial)
 	{
@@ -868,13 +1092,11 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 				btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame;
 				URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
 				
-				linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
-
 				if (!mappings.m_createMultiBody)
 				{
 					btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
 					rbci.m_startWorldTransform = inertialFrameInWorldSpace;
-					
+					linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
 					//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
 					btRigidBody* body = new btRigidBody(rbci);
 					world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
@@ -995,7 +1217,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                         {
 							if (mappings.m_createMultiBody)
 							{
-								printf("Revolute joint (btMultiBody)\n");
 								//todo: adjust the center of mass transform and pivot axis properly
 								/*mappings.m_bulletMultiBody->setupRevolute(
 									linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
@@ -1014,34 +1235,10 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 								disableParentCollision);
 								//linkInfo->m_localVisualFrame.setIdentity();
 								
-								{
-									if (data->m_numMotors<MAX_NUM_MOTORS)
-									{
-										const char* jointName = pj->name.c_str();
-										char motorName[1024];
-										sprintf(motorName,"%s q'", jointName);
-
-										btScalar* motorVel = &data->m_motorTargetVelocities[data->m_numMotors];
-										*motorVel = 0.f;
-
-										SliderParams slider(motorName,motorVel);
-										slider.m_minVal=-4;
-										slider.m_maxVal=4;
-										gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
-										btScalar maxMotorImpulse = 0.1f;
-
-										btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mappings.m_bulletMultiBody,linkIndex-1,0,0,maxMotorImpulse);
-										data->m_jointMotors[data->m_numMotors]=motor;
-										world1->addMultiBodyConstraint(motor);
-										data->m_numMotors++;
-									}
-								}
-								
 							} else
 							{
 								//only handle principle axis at the moment, 
 								//@todo(erwincoumans) orient the constraint for non-principal axis
-								printf("Hinge joint (btGeneric6DofSpring2Constraint)\n");
 								btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
 								int principleAxis = axis.closestAxis();
 								switch (principleAxis)
@@ -1094,7 +1291,11 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                         {
 							if (mappings.m_createMultiBody)
 							{
-								printf("Prismatic joint (btMultiBody)\n");
+								//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
+								//	parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
+
+								//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
+								//	parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
 
 								mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
 								offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
@@ -1103,11 +1304,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 
 								
 
-
 							} else
 							{
 								
-								printf("Slider joint (btGeneric6DofSpring2Constraint)\n");
 								btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
 								//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
 								btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
@@ -1211,7 +1410,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
     {
         if (*child)
         {
-            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix, data);
+            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix);
 
         }
         else
@@ -1297,31 +1496,49 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
     // print entire tree
     printTree(root_link);
+    printf("now using new interface\n");
+    std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
+    
+    //now print the tree using the new interface
+    MyURDF2Bullet u2b(robot,gfxBridge);
+    printTree(u2b, 0,0);
+    
     btTransform identityTrans;
 	identityTrans.setIdentity();
 	
 	int numJoints = (*robot).m_numJoints;
 
 	static bool useFeatherstone = true;
+    bool useUrdfInterfaceClass = true;
+    
     {
         URDF2BulletMappings mappings;
-		mappings.m_createMultiBody = useFeatherstone;
+      
-		mappings.m_totalNumJoints = numJoints;
+        btMultiBody* mb = 0;
-        URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix,m_data);
+        
+		if (!useUrdfInterfaceClass)
+        {
+            mappings.m_createMultiBody = useFeatherstone;
+            mappings.m_totalNumJoints = numJoints;
+            mappings.m_urdfLinkIndices2BulletLinkIndices.resize(numJoints+1,-2);//root and child links (=1+numJoints)
+            URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
+            mb = mappings.m_bulletMultiBody;
+        } else
+        {
+        
+            
+            //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+            int rootLinkIndex = u2b.getRootLinkIndex();
+            printf("urdf root link index = %d\n",rootLinkIndex);
+            ConvertURDF2Bullet(u2b,identityTrans,m_dynamicsWorld,useFeatherstone,pathPrefix);
+            mb = u2b.getBulletMultiBody();
+            
+        }
 		if (useFeatherstone)
 		{
-			btMultiBody* mb = mappings.m_bulletMultiBody;
+            mb->setHasSelfCollision(false);
-			mb->setHasSelfCollision(false);
+			mb->finalizeMultiDof();
-			if (mb->isMultiDof())
-			{
-				mb->finalizeMultiDof();
-			}
 			m_dynamicsWorld->addMultiBody(mb);
-
-			for (int i=0;i<m_data->m_numMotors;i++)
-			{
-				m_data->m_jointMotors[i]->finalizeMultiDof();
-			}
 		}
     }
 
@@ -1341,7 +1558,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createCollisionShapeGraphicsObject(box);
         btTransform start; start.setIdentity();
         btVector3 groundOrigin(0,0,0);
-        groundOrigin[upAxis]=-2.;
+        groundOrigin[upAxis]=-2;//.5;
         start.setOrigin(groundOrigin);
         btRigidBody* body =  createRigidBody(0,start,box);
         //m_dynamicsWorld->removeRigidBody(body);
@@ -1358,14 +1575,7 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
 {
 	if (m_dynamicsWorld)
 	{
-		//set the new target velocities
-		for (int i=0;i<m_data->m_numMotors;i++)
-		{
-			m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
-		}
 		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
-		int actualSteps = m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
-		//int actualSteps = m_dynamicsWorld->stepSimulation(deltaTime,1000,1./3000.f);//240.);
-		//printf("deltaTime %f took actualSteps = %d\n",deltaTime,actualSteps);
 	}
 }

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -7,7 +7,6 @@
 class ImportUrdfSetup : public CommonMultiBodySetup
 {
     char m_fileName[1024];
-	struct ImportUrdfInternalData* m_data;
     
 public:
     ImportUrdfSetup();

Demos3/ImportURDFDemo/URDF2Bullet.cpp

@@ -0,0 +1,505 @@
+#include "URDF2Bullet.h"
+#include <stdio.h>
+#include "LinearMath/btTransform.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
+static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
+static bool enableConstraints = true;
+
+static btVector4 colors[4] =
+{
+    btVector4(1,0,0,1),
+    btVector4(0,1,0,1),
+    btVector4(0,1,1,1),
+    btVector4(1,1,0,1),
+};
+
+
+static btVector3 selectColor2()
+{
+    
+    static int curColor = 0;
+    btVector4 color = colors[curColor];
+    curColor++;
+    curColor&=3;
+    return color;
+}
+
+void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
+{
+    btAlignedObjectArray<int> childIndices;
+    u2b.getLinkChildIndices(linkIndex,childIndices);
+    
+    int numChildren = childIndices.size();
+    
+    indentationLevel+=2;
+    int count = 0;
+    for (int i=0;i<numChildren;i++)
+    {
+        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
+        printTree(u2b,childLinkIndex,indentationLevel);
+    }
+}
+
+struct URDF2BulletCachedData
+{
+    URDF2BulletCachedData()
+    :m_totalNumJoints1(0),
+    m_currentMultiBodyLinkIndex(-1),
+    m_bulletMultiBody(0)
+    {
+        
+    }
+    //these arrays will be initialized in the 'InitURDF2BulletCache'
+    
+    btAlignedObjectArray<int> m_urdfLinkParentIndices;
+    btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
+    btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
+    btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
+    
+    int m_currentMultiBodyLinkIndex;
+    
+    class btMultiBody* m_bulletMultiBody;
+    
+    //this will be initialized in the constructor
+    int m_totalNumJoints1;
+    int getParentUrdfIndex(int linkIndex) const
+    {
+        return m_urdfLinkParentIndices[linkIndex];
+    }
+    int getMbIndexFromUrdfIndex(int urdfIndex) const
+    {
+        if (urdfIndex==-2)
+            return -2;
+        return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
+    }
+    
+    
+    void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
+    {
+        m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
+    }
+    
+    class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
+    {
+        return m_urdfLink2rigidBodies[urdfLinkIndex];
+    }
+    
+    void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
+    {
+        btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
+        
+        m_urdfLink2rigidBodies[urdfLinkIndex] = body;
+        m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
+    }
+    
+};
+
+void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex)
+{
+    btAlignedObjectArray<int> childIndices;
+    u2b.getLinkChildIndices(linkIndex,childIndices);
+    printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
+    for (int i=0;i<childIndices.size();i++)
+    {
+        printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
+    }
+    cache.m_totalNumJoints1 += childIndices.size();
+    for (int i=0;i<childIndices.size();i++)
+    {
+        int childIndex =childIndices[i];
+        ComputeTotalNumberOfJoints(u2b,cache,childIndex);
+    }
+}
+
+void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
+{
+    cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
+    cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
+    
+    btAlignedObjectArray<int> childIndices;
+    u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
+    for (int i=0;i<childIndices.size();i++)
+    {
+        ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
+    }
+}
+
+void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
+{
+    //compute the number of links, and compute parent indices array (and possibly other cached data?)
+    cache.m_totalNumJoints1 = 0;
+
+    int rootLinkIndex = u2b.getRootLinkIndex();
+    if (rootLinkIndex>=0)
+    {
+        ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
+        int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;
+        
+        cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
+        cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
+        cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
+        
+        cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
+        ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
+    }
+    
+}
+
+void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
+{
+    printf("start converting/extracting data from URDF interface\n");
+    
+    btTransform linkTransformInWorldSpace;
+    linkTransformInWorldSpace.setIdentity();
+    
+
+    int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
+
+    int urdfParentIndex =   cache.getParentUrdfIndex(urdfLinkIndex);
+    int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
+    btRigidBody* parentRigidBody = 0;
+    
+    std::string name = u2b.getLinkName(urdfLinkIndex);
+    printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
+    printf("mb link index = %d\n",mbLinkIndex);
+
+	btTransform parentLocalInertialFrame;
+	parentLocalInertialFrame.setIdentity();
+	btScalar parentMass(1);
+	btVector3 parentLocalInertiaDiagonal(1,1,1);
+
+    if (urdfParentIndex==-2)
+    {
+        printf("root link has no parent\n");
+    } else
+    {
+        printf("urdf parent index = %d\n",urdfParentIndex);
+        printf("mb parent index = %d\n",mbParentIndex);
+        parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
+		u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
+        
+    }
+    
+    btScalar mass = 0;
+    btTransform localInertialFrame;
+    localInertialFrame.setIdentity();
+    btVector3 localInertiaDiagonal(0,0,0);
+    u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
+    
+	
+	
+    
+    
+    btTransform parent2joint;
+    parent2joint.setIdentity();
+
+    int jointType;
+    btVector3 jointAxisInJointSpace;
+    btScalar jointLowerLimit;
+    btScalar jointUpperLimit;
+    
+
+    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
+    
+    
+    linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+
+    int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+    
+    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+    
+    if (compoundShape)
+    {
+        
+        
+        btVector3 color = selectColor2();
+        /* 
+         if (visual->material.get())
+         {
+            color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
+         }
+         */
+        //btVector3 localInertiaDiagonal(0, 0, 0);
+        //if (mass)
+        //{
+        //	shape->calculateLocalInertia(mass, localInertiaDiagonal);
+        //}
+        
+        btRigidBody* linkRigidBody = 0;
+        btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
+        
+        if (!createMultiBody)
+        {
+            btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
+            linkRigidBody = body;
+            
+            world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
+            
+            compoundShape->setUserIndex(graphicsIndex);
+            
+            u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
+            cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
+        } else
+        {
+            if (cache.m_bulletMultiBody==0)
+            {
+                bool multiDof = true;
+                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.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
+            }
+            
+        }
+        
+        //create a joint if necessary
+        if (hasParentJoint)        {
+            
+            btTransform offsetInA,offsetInB;
+            offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
+            offsetInB = localInertialFrame.inverse();
+            
+            bool disableParentCollision = true;
+            switch (jointType)
+            {
+                case URDF2Bullet::FixedJoint:
+                {
+                    if (createMultiBody)
+                    {
+                        //todo: adjust the center of mass transform and pivot axis properly
+                        
+                        printf("Fixed joint (btMultiBody)\n");
+                        btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
+                        cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+                                                               rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
+                        
+                        btMatrix3x3 rm(rot);
+                        btScalar y,p,r;
+                        rm.getEulerZYX(y,p,r);
+                        printf("y=%f,p=%f,r=%f\n", y,p,r);
+                        
+                    } else
+                    {
+                        printf("Fixed joint\n");
+                        
+                        btMatrix3x3 rm(offsetInA.getBasis());
+                        btScalar y,p,r;
+                        rm.getEulerZYX(y,p,r);
+                        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);
+
+                        dof6->setLinearLowerLimit(btVector3(0,0,0));
+                        dof6->setLinearUpperLimit(btVector3(0,0,0));
+                        
+                        dof6->setAngularLowerLimit(btVector3(0,0,0));
+                        dof6->setAngularUpperLimit(btVector3(0,0,0));
+                        
+                        if (enableConstraints)
+                            world1->addConstraint(dof6,true);
+                    }
+                    break;
+                }
+                case URDF2Bullet::ContinuousJoint:
+                case URDF2Bullet::RevoluteJoint:
+                {
+                    if (createMultiBody)
+                    {
+                  
+                        
+                        cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+                                                                  offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
+                                                                  -offsetInB.getOrigin(),
+                                                                  disableParentCollision);
+                        
+                    } else
+                    {
+                        //only handle principle axis at the moment,
+                        //@todo(erwincoumans) orient the constraint for non-principal axis
+                        int principleAxis = jointAxisInJointSpace.closestAxis();
+                        switch (principleAxis)
+                        {
+                            case 0:
+                            {
+                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
+                                dof6->setLinearLowerLimit(btVector3(0,0,0));
+                                dof6->setLinearUpperLimit(btVector3(0,0,0));
+                                
+                                dof6->setAngularUpperLimit(btVector3(-1,0,0));
+                                dof6->setAngularLowerLimit(btVector3(1,0,0));
+                                
+                                if (enableConstraints)
+                                    world1->addConstraint(dof6,true);
+                                break;
+                            }
+                            case 1:
+                            {
+                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
+                                dof6->setLinearLowerLimit(btVector3(0,0,0));
+                                dof6->setLinearUpperLimit(btVector3(0,0,0));
+                                
+                                dof6->setAngularUpperLimit(btVector3(0,-1,0));
+                                dof6->setAngularLowerLimit(btVector3(0,1,0));
+                                
+                                if (enableConstraints)
+                                    world1->addConstraint(dof6,true);
+                                break;
+                            }
+                            case 2:
+                            default:
+                            {
+                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
+                                dof6->setLinearLowerLimit(btVector3(0,0,0));
+                                dof6->setLinearUpperLimit(btVector3(0,0,0));
+                                
+                                dof6->setAngularUpperLimit(btVector3(0,0,-1));
+                                dof6->setAngularLowerLimit(btVector3(0,0,0));
+                                
+                                if (enableConstraints)
+                                    world1->addConstraint(dof6,true);
+                            }
+                        };
+                        printf("Revolute/Continuous joint\n");
+                    }
+                    break;
+                }
+                case URDF2Bullet::PrismaticJoint:
+                {
+                    if (createMultiBody)
+                    {
+                        
+                        cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+                                                                   offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
+                                                                   -offsetInB.getOrigin(),
+                                                                   disableParentCollision);
+                        
+                        
+                        
+                    } else
+                    {
+                        btGeneric6DofSpring2Constraint* dof6 = u2b.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)
+                        {
+                            case 0:
+                            {
+                                dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
+                                dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
+                                break;
+                            }
+                            case 1:
+                            {
+                                dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
+                                dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
+                                break;
+                            }
+                            case 2:
+                            default:
+                            {
+                                dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
+                                dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
+                            }
+                        };
+                        
+                        dof6->setAngularLowerLimit(btVector3(0,0,0));
+                        dof6->setAngularUpperLimit(btVector3(0,0,0));
+                        if (enableConstraints)
+                            world1->addConstraint(dof6,true);
+                        
+                        printf("Prismatic\n");
+                    }
+                    break;
+                }
+                default:
+                {
+                    printf("Error: unsupported joint type in URDF (%d)\n", jointType);
+					btAssert(0);
+                }
+            }
+            
+        }
+        
+        if (createMultiBody)
+        {
+            if (compoundShape->getNumChildShapes()>0)
+            {
+                btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
+                                
+                compoundShape->setUserIndex(graphicsIndex);
+                
+                col->setCollisionShape(compoundShape);
+                
+                btTransform tr;
+                tr.setIdentity();
+                tr = linkTransformInWorldSpace;
+                //if we don't set the initial pose of the btCollisionObject, the simulator will do this 
+                //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
+                
+                col->setWorldTransform(tr);
+                
+                bool isDynamic = true;
+                short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+                short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+                
+                world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
+                
+                btVector3 color = selectColor2();//(0.0,0.0,0.5);
+               
+                u2b.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
+                
+                btScalar friction = 0.5f;
+                
+                col->setFriction(friction);
+                
+                if (mbLinkIndex>=0) //???? double-check +/- 1
+                {
+                    cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
+                } else
+                {
+                    cache.m_bulletMultiBody->setBaseCollider(col);
+                }
+            }
+        }
+    }
+    
+    
+    btAlignedObjectArray<int> urdfChildIndices;
+    u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
+    
+    int numChildren = urdfChildIndices.size();
+    
+    for (int i=0;i<numChildren;i++)
+    {
+        int urdfChildLinkIndex = urdfChildIndices[i];
+        
+        ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
+    }
+    
+}
+
+void ConvertURDF2Bullet(const URDF2Bullet& u2b, 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);
+    
+}
+
+

Demos3/ImportURDFDemo/URDF2Bullet.h

@@ -0,0 +1,68 @@
+#ifndef _URDF2BULLET_H
+#define _URDF2BULLET_H
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include <string>
+class btVector3;
+class btTransform;
+class btMultiBodyDynamicsWorld;
+class btTransform;
+
+class URDF2Bullet
+{
+    
+public:
+
+    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;
+
+    //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;
+    
+};
+
+
+void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0);
+
+
+
+void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix);
+
+
+#endif //_URDF2BULLET_H
+

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -58,7 +58,7 @@ class btRigidBody;
 
 enum RotateOrder
 {
-	RO_XYZ,
+	RO_XYZ=0,
 	RO_XZY,
 	RO_YXZ,
 	RO_YZX,