From d7f9d662076ed8c7cc2a62720ffbc7aea574cf3e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 22 Feb 2024 23:59:39 +0000 Subject: [PATCH] apply joint forces patch Signed-off-by: Ian Chen --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 1bc2ffe22..579b7eab4 100644 --- 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) {