Merge branch 'feat-contact-sensor' into ambf-2.0

CHANGELOG.md

@@ -0,0 +1,10 @@
+## July 8 2024
+1. Can set ERP and CFM for 6 DOF springs and non spring joints
+2. Added flag to indicate the ERP and CFM has been set in the ADF file
+3. Updating Ghost objects pose based on its parent's inertial pose for better synchronization.
+
+## June 26 2024
+1. Removed setting forces and torques in the Ghost Object update
+2. Added ROS Communication Support for Ghost Objects
+3. Create Contact Sensors and implemented their ROS communication
+4. Created CHANGELOG file

adf_loader/version_1_0/adf_loader_1_0.cpp

@@ -475,6 +475,9 @@ afSensorType ADFUtils::getSensorTypeFromString(const string &a_str)
     else if (a_str.compare("Resistance") == 0 || a_str.compare("resistance") == 0 || a_str.compare("RESISTANCE") == 0){
         type = afSensorType::RESISTANCE;
     }
+    else if (a_str.compare("Contact") == 0 || a_str.compare("contact") == 0 || a_str.compare("CONTACT") == 0){
+        type = afSensorType::CONTACT;
+    }
 
     return type;
 }
@@ -1924,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>();
     }
 
@@ -1982,6 +1987,9 @@ bool ADFLoader_1_0::loadSensorAttribs(YAML::Node *a_node, afSensorAttributes *at
     }
     case afSensorType::RESISTANCE:{
         return loadResistanceSensorAttribs(a_node, (afResistanceSensorAttributes*)attribs);
+    }
+    case afSensorType::CONTACT:{
+        return loadContactSensorAttribs(a_node, (afContactSensorAttributes*)attribs);
     }
         break;
     default:{
@@ -2117,7 +2125,7 @@ bool ADFLoader_1_0::loadResistanceSensorAttribs(YAML::Node *a_node, afResistance
 {
     YAML::Node& node = *a_node;
     if (node.IsNull()){
-        cerr << "ERROR! ACTUATOR'S YAML CONFIG DATA IS NULL\n";
+        cerr << "ERROR! SENSOR'S YAML CONFIG DATA IS NULL\n";
         return 0;
     }
     ADFUtils::saveRawData(a_node, attribs);
@@ -2163,6 +2171,51 @@ bool ADFLoader_1_0::loadResistanceSensorAttribs(YAML::Node *a_node, afResistance
         return result;
 }
 
+bool ADFLoader_1_0::loadContactSensorAttribs(YAML::Node *a_node, afContactSensorAttributes *attribs)
+{
+    YAML::Node& node = *a_node;
+    if (node.IsNull()){
+        cerr << "ERROR! SENSOR'S YAML CONFIG DATA IS NULL\n";
+        return 0;
+    }
+    ADFUtils::saveRawData(a_node, attribs);
+
+    bool result = true;
+
+    YAML::Node nameNode = node["name"];
+    YAML::Node namespaceNode = node["namespace"];
+    YAML::Node parentNameNode = node["parent"];
+    YAML::Node visibleNode = node["visible"];
+    YAML::Node visibleSizeNode = node["visible size"];
+    YAML::Node publishFrequencyNode = node["publish frequency"];
+    YAML::Node distanceThresholdNode = node["distance threshold"];
+    YAML::Node processContactDetailsNode = node["process contact details"];
+
+    ADFUtils::getIdentificationAttribsFromNode(a_node, &attribs->m_identificationAttribs);
+    ADFUtils::getHierarchyAttribsFromNode(a_node, &attribs->m_hierarchyAttribs);
+    ADFUtils::getCommunicationAttribsFromNode(a_node, &attribs->m_communicationAttribs);
+    ADFUtils::getPluginAttribsFromNode(a_node, &attribs->m_pluginAttribs);
+
+
+    if (visibleNode.IsDefined()){
+        attribs->m_visible = visibleNode.as<bool>();
+    }
+
+    if (visibleSizeNode.IsDefined()){
+        attribs->m_visibleSize = visibleSizeNode.as<double>();
+    }
+
+    if (distanceThresholdNode.IsDefined()){
+        attribs->m_distanceThreshold = distanceThresholdNode.as<double>();
+    }
+
+    if (processContactDetailsNode.IsDefined()){
+        attribs->m_processContactDetails = processContactDetailsNode.as<bool>();
+    }
+
+    return result;
+}
+
 bool ADFLoader_1_0::loadActuatorAttribs(YAML::Node *a_node, afActuatorAttributes *attribs)
 {
     YAML::Node& node = *a_node;
@@ -2729,6 +2782,10 @@ bool ADFLoader_1_0::loadModelAttribs(YAML::Node *a_node, afModelAttributes *attr
                 senAttribs = new afResistanceSensorAttributes();
                 break;
             }
+            case afSensorType::CONTACT:{
+                senAttribs = new afContactSensorAttributes();
+                break;
+            }
             default:
                 break;
             }

adf_loader/version_1_0/adf_loader_1_0.h

@@ -151,6 +151,9 @@ class ADFLoader_1_0: public ADFLoaderBase{
     // Load joint from a YAML::Node
     virtual bool loadResistanceSensorAttribs(YAML::Node* a_node, afResistanceSensorAttributes* attribs);
 
+    // Load joint from a YAML::Node
+    virtual bool loadContactSensorAttribs(YAML::Node* a_node, afContactSensorAttributes* attribs);
+
     // Load actuator from a YAML::Node
     virtual bool loadActuatorAttribs(YAML::Node* a_node, afActuatorAttributes* attribs);
 

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
@@ -1104,6 +1108,22 @@ struct afResistanceSensorAttributes: public afRayTracerSensorAttributes{
 };
 
 
+struct afContactSensorAttributes: public afSensorAttributes{
+public:
+    afContactSensorAttributes(){
+        m_distanceThreshold = 0.0;
+        m_processContactDetails = true;
+        m_visible = false;
+        m_visibleSize = 10;
+    }
+
+    double m_distanceThreshold; // Distance threshold between objects for contact to count
+    bool m_processContactDetails; // If true, process contact ponits, normals, etc. Otherwise just names of objects in contact
+    bool m_visible;
+    double m_visibleSize;
+};
+
+
 struct afFileObjectAttributes{
 public:
     afFileObjectAttributes(){}

ambf_framework/afEnums.h

@@ -174,7 +174,8 @@ enum class afSensorType{
     RAYTRACER = 0,
     RANGE = 1,
     RESISTANCE = 2,
-    INVALID = 3
+    CONTACT = 3,
+    INVALID = 4
 };