Merge pull request #4583 from iche033/iche033/joint_force_torque

Fix joint force/torque feedback
bulletphysics · Apr 9, 2024 · 5e89410 · 5e89410
2 parents 6bb8d11 + d7f9d66
commit 5e89410
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -984,7 +984,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			//
 			hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
 			//
-			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
+			btScalar jointTorque = 0;
+			if (isConstraintPass) jointTorque = 0;
+			else jointTorque = m_links[i].m_jointTorque[dof];
+			Y[m_links[i].m_dofOffset + dof] = jointTorque - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
+
 		}
 		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{