fix an issue with btMultiBodyConstraint, automatically 'finalizeMultiDof' to pre-allocate jacobian data

enable joint limit for slider/prismatic joint in btMultiBody version of URDF loader

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -29,17 +29,17 @@
 class ImportUrdfSetup : public CommonMultiBodyBase
 {
     char m_fileName[1024];
-    
+
     struct ImportUrdfInternalData* m_data;
 	bool m_useMultiBody;
-    
+
 public:
     ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
     virtual ~ImportUrdfSetup();
 
 	virtual void initPhysics();
 	virtual void stepSimulation(float deltaTime);
-    
+
     void setFileName(const char* urdfFileName);
 
 	virtual void resetCamera()
@@ -64,7 +64,7 @@ struct ImportUrdfInternalData
     :m_numMotors(0)
     {
     }
-    
+
     btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
     btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
     int m_numMotors;
@@ -93,10 +93,10 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		gFileNameArray.clear();
 		gFileNameArray.push_back("r2d2.urdf");
 
-		
+
-    
+
 		//load additional urdf file names from file
-    
+
 		FILE* f = fopen("urdf_files.txt","r");
 		if (f)
 		{
@@ -112,10 +112,10 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 					gFileNameArray.push_back(fileName);
 				}
 			} while (result==1);
-        
+
 			fclose(f);
 		}
-    
+
 		int numFileNames = gFileNameArray.size();
 
 		if (count>=numFileNames)
@@ -173,14 +173,14 @@ void ImportUrdfSetup::initPhysics()
     +btIDebugDraw::DBG_DrawAabb
         );//+btIDebugDraw::DBG_DrawConstraintLimits);
 
-	
+
 	btVector3 gravity(0,0,0);
 	gravity[upAxis]=-9.8;
 
 	m_dynamicsWorld->setGravity(gravity);
-    
 
-	
+
+
     //now print the tree using the new interface
     MyURDFImporter u2b(m_guiHelper);
 	bool loadOk =  u2b.loadURDF(m_fileName);
@@ -188,17 +188,17 @@ void ImportUrdfSetup::initPhysics()
 	if (loadOk)
 	{
 		u2b.printTree();
-    
+
 		btTransform identityTrans;
 		identityTrans.setIdentity();
-	
+
-	
+
 		{
-        
+
-      
+
 			btMultiBody* mb = 0;
-        
+
-            
+
 			//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);
@@ -209,22 +209,22 @@ void ImportUrdfSetup::initPhysics()
 
 			if (m_useMultiBody)
 			{
-           
+
-            
+
-                
+
 				//create motors for each joint
-                
+
 				for (int i=0;i<mb->getNumLinks();i++)
 				{
 					int mbLinkIndex = i;
-					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute 
+					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
 					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
 					)
 					{
 						if (m_data->m_numMotors<MAX_NUM_MOTORS)
 						{
 							int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
-                            
+
 							std::string jointName = u2b.getJointName(urdfLinkIndex);
 							char motorName[1024];
 							sprintf(motorName,"%s q'", jointName.c_str());
@@ -241,15 +241,19 @@ void ImportUrdfSetup::initPhysics()
 							m_data->m_numMotors++;
 						}
 					}
-                    
+
 				}
 			}
 		}
-	
+
 		//the btMultiBody support is work-in-progress :-)
 
-	
+        for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
-	
+        {
+            m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
+        }
+
+
 
 		bool createGround=true;
 		if (createGround)
@@ -284,7 +288,7 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
         {
             m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
         }
-        
+
 		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
 		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
 	}

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -25,7 +25,7 @@ static btVector4 colors[4] =
 
 static btVector3 selectColor2()
 {
-    
+
     static int curColor = 0;
     btVector4 color = colors[curColor];
     curColor++;
@@ -37,9 +37,9 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL
 {
     btAlignedObjectArray<int> childIndices;
     u2b.getLinkChildIndices(linkIndex,childIndices);
-    
+
     int numChildren = childIndices.size();
-    
+
     indentationLevel+=2;
     int count = 0;
     for (int i=0;i<numChildren;i++)
@@ -61,19 +61,19 @@ struct URDF2BulletCachedData
     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
@@ -86,26 +86,26 @@ struct URDF2BulletCachedData
             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 URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
@@ -129,7 +129,7 @@ void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedDat
 {
     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++)
@@ -148,32 +148,32 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
     {
         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 URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
 {
     printf("start converting/extracting data from URDF interface\n");
-    
+
     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);
@@ -192,19 +192,19 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
         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();
 
@@ -212,23 +212,23 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
     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);
@@ -239,19 +239,19 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
         //{
         //	shape->calculateLocalInertia(mass, localInertiaDiagonal);
         //}
-        
+
         btRigidBody* linkRigidBody = 0;
         btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
-        
+
         if (!createMultiBody)
         {
             btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
             linkRigidBody = body;
-            
+
             world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
-            
+
             compoundShape->setUserIndex(graphicsIndex);
-            
+
             creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
             cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
         } else
@@ -263,19 +263,19 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                 bool isFixedBase = (mass==0);//todo: figure out when base is fixed
                 int totalNumJoints = cache.m_totalNumJoints1;
                 cache.m_bulletMultiBody = creation.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)
             {
@@ -284,32 +284,32 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                     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);
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-                        
+
-                                             
+
                     } 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 = creation.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);
                     }
@@ -320,14 +320,14 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                 {
                     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);
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-                        
+
                     } else
                     {
                         //only handle principle axis at the moment,
@@ -340,10 +340,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                                 btGeneric6DofSpring2Constraint* dof6 = creation.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;
@@ -353,10 +353,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                                 btGeneric6DofSpring2Constraint* dof6 = creation.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;
@@ -367,10 +367,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                                 btGeneric6DofSpring2Constraint* dof6 = creation.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);
                             }
@@ -383,17 +383,17 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                 {
                     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);
-                        
+
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-                       btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
+                        btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
-			//world1->addMultiBodyConstraint(con);
+                        world1->addMultiBodyConstraint(con);
-			printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
+                        //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
- 
+
                     } else
                     {
                         btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
@@ -420,12 +420,12 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                                 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;
@@ -436,41 +436,41 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
 					btAssert(0);
                 }
             }
-            
+
         }
-        
+
         if (createMultiBody)
         {
             if (compoundShape->getNumChildShapes()>0)
             {
                 btMultiBodyLinkCollider* col= creation.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 
+                //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);
-               
+
                 creation.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;
@@ -481,20 +481,20 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
             }
         }
     }
-    
+
-    
+
     btAlignedObjectArray<int> urdfChildIndices;
     u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
-    
+
     int numChildren = urdfChildIndices.size();
-    
+
     for (int i=0;i<numChildren;i++)
     {
         int urdfChildLinkIndex = urdfChildIndices[i];
-        
+
         ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
     }
-    
+
 }
 
 void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
@@ -504,7 +504,7 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
     InitURDF2BulletCache(u2b,cache);
     int urdfLinkIndex = u2b.getRootLinkIndex();
     ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
-    
+
 	if (world1 && cache.m_bulletMultiBody)
 	{
 		btMultiBody* mb = cache.m_bulletMultiBody;

examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp

@@ -48,7 +48,7 @@ typedef struct
 	float m_frictionCoeff;
 } Body;
 
-__kernel void 
+__kernel void
 	copyTransformsToVBOKernel( __global Body* gBodies, __global float4* posOrnColor, const int numNodes)
 {
 	int nodeID = get_global_id(0);
@@ -76,14 +76,14 @@ m_window(0)
 	{
 		m_instancingRenderer = 0;
 	}
-	
+
 	m_window = helper->getAppInterface()->m_window;
 
 	m_data = new GpuRigidBodyDemoInternalData;
 }
 GpuRigidBodyDemo::~GpuRigidBodyDemo()
 {
-	
+
 	delete m_data;
 }
 
@@ -97,9 +97,9 @@ static void PairKeyboardCallback(int key, int state)
 {
 	if (key=='R' && state)
 	{
-		gReset = true;
+		//gReset = true;
 	}
-	
+
 	//b3DefaultKeyboardCallback(key,state);
 	oldCallback(key,state);
 }
@@ -122,13 +122,13 @@ void	GpuRigidBodyDemo::initPhysics()
 
 		cl_program rbProg=0;
 		m_data->m_copyTransformsToVBOKernel = b3OpenCLUtils::compileCLKernelFromString(m_clData->m_clContext,m_clData->m_clDevice,s_rigidBodyKernelString,"copyTransformsToVBOKernel",&errNum,rbProg);
-		
+
 		m_data->m_config.m_maxConvexBodies = b3Max(m_data->m_config.m_maxConvexBodies,gGpuArraySizeX*gGpuArraySizeY*gGpuArraySizeZ+10);
 		m_data->m_config.m_maxConvexShapes = m_data->m_config.m_maxConvexBodies;
 		int maxPairsPerBody = 16;
 		m_data->m_config.m_maxBroadphasePairs = maxPairsPerBody*m_data->m_config.m_maxConvexBodies;
 		m_data->m_config.m_maxContactCapacity = m_data->m_config.m_maxBroadphasePairs;
-		
+
 
 		b3GpuNarrowPhase* np = new b3GpuNarrowPhase(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue,m_data->m_config);
 		b3GpuBroadphaseInterface* bp =0;
@@ -155,11 +155,11 @@ void	GpuRigidBodyDemo::initPhysics()
 
 	}
 
-	
+
 
 	m_guiHelper->getRenderInterface()->writeTransforms();
-	
+
-	
+
 
 }
 
@@ -171,8 +171,8 @@ void	GpuRigidBodyDemo::exitPhysics()
 	delete m_data->m_rigidBodyPipeline;
 	delete m_data->m_broadphaseDbvt;
 
-	
+
-	
+
 	delete m_data->m_np;
 	m_data->m_np = 0;
 	delete m_data->m_bp;
@@ -185,7 +185,7 @@ void	GpuRigidBodyDemo::exitPhysics()
 void GpuRigidBodyDemo::renderScene()
 {
 	m_guiHelper->getRenderInterface()->renderScene();
-	
+
 
 }
 
@@ -216,7 +216,7 @@ void GpuRigidBodyDemo::stepSimulation(float deltaTime)
 	if (animate && numObjects)
 	{
 		B3_PROFILE("gl2cl");
-		
+
 		if (!m_data->m_instancePosOrnColor)
 		{
 			GLuint vbo = m_instancingRenderer->getInternalData()->m_vbo;
@@ -234,11 +234,11 @@ void GpuRigidBodyDemo::stepSimulation(float deltaTime)
 			assert(err==GL_NO_ERROR);
 		}
 	}
-	
+
 	{
 		B3_PROFILE("stepSimulation");
 		m_data->m_rigidBodyPipeline->stepSimulation(1./60.f);
-		
+
 	}
 
 	if (numObjects)
@@ -247,7 +247,7 @@ void GpuRigidBodyDemo::stepSimulation(float deltaTime)
 		{
 			b3GpuNarrowPhaseInternalData*	npData = m_data->m_np->getInternalData();
 			npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU);
-			
+
 			b3AlignedObjectArray<b3Vector4> vboCPU;
 			m_data->m_instancePosOrnColor->copyToHost(vboCPU);
 
@@ -301,7 +301,7 @@ b3Vector3	GpuRigidBodyDemo::getRayTo(int x,int y)
 {
 	if (!m_instancingRenderer)
 		return b3MakeVector3(0,0,0);
-		
+
 	float top = 1.f;
 	float bottom = -1.f;
 	float nearPlane = 1.f;
@@ -339,7 +339,7 @@ b3Vector3	GpuRigidBodyDemo::getRayTo(int x,int y)
 	float height = m_instancingRenderer->getScreenHeight();
 
 	aspect =  width / height;
-	
+
 	hor*=aspect;
 
 
@@ -362,7 +362,7 @@ unsigned char* GpuRigidBodyDemo::loadImage(const char* fileName, int& width, int
 
 bool	GpuRigidBodyDemo::keyboardCallback(int key, int state)
 {
-	
+
 	if (m_data)
 	{
 		if (key==B3G_ALT )
@@ -429,17 +429,17 @@ bool	GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float
 			m_data->m_rigidBodyPipeline->castRays(rays,hitResults);
 			if (hitResults[0].m_hitFraction<1.f)
 			{
-				
+
 				int hitBodyA = hitResults[0].m_hitBody;
 				if (m_data->m_np->getBodiesCpu()[hitBodyA].m_invMass)
 				{
 					//printf("hit!\n");
 					m_data->m_np->readbackAllBodiesToCpu();
 					m_data->m_pickBody = hitBodyA;
-					
 
-				
+
-			
+
+
 					//pivotInA
 					b3Vector3 pivotInB;
 					pivotInB.setInterpolate3(ray.m_from,ray.m_to,hitResults[0].m_hitFraction);
@@ -458,14 +458,14 @@ bool	GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float
 						m_data->m_pickFixedBody = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0,pos,orn,fixedSphere,0,false);
 						m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
 						m_data->m_bp->writeAabbsToGpu();
-					
+
 						if (m_data->m_pickGraphicsShapeIndex<0)
 						{
 							int strideInBytes = 9*sizeof(float);
 							int numVertices = sizeof(point_sphere_vertices)/strideInBytes;
 							int numIndices = sizeof(point_sphere_indices)/sizeof(int);
 							m_data->m_pickGraphicsShapeIndex = m_guiHelper->getRenderInterface()->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,B3_GL_POINTS);
-							
+
 							float color[4] ={1,0,0,1};
 							float scaling[4]={1,1,1,1};
 
@@ -480,7 +480,7 @@ bool	GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float
 								m_instancingRenderer->writeSingleInstanceTransformToGPU(pivotInB,orn,m_data->m_pickGraphicsShapeInstance);
 							m_data->m_np->setObjectTransformCpu(pos,orn,m_data->m_pickFixedBody);
 						}
-			
+
 					}
 					pivotInB.w = 0.f;
 					m_data->m_pickPivotInA = pivotInA;

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -3,7 +3,7 @@
 #include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
 
 
-	
+
 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
 	:m_bodyA(bodyA),
 	m_bodyB(bodyB),
@@ -13,14 +13,15 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod
 	m_jacSizeA(0),
 	m_jacSizeBoth(0),
 	m_isUnilateral(isUnilateral),
-	m_maxAppliedImpulse(100)
+	m_maxAppliedImpulse(100),
+	m_numDofsFinalized(-1)
 {
-	finalizeMultiDof();
+
 }
 
-void btMultiBodyConstraint::finalizeMultiDof()
+void btMultiBodyConstraint::updateJacobianSizes()
 {
-	if(m_bodyA)
+    if(m_bodyA)
 	{
 		if(m_bodyA->isMultiDof())
 			m_jacSizeA = (6 + m_bodyA->getNumDofs());
@@ -37,7 +38,12 @@ void btMultiBodyConstraint::finalizeMultiDof()
 	}
 	else
 		m_jacSizeBoth = m_jacSizeA;
-	
+}
+
+void btMultiBodyConstraint::allocateJacobiansMultiDof()
+{
+	updateJacobianSizes();
+
 	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
 	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
 }
@@ -48,25 +54,27 @@ btMultiBodyConstraint::~btMultiBodyConstraint()
 
 void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
-	for (int i = 0; i < ndof; ++i) 
+	for (int i = 0; i < ndof; ++i)
 		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
 }
 
-btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint, 
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint,
 															btMultiBodyJacobianData& data,
 															btScalar* jacOrgA, btScalar* jacOrgB,
 															const btVector3& contactNormalOnB,
-															const btVector3& posAworld, const btVector3& posBworld, 
+															const btVector3& posAworld, const btVector3& posBworld,
 															btScalar posError,
 															const btContactSolverInfo& infoGlobal,
 															btScalar lowerLimit, btScalar upperLimit,
 															btScalar relaxation,
 															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
+
+
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
 	solverConstraint.m_linkA = m_linkA;
-	solverConstraint.m_linkB = m_linkB;	
+	solverConstraint.m_linkB = m_linkB;
 
 	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
 	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
@@ -181,13 +189,13 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 	}
 	else if(rb1)
 	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
 		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormalOnB;
 	}
 	{
-						
+
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
@@ -233,7 +241,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
 		}
-		
+
 		//
 		btScalar d = denom0+denom1;
 		if (d>SIMD_EPSILON)
@@ -244,10 +252,10 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		{
 		//disable the constraint row to handle singularity/redundant constraint
 			solverConstraint.m_jacDiagABInv  = 0.f;
-		}		
+		}
 	}
 
-	
+
 	//compute rhs and remaining solverConstraint fields
 	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
 
@@ -260,7 +268,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		{
 			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
+			for (int i = 0; i < ndofA ; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
 		}
 		else if(rb0)
@@ -271,7 +279,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		{
 			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
+			for (int i = 0; i < ndofB ; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
 
 		}
@@ -321,11 +329,11 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
-	{	
+	{
 
 		btScalar positionalError = 0.f;
 		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
-					
+
 
 		btScalar erp = infoGlobal.m_erp2;
 		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -54,7 +54,7 @@ protected:
     int				m_posOffset;
 
 	bool			m_isUnilateral;
-
+	int				m_numDofsFinalized;
 	btScalar		m_maxAppliedImpulse;
 
 
@@ -66,11 +66,11 @@ protected:
 
 	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
 
-	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
 																btMultiBodyJacobianData& data,
 																btScalar* jacOrgA, btScalar* jacOrgB,
 																const btVector3& contactNormalOnB,
-																const btVector3& posAworld, const btVector3& posBworld, 
+																const btVector3& posAworld, const btVector3& posBworld,
 																btScalar posError,
 																const btContactSolverInfo& infoGlobal,
 																btScalar lowerLimit, btScalar upperLimit,
@@ -82,11 +82,14 @@ public:
 	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
 	virtual ~btMultiBodyConstraint();
 
-	void finalizeMultiDof();
+	void updateJacobianSizes();
+	void allocateJacobiansMultiDof();
+
+	virtual void finalizeMultiDof()=0;
 
 	virtual int getIslandIdA() const =0;
 	virtual int getIslandIdB() const =0;
-	
+
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal)=0;
@@ -108,17 +111,17 @@ public:
 	// current constraint position
     // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
     // NOTE: ignored position for friction rows.
-    btScalar getPosition(int row) const 
+    btScalar getPosition(int row) const
-	{ 
+	{
-		return m_data[m_posOffset + row]; 
+		return m_data[m_posOffset + row];
 	}
 
-    void setPosition(int row, btScalar pos) 
+    void setPosition(int row, btScalar pos)
-	{ 
+	{
-		m_data[m_posOffset + row] = pos; 
+		m_data[m_posOffset + row] = pos;
 	}
 
-	
+
 	bool isUnilateral() const
 	{
 		return m_isUnilateral;
@@ -127,21 +130,21 @@ public:
 	// jacobian blocks.
     // each of size 6 + num_links. (jacobian2 is null if no body2.)
     // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
-    btScalar* jacobianA(int row) 
+    btScalar* jacobianA(int row)
-	{ 
+	{
-		return &m_data[m_numRows + row * m_jacSizeBoth]; 
+		return &m_data[m_numRows + row * m_jacSizeBoth];
 	}
-    const btScalar* jacobianA(int row) const 
+    const btScalar* jacobianA(int row) const
-	{ 
+	{
-		return &m_data[m_numRows + (row * m_jacSizeBoth)]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth)];
 	}
-    btScalar* jacobianB(int row) 
+    btScalar* jacobianB(int row)
-	{ 
+	{
-		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
-    const btScalar* jacobianB(int row) const 
+    const btScalar* jacobianB(int row) const
-	{ 
+	{
-		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
 
 	btScalar	getMaxAppliedImpulse() const
@@ -152,7 +155,7 @@ public:
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
-	
+
 	virtual void debugDraw(class btIDebugDraw* drawer)=0;
 
 };

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -38,11 +38,11 @@ protected:
 	virtual void	calculateSimulationIslands();
 	virtual void	updateActivationState(btScalar timeStep);
 	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	
+
 public:
 
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
-	
+
 	virtual ~btMultiBodyDynamicsWorld ();
 
 	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
@@ -51,12 +51,27 @@ public:
 
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
+	virtual int     getNumMultiBodyConstraints() const
+	{
+        return m_multiBodyConstraints.size();
+	}
+
+	virtual btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex)
+	{
+        return m_multiBodyConstraints[constraintIndex];
+	}
+
+	virtual const btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex) const
+	{
+        return m_multiBodyConstraints[constraintIndex];
+	}
+
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual void	integrateTransforms(btScalar timeStep);
 
 	virtual void	debugDrawWorld();
-	
+
 	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -26,14 +26,18 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo
 	:btMultiBodyConstraint(body,body,link,link,2,true),
 	m_lowerBound(lower),
 	m_upperBound(upper)
+{
+
+}
+
+void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 {
 	// the data.m_jacobians never change, so may as well
     // initialize them here
-        
-    // note: we rely on the fact that data.m_jacobians are
-    // always initialized to zero by the Constraint ctor
 
-	unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
+	allocateJacobiansMultiDof();
+
+	unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
 
 	// row 0: the lower bound
 	jacobianA(0)[offset] = 1;
@@ -41,7 +45,10 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo
 	//jacobianA(1)[offset] = -1;
 
 	jacobianB(1)[offset] = -1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
 }
+
 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
 }
@@ -87,7 +94,13 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 {
     // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
-    
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+        finalizeMultiDof();
+	}
+
+
     // row 0: the lower bound
     setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
 
@@ -101,7 +114,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		constraintRow.m_multiBodyB = m_bodyB;
 		const btScalar posError = 0;						//why assume it's zero?
 		const btVector3 dummy(0, 0, 0);
-		
+
 		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
 		{
 			btScalar penetration = getPosition(row);
@@ -139,7 +152,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 	}
 
 }
-	
+
-	
+
-	
+
 

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -30,6 +30,8 @@ public:
 	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
 	virtual ~btMultiBodyJointLimitConstraint();
 
+	virtual void finalizeMultiDof();
+
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
@@ -41,7 +43,7 @@ public:
 	{
 		//todo(erwincoumans)
 	}
-	
+
 };
 
 #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -23,44 +23,41 @@ subject to the following restrictions:
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	:btMultiBodyConstraint(body,body,link,link,1,true),
-	m_desiredVelocity(desiredVelocity)	
+	m_desiredVelocity(desiredVelocity)
 {
-	int linkDoF = 0;
+
 
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
     // initialize them here
-        
-    // note: we rely on the fact that data.m_jacobians are
-    // always initialized to zero by the Constraint ctor
 
-    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
 
-	// row 0: the lower bound
-	// row 0: the lower bound
-    jacobianA(0)[offset] = 1;
 }
+
+void btMultiBodyJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA);
 
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
 	:btMultiBodyConstraint(body,body,link,link,1,true),
-	m_desiredVelocity(desiredVelocity)	
+	m_desiredVelocity(desiredVelocity)
 {
 	btAssert(linkDoF < body->getLink(link).m_dofCount);
 
 	m_maxAppliedImpulse = maxMotorImpulse;
-	// the data.m_jacobians never change, so may as well
-    // initialize them here
-        
-    // note: we rely on the fact that data.m_jacobians are
-    // always initialized to zero by the Constraint ctor
 
-    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
-
-	// row 0: the lower bound
-	// row 0: the lower bound
-    jacobianA(0)[offset] = 1;
 }
 btMultiBodyJointMotor::~btMultiBodyJointMotor()
 {
@@ -101,18 +98,26 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 {
     // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
-    
+
-  
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+        finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
 	const btScalar posError = 0;
 	const btVector3 dummy(0, 0, 0);
 
 	for (int row=0;row<getNumRows();row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-		
+
-		
+
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
 	}
 
 }
-	
+

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -25,7 +25,7 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint
 {
 protected:
 
-	
+
 	btScalar	m_desiredVelocity;
 
 public:
@@ -33,6 +33,7 @@ public:
 	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	virtual ~btMultiBodyJointMotor();
+    virtual void finalizeMultiDof();
 
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
@@ -40,7 +41,7 @@ public:
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
-	
+
     virtual void setVelocityTarget(btScalar velTarget)
     {
         m_desiredVelocity = velTarget;

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -43,7 +43,12 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
 	m_pivotInB(pivotInB)
 {
 }
-
+
+void btMultiBodyPoint2Point::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
 
 btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
 {

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -30,7 +30,7 @@ protected:
 	btRigidBody*	m_rigidBodyB;
 	btVector3		m_pivotInA;
 	btVector3		m_pivotInB;
-	
+
 
 public:
 
@@ -39,6 +39,8 @@ public:
 
 	virtual ~btMultiBodyPoint2Point();
 
+	virtual void finalizeMultiDof();
+
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
@@ -57,7 +59,7 @@ public:
 	}
 
 	virtual void debugDraw(class btIDebugDraw* drawer);
-	
+
 };
 
 #endif //BT_MULTIBODY_POINT2POINT_H