Set contact points based on prox measurements.

isri-aist · Aug 16, 2024 · c810c8c · c810c8c
1 parent edb5136
commit c810c8c
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git a/include/McRtcTactileSensorPlugin/TactileSensorPlugin.h b/include/McRtcTactileSensorPlugin/TactileSensorPlugin.h
@@ -49,6 +49,9 @@ struct TactileSensorPlugin : public mc_control::GlobalPlugin
 
     //! Force threshold for determining contact in each cell [N]
     double contactForceThre = 3.0;
+
+    //! Proximity threshold for determining contact in each cell
+    double contactProxThre = 1.0;
   };
 
   /** \brief Sensor data. */

diff --git a/src/TactileSensorPlugin.cpp b/src/TactileSensorPlugin.cpp
@@ -116,6 +116,7 @@ void TactileSensorPlugin::init(mc_control::MCGlobalController & gc, const mc_rtc
 
     sensorConfig("forceScale", sensorInfo.forceScale);
     sensorConfig("contactForceThre", sensorInfo.contactForceThre);
+    sensorConfig("contactProxThre", sensorInfo.contactProxThre);
 
     sensorInfoList_.push_back(sensorInfo);
   }
@@ -266,7 +267,7 @@ void TactileSensorPlugin::eskinSensorCallback(const eskin_ros_utils::PatchData::
     sensorData->wrench.force() += force;
     sensorData->wrench.moment() += moment;
 
-    if(force.z() > sensorInfo.contactForceThre)
+    if(force.z() > sensorInfo.contactForceThre || cellMsg.prox > sensorInfo.contactProxThre)
     {
       contactPoints.push_back(position);
     }