From 32a7b380aa8d0c6a03aef7206fdbc42b15f53ae2 Mon Sep 17 00:00:00 2001
From: Adnan Munawar <adnan.munawar@live.com>
Date: Mon, 8 Jul 2024 23:25:40 +0500
Subject: [PATCH] Adding ERP and CFM override for 6 DOF joints. Adding a flag
 to indicate override

---
 adf_loader/version_1_0/adf_loader_1_0.cpp | 2 ++
 ambf_framework/afAttributes.h             | 4 ++++
 ambf_framework/afFramework.cpp            | 5 +++++
 3 files changed, 11 insertions(+)

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<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
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;