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

@@ -248,6 +248,10 @@ void ImportUrdfSetup::initPhysics()
 
 		//the btMultiBody support is work-in-progress :-)
 
+        for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
+        {
+            m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
+        }
 
 
 

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -391,8 +391,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
 
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
                         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
                     {

examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp

@@ -97,7 +97,7 @@ static void PairKeyboardCallback(int key, int state)
 {
 	if (key=='R' && state)
 	{
-		gReset = true;
+		//gReset = true;
 	}
 
 	//b3DefaultKeyboardCallback(key,state);

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -13,12 +13,13 @@ 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)
 	{
@@ -37,6 +38,11 @@ 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);
@@ -63,6 +69,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 															btScalar relaxation,
 															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
+
+
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
 	solverConstraint.m_linkA = m_linkA;

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -54,7 +54,7 @@ protected:
     int				m_posOffset;
 
 	bool			m_isUnilateral;
-
+	int				m_numDofsFinalized;
 	btScalar		m_maxAppliedImpulse;
 
 
@@ -82,7 +82,10 @@ 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;

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -51,6 +51,21 @@ 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);

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -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
+	allocateJacobiansMultiDof();
-    // always initialized to zero by the Constraint ctor
 
-	unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
+	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()
 {
 }
@@ -88,6 +95,12 @@ 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
 

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -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;
 

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -25,22 +25,29 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
 	:btMultiBodyConstraint(body,body,link,link,1,true),
 	m_desiredVelocity(desiredVelocity)
 {
-	int linkDoF = 0;
+
 
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
     // initialize them here
 
+
+}
+
+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 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
+	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),
@@ -50,17 +57,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
 	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()
 {
@@ -102,6 +99,14 @@ 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);

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -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;

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -44,6 +44,11 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
 {
 }
 
+void btMultiBodyPoint2Point::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
 
 btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
 {

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -39,6 +39,8 @@ public:
 
 	virtual ~btMultiBodyPoint2Point();
 
+	virtual void finalizeMultiDof();
+
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;