diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp | 21 |
1 files changed, 15 insertions, 6 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp index 5c20d2a0d4..00a7ef3579 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -26,10 +26,13 @@ btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), m_desiredVelocity(0, 0, 0), m_desiredPosition(0,0,0,1), - m_kd(1.), - m_kp(0.2), + m_use_multi_dof_params(false), + m_kd(1., 1., 1.), + m_kp(0.2, 0.2, 0.2), m_erp(1), - m_rhsClamp(SIMD_INFINITY) + m_rhsClamp(SIMD_INFINITY), + m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse), + m_damping(1.0, 1.0, 1.0) { m_maxAppliedImpulse = maxMotorImpulse; @@ -45,6 +48,7 @@ void btMultiBodySphericalJointMotor::finalizeMultiDof() unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); // row 0: the lower bound + // row 0: the lower bound jacobianA(0)[offset] = 1; m_numDofsFinalized = m_jacSizeBoth; @@ -138,7 +142,8 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar desiredVelocity = this->m_desiredVelocity[row]; - btScalar velocityError = desiredVelocity - currentVelocity; + double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0]; + btScalar velocityError = (desiredVelocity - currentVelocity) * kd; btMatrix3x3 frameAworld; frameAworld.setIdentity(); @@ -151,12 +156,16 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; case btMultibodyLink::eSpherical: { btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); - posError = m_kp*angleDiff[row % 3]; + double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0]; + posError = kp*angleDiff[row % 3]; + double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse; fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, btVector3(0,0,0), dummy, dummy, posError, infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + -max_applied_impulse, max_applied_impulse, true, + 1.0, false, 0, 0, + m_damping[row % 3]); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; break; |