First step in btMultiBody joint force/torque feedback. There is still some work to be done for 'mobilizer limit/motor'.

See examples/MultiBody/TestJointTorqueSetup and examples/Constraints/TestHingeTorque for joint feedback.

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -33,10 +33,10 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
 
 
 }
-
-void btMultiBodyJointMotor::finalizeMultiDof()
-{
-	allocateJacobiansMultiDof();
+
+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;
@@ -44,9 +44,9 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 
 	// row 0: the lower bound
 	// row 0: the lower bound
-	jacobianA(0)[offset] = 1;
-
-	m_numDofsFinalized = m_jacSizeBoth;
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
 }
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
@@ -98,14 +98,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)
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+        finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
 		return;
 
 	const btScalar posError = 0;
@@ -117,6 +117,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 
 
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+		constraintRow.m_useJointForce = true;
 	}
 
 }