Adding ERP and CFM override for 6 DOF joints. Adding a flag to indica…

…te override
WPI-AIM · Jul 8, 2024 · 32a7b38 · 32a7b38
1 parent fd5520b
commit 32a7b38
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 0 deletions.
diff --git 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<double>();
     }
 
     if(cfmNode.IsDefined()){
+        attribs->m_override_cfm = true;
         attribs->m_cfm = cfmNode.as<double>();
     }
 

diff --git 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
@@ -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;