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

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -2,6 +2,7 @@
 #include "ImportURDFSetup.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "Bullet3Common/b3FileUtils.h"
+#include "../ImportObjDemo/LoadMeshFromObj.h"
 #include "../ImportSTLDemo/LoadMeshFromSTL.h"
 #include "../ImportColladaDemo/LoadMeshFromCollada.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -33,11 +34,13 @@ class MyURDF2Bullet : public URDF2Bullet
     my_shared_ptr<ModelInterface> m_robot;
     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_gfxBridge(gfxBridge),
+    m_bulletMultiBody(0)
     {
         m_robot->getLinks(m_links);
         
@@ -53,7 +56,7 @@ public:
         if (m_links.size())
         {
             int rootLinkIndex = m_robot->getRoot()->m_link_index;
-            btAssert(m_links[0]->m_link_index == rootLinkIndex);
+           // btAssert(m_links[0]->m_link_index == rootLinkIndex);
             return rootLinkIndex;
         }
         return -1;
@@ -92,7 +95,7 @@ public:
         }
     }
     
-    virtual bool getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
     {
         jointLowerLimit = 0.f;
         jointUpperLimit = 0.f;
@@ -146,7 +149,7 @@ public:
         }
     }
 
-    virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
+    virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
     {
         btAlignedObjectArray<GLInstanceVertex> vertices;
         btAlignedObjectArray<int> indices;
@@ -175,7 +178,7 @@ public:
         
     }
     
-    virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
+    virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
     {
         
         btCompoundShape* compoundShape = new btCompoundShape();
@@ -201,6 +204,34 @@ public:
         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
     {
         
@@ -212,25 +243,50 @@ public:
         m_gfxBridge.createCollisionObjectGraphicsObject(colObj,colorRgba);
     }
 
+    btMultiBody* getBulletMultiBody()
+    {
+        return m_bulletMultiBody;
+    }
+    
 };
 
 
 
-const char* fileNames[] =
+btAlignedObjectArray<std::string> gFileNameArray;
-{
-	"r2d2.urdf",
-};
 
 ImportUrdfSetup::ImportUrdfSetup()
 {
 	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()
@@ -346,7 +402,8 @@ struct URDF2BulletMappings
 enum MyFileType
 {
 	FILE_STL=1,
-	FILE_COLLADA=2
+	FILE_COLLADA=2,
+    FILE_OBJ=3,
 };
 
 
@@ -431,6 +488,10 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
 					{
 						fileType = FILE_STL;
 					}
+                    if (strstr(fullPath,".obj"))
+                    {
+                        fileType = FILE_OBJ;
+                    }
 
 
 					sprintf(fullPath, "%s%s", pathPrefix, filename);
@@ -443,6 +504,12 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
 
 						switch (fileType)
 						{
+                            case FILE_OBJ:
+                            {
+                                glmesh = LoadMeshFromObj(fullPath,pathPrefix);
+                                break;
+                            }
+                           
 						case FILE_STL:
 						{
 							glmesh = LoadMeshFromSTL(fullPath);
@@ -531,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);
 						}
 						}
 
@@ -723,7 +792,10 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
 					{
 						fileType = FILE_STL;
 					}
-					
+                    if (strstr(fullPath,".obj"))
+                   {
+                       fileType = FILE_OBJ;
+                   }
 
 					sprintf(fullPath,"%s%s",pathPrefix,filename);
 					FILE* f = fopen(fullPath,"rb");
@@ -735,6 +807,11 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
 						
 						switch (fileType)
 						{
+                            case FILE_OBJ:
+                            {
+                                glmesh = LoadMeshFromObj(fullPath,pathPrefix);
+                                break;
+                            }
 						case FILE_STL:
 							{
 								glmesh = LoadMeshFromSTL(fullPath);
@@ -821,6 +898,8 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
 							}
 						default:
 							{
+                                printf("Unsupported file type in Collision: %s\n",fullPath);
+                                btAssert(0);
 							}
 						}
 					
@@ -1422,7 +1501,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
     
     //now print the tree using the new interface
     MyURDF2Bullet u2b(robot,gfxBridge);
-    u2b.printTree(0,0);
+    printTree(u2b, 0,0);
     
     btTransform identityTrans;
 	identityTrans.setIdentity();
@@ -1434,7 +1513,8 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
     
     {
         URDF2BulletMappings mappings;
-        URDF2BulletCachedData cache;
+      
+        btMultiBody* mb = 0;
         
 		if (!useUrdfInterfaceClass)
         {
@@ -1442,25 +1522,21 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
             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
         {
         
-            URDF2BulletConfig config;
+            
-			config.m_createMultiBody = useFeatherstone;
-            //ConvertURDF2Bullet(URDF2Bullet& u2b, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,URDF2BulletConfig& config, const char* pathPrefix)
-           
             //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();
             
-            
-            InitURDF2BulletCache(u2b,cache);
-            ConvertURDF2Bullet(u2b,cache,rootLinkIndex,identityTrans,m_dynamicsWorld,config,pathPrefix);
         }
 		if (useFeatherstone)
 		{
-            btMultiBody* mb = useUrdfInterfaceClass? cache.m_bulletMultiBody : mappings.m_bulletMultiBody;
+            mb->setHasSelfCollision(false);
-			mb->setHasSelfCollision(false);
 			mb->finalizeMultiDof();
 			m_dynamicsWorld->addMultiBody(mb);
 		}
@@ -1482,7 +1558,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createCollisionShapeGraphicsObject(box);
         btTransform start; start.setIdentity();
         btVector3 groundOrigin(0,0,0);
-        groundOrigin[upAxis]=-1.5;
+        groundOrigin[upAxis]=-2;//.5;
         start.setOrigin(groundOrigin);
         btRigidBody* body =  createRigidBody(0,start,box);
         //m_dynamicsWorld->removeRigidBody(body);

Demos3/ImportURDFDemo/URDF2Bullet.cpp

@@ -30,10 +30,10 @@ static btVector3 selectColor2()
     return color;
 }
 
-void URDF2Bullet::printTree(int linkIndex, int indentationLevel)
+void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
 {
     btAlignedObjectArray<int> childIndices;
-    getLinkChildIndices(linkIndex,childIndices);
+    u2b.getLinkChildIndices(linkIndex,childIndices);
     
     int numChildren = childIndices.size();
     
@@ -42,14 +42,68 @@ void URDF2Bullet::printTree(int linkIndex, int indentationLevel)
     for (int i=0;i<numChildren;i++)
     {
         int childLinkIndex = childIndices[i];
-        std::string name = getLinkName(childLinkIndex);
+        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(childLinkIndex,indentationLevel);
+        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;
@@ -102,7 +156,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
     
 }
 
-void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, const char* pathPrefix)
+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");
     
@@ -156,23 +210,24 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
     btScalar jointUpperLimit;
     
 
-    bool hasParentJoint = u2b.getParent2JointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
+    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
     
     
     linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
 
-    int graphicsIndex = u2b.convertLinkVisuals(urdfLinkIndex,pathPrefix,localInertialFrame);
+    int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
     
-    btCompoundShape* compoundShape = u2b.convertLinkCollisions(urdfLinkIndex,pathPrefix,localInertialFrame);
+    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
     
     if (compoundShape)
     {
         
         
         btVector3 color = selectColor2();
-        /*                if (visual->material.get())
+        /* 
+         if (visual->material.get())
          {
-         color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
+            color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
          }
          */
         //btVector3 localInertiaDiagonal(0, 0, 0);
@@ -182,18 +237,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
         //}
         
         btRigidBody* linkRigidBody = 0;
-        
-        //btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame;
         btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
-       // URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
         
-        if (!mappings.m_createMultiBody)
+        if (!createMultiBody)
         {
-            btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
+            btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
-            rbci.m_startWorldTransform = inertialFrameInWorldSpace;
-           // linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
-            //rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
-            btRigidBody* body = new btRigidBody(rbci);
             linkRigidBody = body;
             
             world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
@@ -201,13 +249,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
             compoundShape->setUserIndex(graphicsIndex);
             
             u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
-            
-//            gfxBridge.createRigidBodyGraphicsObject(body, color);
-            
-           // linkInfo->m_bulletRigidBody = body;
-            
             cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
-            
         } else
         {
             if (cache.m_bulletMultiBody==0)
@@ -216,74 +258,39 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                 bool canSleep = false;
                 bool isFixedBase = (mass==0);//todo: figure out when base is fixed
                 int totalNumJoints = cache.m_totalNumJoints1;
-                cache.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
+                cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
                 
                 cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
             }
             
         }
         
-        
-      //  linkInfo->m_collisionShape = compoundShape;
-      //  linkInfo->m_localInertiaDiagonal = localInertiaDiagonal;
-      //  linkInfo->m_mass = mass;
-        //linkInfo->m_localVisualFrame =visual_frame;
-      //  linkInfo->m_localInertialFrame =inertialFrame;
-        
-        
-        
-      //  linkInfo->m_thisLink = link.get();
-      //  const Link* p = link.get();
-     //   mappings.m_link2rigidbody.insert(p, linkInfo);
-        
-        
         //create a joint if necessary
-        if (hasParentJoint)//(*link).parent_joint && pp)
+        if (hasParentJoint)        {
-        {
             
-//            const Joint* pj = (*link).parent_joint.get();
             btTransform offsetInA,offsetInB;
-            static bool once = true;
-            
-            offsetInA.setIdentity();
-            static bool toggle=false;
-            
-            //offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
             offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
-            
-            offsetInB.setIdentity();
-            //offsetInB = visual_frame.inverse();
             offsetInB = localInertialFrame.inverse();
             
-            
             bool disableParentCollision = true;
             switch (jointType)
             {
                 case URDF2Bullet::FixedJoint:
                 {
-                    if (mappings.m_createMultiBody)
+                    if (createMultiBody)
                     {
                         //todo: adjust the center of mass transform and pivot axis properly
                         
                         printf("Fixed joint (btMultiBody)\n");
-                        //btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
                         btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
-                        //toggle=!toggle;
-                        //mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
-                        //	rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
                         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);
-                        //parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
-                        //linkInfo->m_localVisualFrame.setIdentity();
                         printf("y=%f,p=%f,r=%f\n", y,p,r);
                         
-                        
-                        
                     } else
                     {
                         printf("Fixed joint\n");
@@ -291,12 +298,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                         btMatrix3x3 rm(offsetInA.getBasis());
                         btScalar y,p,r;
                         rm.getEulerZYX(y,p,r);
-                        //parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
-                        //linkInfo->m_localVisualFrame.setIdentity();
                         printf("y=%f,p=%f,r=%f\n", y,p,r);
                         
-                        btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+                        //we could also use btFixedConstraint but it has some issues
-                        //                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+                        btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+
                         dof6->setLinearLowerLimit(btVector3(0,0,0));
                         dof6->setLinearUpperLimit(btVector3(0,0,0));
                         
@@ -305,34 +311,20 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                         
                         if (enableConstraints)
                             world1->addConstraint(dof6,true);
-                        
-                        //                            btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
-                        //                          world->addConstraint(fixed,true);
                     }
                     break;
                 }
                 case URDF2Bullet::ContinuousJoint:
                 case URDF2Bullet::RevoluteJoint:
                 {
-                    if (mappings.m_createMultiBody)
+                    if (createMultiBody)
                     {
-                        //todo: adjust the center of mass transform and pivot axis properly
+                  
-                        /*mappings.m_bulletMultiBody->setupRevolute(
-                         linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
-                         
-                         parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(),
-                         btVector3(0,0,0),//offsetInB.getOrigin(),
-                         disableParentCollision);
-                         */
-                        
                         
                         cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
-                                                                  //parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
                                                                   offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-                                                                  
                                                                   -offsetInB.getOrigin(),
                                                                   disableParentCollision);
-                        //linkInfo->m_localVisualFrame.setIdentity();
                         
                     } else
                     {
@@ -343,7 +335,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                         {
                             case 0:
                             {
-                                btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
+                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
                                 dof6->setLinearLowerLimit(btVector3(0,0,0));
                                 dof6->setLinearUpperLimit(btVector3(0,0,0));
                                 
@@ -356,7 +348,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                             }
                             case 1:
                             {
-                                btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
+                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
                                 dof6->setLinearLowerLimit(btVector3(0,0,0));
                                 dof6->setLinearUpperLimit(btVector3(0,0,0));
                                 
@@ -370,7 +362,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                             case 2:
                             default:
                             {
-                                btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
+                                btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
                                 dof6->setLinearLowerLimit(btVector3(0,0,0));
                                 dof6->setLinearUpperLimit(btVector3(0,0,0));
                                 
@@ -387,7 +379,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                 }
                 case URDF2Bullet::PrismaticJoint:
                 {
-                    if (mappings.m_createMultiBody)
+                    if (createMultiBody)
                     {
                         
                         cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
@@ -399,8 +391,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                         
                     } else
                     {
-                        
+                        btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
-                        btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
                         //todo(erwincoumans) for now, we only support principle axis along X, Y or Z
                         int principleAxis = jointAxisInJointSpace.closestAxis();
                         switch (principleAxis)
@@ -437,21 +428,18 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                 default:
                 {
                     printf("Error: unsupported joint type in URDF (%d)\n", jointType);
-					 btAssert(0);
+					btAssert(0);
                 }
             }
             
         }
         
-        if (mappings.m_createMultiBody)
+        if (createMultiBody)
         {
             if (compoundShape->getNumChildShapes()>0)
             {
-                btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(cache.m_bulletMultiBody, mbLinkIndex); //or mbLinkIndex-1??? double-check
+                btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
-                
+                                
-                //btCompoundShape* comp = new btCompoundShape();
-                //comp->addChildShape(linkInfo->m_localVisualFrame,shape);
-                
                 compoundShape->setUserIndex(graphicsIndex);
                 
                 col->setCollisionShape(compoundShape);
@@ -462,8 +450,6 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                 //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
                 
-                //tr.setOrigin(local_origin[0]);
-                //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
                 col->setWorldTransform(tr);
                 
                 bool isDynamic = true;
@@ -501,7 +487,19 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
     {
         int urdfChildLinkIndex = urdfChildIndices[i];
         
-        ConvertURDF2Bullet(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,mappings,pathPrefix);
+        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

@@ -5,115 +5,14 @@
 #include <string>
 class btVector3;
 class btTransform;
-
-struct URDF2BulletConfig
-{
-    URDF2BulletConfig()
-    :m_createMultiBody(true)
-    {
-        
-    }
-    
-    //true to create a btMultiBody, false to use btRigidBody
-    bool m_createMultiBody;
-};
-
-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;
-        
-        /*
-        struct URDF_LinkInformation
-        {
-            const Link* m_thisLink;
-            int m_linkIndex;
-            //int m_parentIndex;
-            
-            btTransform m_localInertialFrame;
-            //btTransform m_localVisualFrame;
-            btTransform m_bodyWorldTransform;
-            btVector3 m_localInertiaDiagonal;
-            btScalar m_mass;
-            
-            btCollisionShape* m_collisionShape;
-            btRigidBody* m_bulletRigidBody;
-            
-            URDF_LinkInformation()
-            :m_thisLink(0),
-            m_linkIndex(-2),
-            //m_parentIndex(-2),
-            m_collisionShape(0),
-            m_bulletRigidBody(0)
-            {
-                
-            }
-            virtual ~URDF_LinkInformation()
-            {
-                printf("~\n");
-            }
-        };
-         */
-        
-    }
-    
-    
-};
-
 class btMultiBodyDynamicsWorld;
 class btTransform;
 
 class URDF2Bullet
 {
-
     
 public:
+
     enum {
         RevoluteJoint=1,
         PrismaticJoint,
@@ -122,8 +21,7 @@ public:
         PlanarJoint,
         FixedJoint,
     };
-    void printTree(int linkIndex, int indentationLevel=0);
+ 
-
     ///return >=0 for the root link index, -1 if there is no root link
     virtual int getRootLinkIndex() const = 0;
     
@@ -136,23 +34,35 @@ public:
     ///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 getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
     
-    virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
+    virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
     
-    virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) 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, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) 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 InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache);
+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);
+
 
-void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, 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,