Save convex vertices in datastore.

include/McRtcTactileSensorPlugin/TactileSensorPlugin.h

@@ -46,13 +46,19 @@ struct TactileSensorPlugin : public mc_control::GlobalPlugin
 
     //! Force scale (this value is multiplied to the tactile measurement value)
     double forceScale = 1.0;
+
+    //! Force threshold for determining contact in each cell [N]
+    double contactForceThre = 3.0;
   };
 
   /** \brief Sensor data. */
   struct SensorData
   {
     //! Wrench (represented in the frame of tactile sensor)
     sva::ForceVecd wrench = sva::ForceVecd::Zero();
+
+    //! Vertices of the convex hull of the contact region (represented in the frame of tactile sensor)
+    std::vector<Eigen::Vector2d> convexVertices;
   };
 
 public:
@@ -72,6 +78,10 @@ struct TactileSensorPlugin : public mc_control::GlobalPlugin
   inline void after(mc_control::MCGlobalController & gc) override{};
 
 protected:
+  /** \brief Set datastore entry. */
+  template<class ValueType>
+  void setDatastore(mc_rtc::DataStore & datastore, const std::string & key, const ValueType & value);
+
 #if ENABLE_MUJOCO
   /** \brief ROS callback of tactile sensor topic from MuJoCo.
       \param sensorMsg sensor message

src/TactileSensorPlugin.cpp

@@ -1,9 +1,28 @@
 #include <mc_control/GlobalPluginMacros.h>
+#include <mc_rtc/DataStore.h>
 
 #include <McRtcTactileSensorPlugin/TactileSensorPlugin.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <functional>
 
+namespace
+{
+std::vector<Eigen::Vector2d> getRectVertices(const std::vector<Eigen::Vector3d> & points)
+{
+  Eigen::Vector2d vertexMin = Eigen::Vector2d::Constant(std::numeric_limits<double>::max());
+  Eigen::Vector2d vertexMax = Eigen::Vector2d::Constant(std::numeric_limits<double>::lowest());
+
+  for(const auto & point : points)
+  {
+    vertexMin = vertexMin.cwiseMin(point.head<2>());
+    vertexMax = vertexMax.cwiseMax(point.head<2>());
+  }
+
+  return {vertexMax, Eigen::Vector2d(vertexMin.x(), vertexMax.y()), vertexMin,
+          Eigen::Vector2d(vertexMax.x(), vertexMin.y())};
+}
+} // namespace
+
 namespace mc_plugin
 {
 
@@ -153,12 +172,36 @@ void TactileSensorPlugin::before(mc_control::MCGlobalController & gc)
       continue;
     }
 
-    // Transform wrench from tactile sensor frame to force sensor frame
+    // Transform from tactile sensor frame to force sensor frame
     sva::PTransformd tactileSensorPose = robot.frame(sensorInfo.tactileSensorFrameName).position();
     sva::PTransformd forceSensorPose = forceSensor.X_fsmodel_fsactual() * forceSensor.X_0_f(robot);
     sva::PTransformd tactileToForceTrans = forceSensorPose * tactileSensorPose.inv();
+
     sva::ForceVecd wrenchInForceSensorFrame = tactileToForceTrans.dualMul(sensorData->wrench);
     forceSensor.wrench(wrenchInForceSensorFrame);
+
+    std::vector<Eigen::Vector2d> convexVerticesInForceSensorFrame;
+    for(const auto & vertex : sensorData->convexVertices)
+    {
+      sva::PTransformd vertexPose =
+          sva::PTransformd(Eigen::Matrix3d::Identity(), Eigen::Vector3d(vertex.x(), vertex.y(), 0.0));
+      convexVerticesInForceSensorFrame.push_back((vertexPose * tactileToForceTrans.inv()).translation().head<2>());
+    }
+    setDatastore<std::vector<Eigen::Vector2d>>(controller.datastore(), sensorInfo.forceSensorName + "::convexVertices",
+                                               convexVerticesInForceSensorFrame);
+  }
+}
+
+template<class ValueType>
+void TactileSensorPlugin::setDatastore(mc_rtc::DataStore & datastore, const std::string & key, const ValueType & value)
+{
+  if(datastore.has(key))
+  {
+    datastore.assign<ValueType>(key, value);
+  }
+  else
+  {
+    datastore.make<ValueType>(key, value);
   }
 }
 
@@ -171,6 +214,7 @@ void TactileSensorPlugin::mujocoSensorCallback(
   auto sensorData = std::make_shared<SensorData>();
 
   // Calculate wrench by integrating tactile sensor measurements
+  std::vector<Eigen::Vector3d> contactPoints;
   for(size_t i = 0; i < sensorMsg->forces.size(); i++)
   {
     Eigen::Vector3d position;
@@ -181,8 +225,15 @@ void TactileSensorPlugin::mujocoSensorCallback(
     Eigen::Vector3d moment = position.cross(force);
     sensorData->wrench.force() += force;
     sensorData->wrench.moment() += moment;
+
+    if(force.z() > sensorInfo.contactForceThre)
+    {
+      contactPoints.push_back(position);
+    }
   }
 
+  sensorData->convexVertices = getRectVertices(contactPoints);
+
   sensorDataList_[sensorIdx] = sensorData;
 }
 #endif
@@ -194,6 +245,7 @@ void TactileSensorPlugin::eskinSensorCallback(const eskin_ros_utils::PatchData::
   auto sensorData = std::make_shared<SensorData>();
 
   // Calculate wrench by integrating tactile sensor measurements
+  std::vector<Eigen::Vector3d> contactPoints;
   for(const auto & cellMsg : sensorMsg->cells)
   {
     Eigen::Vector3d position;
@@ -207,8 +259,15 @@ void TactileSensorPlugin::eskinSensorCallback(const eskin_ros_utils::PatchData::
     Eigen::Vector3d moment = position.cross(force);
     sensorData->wrench.force() += force;
     sensorData->wrench.moment() += moment;
+
+    if(force.z() > sensorInfo.contactForceThre)
+    {
+      contactPoints.push_back(position);
+    }
   }
 
+  sensorData->convexVertices = getRectVertices(contactPoints);
+
   sensorDataList_[sensorIdx] = sensorData;
 }
 #endif