diff --git a/adf_loader/version_1_0/adf_loader_1_0.cpp b/adf_loader/version_1_0/adf_loader_1_0.cpp index cce63690..f089bf1f 100644 --- a/adf_loader/version_1_0/adf_loader_1_0.cpp +++ b/adf_loader/version_1_0/adf_loader_1_0.cpp @@ -1927,10 +1927,12 @@ bool ADFLoader_1_0::loadJointAttribs(YAML::Node *a_node, afJointAttributes *attr } if(erpNode.IsDefined()){ + attribs->m_override_erp = true; attribs->m_erp = erpNode.as(); } if(cfmNode.IsDefined()){ + attribs->m_override_cfm = true; attribs->m_cfm = cfmNode.as(); } diff --git a/ambf_framework/afAttributes.h b/ambf_framework/afAttributes.h index a9f1fab8..6bfd9673 100644 --- a/ambf_framework/afAttributes.h +++ b/ambf_framework/afAttributes.h @@ -664,7 +664,9 @@ struct afJointAttributes: public afBaseObjectAttributes m_equilibriumPoint = 0.0; m_ignoreInterCollision = true; m_erp = 0.1; + m_override_erp = false; m_cfm = 0.1; + m_override_cfm = false; } struct afConeTwistLimits{ @@ -709,7 +711,9 @@ struct afJointAttributes: public afBaseObjectAttributes afSixDofLimits m_sixDofLimits; afSixDofSpringAttribs m_sixDofSpringAttribs; double m_erp; + bool m_override_erp; double m_cfm; + bool m_override_cfm; // Rotational offset of joint along the free joint axis double m_jointOffset; // Rotation offset of child along the free joint axis diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index 4b257bc0..7bd2a7c5 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -3484,6 +3484,9 @@ bool afJoint::createFromAttribs(afJointAttributes *a_attribs) // The rotational limits are inverted in Bullet m_sixDof->setLimit(id, -a_attribs->m_sixDofLimits.m_upperLimit[id], -a_attribs->m_sixDofLimits.m_lowerLimit[id]); } + for (int id = 0 ; id < 6 ; id++){ + if (a_attribs->m_override_cfm) m_sixDof->setParam(BT_CONSTRAINT_CFM, a_attribs->m_cfm, id); + } m_btConstraint = m_sixDof; @@ -3504,6 +3507,8 @@ bool afJoint::createFromAttribs(afJointAttributes *a_attribs) m_sixDofSpring->setDamping(id, a_attribs->m_sixDofSpringAttribs.m_damping[id]); m_sixDofSpring->setStiffness(id, a_attribs->m_sixDofSpringAttribs.m_stiffness[id]); m_sixDofSpring->enableSpring(id, true); + if (a_attribs->m_override_erp) m_sixDofSpring->setParam(BT_CONSTRAINT_ERP, a_attribs->m_erp, id); + if (a_attribs->m_override_cfm) m_sixDofSpring->setParam(BT_CONSTRAINT_CFM, a_attribs->m_cfm, id); } m_btConstraint = m_sixDofSpring;