add some minor fixes for URDF2Bullet to make it work (needs more testing)

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -123,7 +123,7 @@ public:
                     jointType = URDF2Bullet::PlanarJoint;
                     break;
                 case Joint::CONTINUOUS:
-                    jointType = URDF2Bullet::FloatingJoint;
+					jointType = URDF2Bullet::ContinuousJoint;
                     break;
                 default:
                 {
@@ -1429,8 +1429,8 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	
 	int numJoints = (*robot).m_numJoints;
 
-	static bool useFeatherstone = false;
+	static bool useFeatherstone = true;
-    bool useUrdfInterfaceClass = false;
+    bool useUrdfInterfaceClass = true;
     
     {
         URDF2BulletMappings mappings;
@@ -1446,7 +1446,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         {
         
             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

Demos3/ImportURDFDemo/URDF2Bullet.cpp

@@ -94,6 +94,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
         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);
@@ -119,6 +120,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
     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");
@@ -127,6 +133,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
         printf("urdf parent index = %d\n",urdfParentIndex);
         printf("mb parent index = %d\n",mbParentIndex);
         parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
+		u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
         
     }
     
@@ -138,6 +145,8 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
     
 	
 	
+    
+    
     btTransform parent2joint;
     parent2joint.setIdentity();
 
@@ -240,7 +249,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
             static bool toggle=false;
             
             //offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
-            offsetInA = localInertialFrame.inverse()*parent2joint;
+            offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
             
             offsetInB.setIdentity();
             //offsetInB = visual_frame.inverse();
@@ -428,6 +437,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                 default:
                 {
                     printf("Error: unsupported joint type in URDF (%d)\n", jointType);
+					 btAssert(0);
                 }
             }
             
@@ -470,7 +480,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
                 
                 col->setFriction(friction);
                 
-                if (mbParentIndex>=0) //???? double-check +/- 1
+                if (mbLinkIndex>=0) //???? double-check +/- 1
                 {
                     cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
                 } else

Demos3/ImportURDFDemo/URDF2Bullet.h

@@ -1,6 +1,7 @@
 #ifndef _URDF2BULLET_H
 #define _URDF2BULLET_H
 #include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
 #include <string>
 class btVector3;
 class btTransform;
@@ -31,6 +32,7 @@ struct URDF2BulletCachedData
     btAlignedObjectArray<int> m_urdfLinkParentIndices;
     btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
     btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
+	btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
     
     int m_currentMultiBodyLinkIndex;
     
@@ -52,7 +54,7 @@ struct URDF2BulletCachedData
     
     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)
@@ -65,6 +67,7 @@ struct URDF2BulletCachedData
         btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
         
         m_urdfLink2rigidBodies[urdfLinkIndex] = body;
+		m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
         
         /*
         struct URDF_LinkInformation