apply newForceTorque.diff patch: it will allow to report

joint reaction force/torque, while using impulse-based response
for btMultiBody

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -357,7 +357,8 @@ void btMultiBody::setupPlanar(int i,
 void btMultiBody::finalizeMultiDof()
 {
 	btAssert(m_isMultiDof);
-
+	m_deltaV.resize(0);
+	m_deltaV.resize(6 + m_dofCount);
 	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
 	m_vectorBuf.resize(2 * m_dofCount);													//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
 
@@ -543,7 +544,17 @@ btVector3 btMultiBody::getAngularMomentum() const
     return result;
 }
 
+void btMultiBody::clearConstraintForces()
+{
+	m_baseConstraintForce.setValue(0, 0, 0);
+	m_baseConstraintTorque.setValue(0, 0, 0);
 
+
+    for (int i = 0; i < getNumLinks(); ++i) {
+        m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
+        m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
+    }
+}
 void btMultiBody::clearForcesAndTorques()
 {
     m_baseForce.setValue(0, 0, 0);
@@ -574,6 +585,18 @@ void btMultiBody::addLinkTorque(int i, const btVector3 &t)
     m_links[i].m_appliedTorque += t;
 }
 
+void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
+{
+    m_links[i].m_appliedConstraintForce += f;
+}
+
+void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
+{
+    m_links[i].m_appliedConstraintTorque += t;
+}
+
+
+
 void btMultiBody::addJointTorque(int i, btScalar Q)
 {
     m_links[i].m_jointTorque[0] += Q;
@@ -637,7 +660,8 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1)				//r
 void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
                                btAlignedObjectArray<btScalar> &scratch_r,
                                btAlignedObjectArray<btVector3> &scratch_v,
-                               btAlignedObjectArray<btMatrix3x3> &scratch_m)
+                               btAlignedObjectArray<btMatrix3x3> &scratch_m,
+				bool isConstraintPass)
 {
     // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
     // and the base linear & angular accelerations.
@@ -734,8 +758,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
     }
 	else 
 	{
+		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
+		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
 		//external forces		
-		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));	
+		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
 
 		//adding damping terms (only)
 		btScalar linDampMult = 1., angDampMult = 1.;
@@ -804,7 +830,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
         // calculate zhat_i^A
 		//
 		//external forces		
-		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
+		btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
+		btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
+ 
+		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
 		
 		if (0)
 		{
@@ -1043,12 +1072,28 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 
 			if (gJointFeedbackInWorldSpace)
 			{
-				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
+				if (isConstraintPass)
-				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
+				{
+ m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
+                                        m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
+				} else
+				{
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
+				}
 			} else
 			{
+				if (isConstraintPass)
+				{
+					  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;                        
+                                m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
+
+				}
+				else
+				{
 				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
 				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
+				}		
 			}	
 	}
 
@@ -1084,9 +1129,13 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 	/////////////////
 
     // Final step: add the accelerations (times dt) to the velocities.
+
+	if (!isConstraintPass)
+	{
 	if(dt > 0.)
 		applyDeltaVeeMultiDof(output, dt);
 
+	}
 	/////
 	//btScalar angularThres = 1;
 	//btScalar maxAngVel = 0.;		

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -273,6 +273,8 @@ public:
     //
 
     void clearForcesAndTorques();
+   void clearConstraintForces();
+
 	void clearVelocities();
 
     void addBaseForce(const btVector3 &f) 
@@ -282,7 +284,17 @@ public:
     void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
     void addLinkForce(int i, const btVector3 &f);
     void addLinkTorque(int i, const btVector3 &t);
-    void addJointTorque(int i, btScalar Q);
+
+ void addBaseConstraintForce(const btVector3 &f)
+        {
+                m_baseConstraintForce += f;
+        }
+    void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
+    void addLinkConstraintForce(int i, const btVector3 &f);
+    void addLinkConstraintTorque(int i, const btVector3 &t);
+       
+
+void addJointTorque(int i, btScalar Q);
 	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
 	void addJointTorqueMultiDof(int i, const btScalar *Q);
 
@@ -318,7 +330,9 @@ public:
 	void stepVelocitiesMultiDof(btScalar dt,
                         btAlignedObjectArray<btScalar> &scratch_r,
                         btAlignedObjectArray<btVector3> &scratch_v,
-                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
+			bool isConstraintPass=false
+		);
 
     // calcAccelerationDeltas
     // input: force vector (in same format as jacobian, i.e.:
@@ -374,6 +388,23 @@ public:
 		}
     }
 
+	void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
+	{
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+                {
+                        m_deltaV[dof] += delta_vee[dof] * multiplier;
+                }
+	}
+	void processDeltaVeeMultiDof2()
+	{
+		applyDeltaVeeMultiDof(&m_deltaV[0],1);
+
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+                {
+			m_deltaV[dof] = 0.f;
+		}
+	}
+
 	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 
 	{
 		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
@@ -591,7 +622,10 @@ private:
 
     btVector3 m_baseForce;     // external force applied to base. World frame.
     btVector3 m_baseTorque;    // external torque applied to base. World frame.
-    
+   
+    btVector3 m_baseConstraintForce;     // external force applied to base. World frame.
+    btVector3 m_baseConstraintTorque;    // external torque applied to base. World frame.
+ 
     btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
 	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
 
@@ -611,7 +645,7 @@ private:
     //  offset         size         array
     //   0              num_links+1  rot_from_parent
     //
-    
+   btAlignedObjectArray<btScalar> m_deltaV; 
     btAlignedObjectArray<btScalar> m_realBuf;
     btAlignedObjectArray<btVector3> m_vectorBuf;
     btAlignedObjectArray<btMatrix3x3> m_matrixBuf;

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -170,7 +170,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 		if(c.m_multiBodyA->isMultiDof())
-			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+			c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
 		else
 			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
@@ -186,7 +186,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 		if(c.m_multiBodyB->isMultiDof())
-			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+			c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
 		else
 			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
@@ -887,7 +887,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
 #include "Bullet3Common/b3Logging.h"
 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
 {
-#if 1
+#if 1 
 	
 	//bod->addBaseForce(m_gravity * bod->getBaseMass());
 	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
@@ -898,78 +898,49 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 		
 		if(c.m_multiBodyA->isMultiDof())
 		{
-			if (c.m_useJointForce)
+			c.m_multiBodyA->setCompanionId(-1);
+			btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
+			btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
+			if (c.m_linkA<0)
 			{
-				c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
+				c.m_multiBodyA->addBaseConstraintForce(force);
-				//c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
+				c.m_multiBodyA->addBaseConstraintTorque(torque);
 			} else
-			//if (c.m_multiBodyA->getCompanionId()>=0)
 			{
-				c.m_multiBodyA->setCompanionId(-1);
+				c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
-				btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
+					//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-				btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
+				c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
-				if (c.m_linkA<0)
-				{
-					c.m_multiBodyA->addBaseForce(force);
-					c.m_multiBodyA->addBaseTorque(torque);
-				} else
-				{
-					if (c.m_useJointForce)
-					{
-						c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
-					} else
-					{
-						c.m_multiBodyA->addLinkForce(c.m_linkA,force);
-						//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-						c.m_multiBodyA->addLinkTorque(c.m_linkA,torque);
-					}
-				}
-				//c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
 			}
 		}
-		else
-		{
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
 	}
 	
 	if (c.m_multiBodyB)
 	{
 		if(c.m_multiBodyB->isMultiDof())
 		{
-			if (c.m_useJointForce)
-			{
-				c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-			} else
-
-			//if (c.m_multiBodyB->getCompanionId()>=0)
 			{
 				c.m_multiBodyB->setCompanionId(-1);
 				btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
 				btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
 				if (c.m_linkB<0)
 				{
-					c.m_multiBodyB->addBaseForce(force);
+					c.m_multiBodyB->addBaseConstraintForce(force);
-					c.m_multiBodyB->addBaseTorque(torque);
+					c.m_multiBodyB->addBaseConstraintTorque(torque);
 				} else
 				{
-					if (!c.m_useJointForce)
 					{
-						c.m_multiBodyB->addLinkForce(c.m_linkB,force);
+						c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
 						//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-						c.m_multiBodyB->addLinkTorque(c.m_linkB,torque);
+						c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
 					}
 					
 				}
-				//c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
 			}
 		}
-		else
-		{
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
 	}
-#else
+#endif
+
+#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 
 	if (c.m_multiBodyA)
 	{
@@ -1006,7 +977,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 	BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
 	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
 	
-#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 
 	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
 	//or as applied force, so we can measure the joint reaction forces easier
@@ -1029,7 +999,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
 		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
 	}
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 
 	
 	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -441,7 +441,7 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
 
-
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	{
 		BT_PROFILE("btMultiBody addForce");
 		for (int i=0;i<this->m_multiBodies.size();i++)
@@ -476,7 +476,7 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			}//if (!isSleeping)
 		}
 	}
-
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	
 
 	{
@@ -671,16 +671,61 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				{
 					bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
 				}
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 				bod->clearForcesAndTorques();
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 			}//if (!isSleeping)
 		}
 	}
 
+	clearMultiBodyConstraintForces();
+
 	m_solverMultiBodyIslandCallback->processConstraints();
 	
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 
+	{
+                BT_PROFILE("btMultiBody stepVelocities");
+                for (int i=0;i<this->m_multiBodies.size();i++)
+                {
+                        btMultiBody* bod = m_multiBodies[i];
 
+                        bool isSleeping = false;
+
+                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                        {
+                                isSleeping = true;
+                        }
+                        for (int b=0;b<bod->getNumLinks();b++)
+                        {
+                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+                                        isSleeping = true;
+                        }
+
+                        if (!isSleeping)
+                        {
+                                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                                scratch_r.resize(bod->getNumLinks()+1);                 //multidof? ("Y"s use it and it is used to store qdd)
+                                scratch_v.resize(bod->getNumLinks()+1);
+                                scratch_m.resize(bod->getNumLinks()+1);
+
+                                if(bod->isMultiDof())
+                                {
+                                        if(!bod->isUsingRK4Integration())
+                                        {
+						bool isConstraintPass = true;
+                                                bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
+                                        }
+				}
+			}
+		}
+	}
+
+	for (int i=0;i<this->m_multiBodies.size();i++)
+       {
+                btMultiBody* bod = m_multiBodies[i];
+		bod->processDeltaVeeMultiDof2();
+	}
 
 }
 
@@ -882,3 +927,87 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 
 	btDiscreteDynamicsWorld::debugDrawWorld();
 }
+
+
+
+void btMultiBodyDynamicsWorld::applyGravity()
+{
+        btDiscreteDynamicsWorld::applyGravity();
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+        BT_PROFILE("btMultiBody addGravity");
+        for (int i=0;i<this->m_multiBodies.size();i++)
+        {
+                btMultiBody* bod = m_multiBodies[i];
+
+                bool isSleeping = false;
+
+                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                {
+                        isSleeping = true;
+                }
+                for (int b=0;b<bod->getNumLinks();b++)
+                {
+                        if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+                                isSleeping = true;
+                }
+
+                if (!isSleeping)
+                {
+                        bod->addBaseForce(m_gravity * bod->getBaseMass());
+
+                        for (int j = 0; j < bod->getNumLinks(); ++j)
+                        {
+                                bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                        }
+                }//if (!isSleeping)
+        }
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+}
+
+void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
+{ 
+  for (int i=0;i<this->m_multiBodies.size();i++)
+                {       
+                        btMultiBody* bod = m_multiBodies[i];
+			bod->clearConstraintForces();
+                  } 
+}
+void btMultiBodyDynamicsWorld::clearMultiBodyForces()
+{
+              {
+                BT_PROFILE("clearMultiBodyForces");
+                for (int i=0;i<this->m_multiBodies.size();i++)
+                {
+                        btMultiBody* bod = m_multiBodies[i];
+
+                        bool isSleeping = false;
+
+                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                        {       
+                                isSleeping = true;
+                        }
+                        for (int b=0;b<bod->getNumLinks();b++)
+                        {       
+                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)     
+                                        isSleeping = true;
+                        }
+
+                        if (!isSleeping)
+                        {
+                        btMultiBody* bod = m_multiBodies[i];
+                        bod->clearForcesAndTorques();
+                	}
+		}
+	}
+
+}
+void btMultiBodyDynamicsWorld::clearForces()
+{
+        btDiscreteDynamicsWorld::clearForces();
+
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+	clearMultiBodyForces();
+#endif
+}
+
+

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
 
+#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 
 class btMultiBody;
 class btMultiBodyConstraint;
@@ -85,6 +86,10 @@ public:
 	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 	
 	void	forwardKinematics();
+	virtual void clearForces();
+	virtual void clearMultiBodyConstraintForces();
+	virtual void clearMultiBodyForces();
+	virtual void applyGravity();
 	
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -106,6 +106,9 @@ struct btMultibodyLink
     btVector3 m_appliedForce;    // In WORLD frame
     btVector3 m_appliedTorque;   // In WORLD frame
 
+btVector3 m_appliedConstraintForce;    // In WORLD frame
+    btVector3 m_appliedConstraintTorque;   // In WORLD frame
+
 	btScalar m_jointPos[7];
     
     //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.