Tactile clean

include/cnoid/BasicSensors

@@ -1,3 +1,4 @@
 #include "src/Body/ForceSensor.h"
+#include "src/Body/TactileSensor.h"
 #include "src/Body/RateGyroSensor.h"
 #include "src/Body/AccelerationSensor.h"

sample/ContactForceExtraction/ContactForceExtractionSamplePlugin.cpp

@@ -116,16 +116,16 @@ void ContactForceExtractorItem::extractBodyContactPoints(DyBody* body, ostream&
         DyLink::ConstraintForceArray& forces = link->constraintForces();
         if(!forces.empty()){
             if(!put){
-                os << body->name() << ":\n";
+	        // os << body->name() << ":\n";  // Rafa commented this
                 put = true;
             }
-            os << " " << link->name() << ":\n";
+            // os << " " << link->name() << ":\n";  // Rafa commented this
             for(size_t i=0; i < forces.size(); ++i){
                 const DyLink::ConstraintForce& force = forces[i];
                 const Vector3& p = force.point;
                 const Vector3& f = force.force;
-                os << "  point(" << p.x() << ", " << p.y() << ", " << p.z()
-                   << "), force(" << f.x() << ", " << f.y() << ", " << f.z() << ")\n";
+                // os << "  point(" << p.x() << ", " << p.y() << ", " << p.z()
+                //    << "), force(" << f.x() << ", " << f.y() << ", " << f.z() << ")\n";  // Rafa commented this
             }
         }
     }

src/Body/BasicSensorSimulationHelper.cpp

@@ -80,10 +80,12 @@ void BasicSensorSimulationHelper::initialize(Body* body, double timeStep, const
     DeviceList<> devices = body->devices();
     if(!devices.empty()){
         forceSensors_.extractFrom(devices);
+	tactileSensors_.extractFrom(devices);
         rateGyroSensors_.extractFrom(devices);
         accelerationSensors_.extractFrom(devices);
 
         if(!forceSensors_.empty() ||
+	   !tactileSensors_.empty() ||
            !rateGyroSensors_.empty() ||
            !accelerationSensors_.empty()){
             impl->initialize(body, timeStep, gravityAcceleration);

src/Body/BasicSensorSimulationHelper.h

@@ -8,6 +8,7 @@
 
 #include "DeviceList.h"
 #include "ForceSensor.h"
+#include "TactileSensor.h"
 #include "RateGyroSensor.h"
 #include "AccelerationSensor.h"
 #include "exportdecl.h"
@@ -31,6 +32,7 @@ class CNOID_EXPORT BasicSensorSimulationHelper
     bool hasGyroOrAccelerationSensors() const { return !rateGyroSensors_.empty() || !accelerationSensors_.empty(); }
 
     const DeviceList<ForceSensor>& forceSensors() const { return forceSensors_; }
+    const DeviceList<TactileSensor>& tactileSensors() const { return tactileSensors_; }
     const DeviceList<RateGyroSensor>& rateGyroSensors() const { return rateGyroSensors_; }
     const DeviceList<AccelerationSensor>& accelerationSensors() const { return accelerationSensors_; }
 
@@ -40,6 +42,7 @@ class CNOID_EXPORT BasicSensorSimulationHelper
     BasicSensorSimulationHelperImpl* impl;
     bool isActive_;
     DeviceList<ForceSensor> forceSensors_;
+    DeviceList<TactileSensor> tactileSensors_;
     DeviceList<RateGyroSensor> rateGyroSensors_;
     DeviceList<AccelerationSensor> accelerationSensors_;
 

src/Body/CMakeLists.txt

@@ -36,6 +36,7 @@ set(sources
   Camera.cpp
   RangeCamera.cpp
   RangeSensor.cpp
+  TactileSensor.cpp
   Light.cpp
   PointLight.cpp
   SpotLight.cpp
@@ -110,6 +111,7 @@ set(headers
   Camera.h
   RangeCamera.h
   RangeSensor.h
+  TactileSensor.h
   Light.h
   PointLight.h
   SpotLight.h

src/Body/ForwardDynamicsABM.cpp

@@ -95,6 +95,9 @@ void ForwardDynamicsABM::calcMotionWithEulerMethod()
     if(!sensorHelper.forceSensors().empty()){
         updateForceSensors();
     }
+    if(!sensorHelper.tactileSensors().empty()){
+        updateTactileSensors();
+    }
 
     DyLink* root = body->rootLink();
 
@@ -177,6 +180,9 @@ void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod()
     if(!sensorHelper.forceSensors().empty()){
         updateForceSensors();
     }
+    if(!sensorHelper.tactileSensors().empty()){
+        updateTactileSensors();
+    }
 
     integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
     calcABMPhase1(false);
@@ -515,3 +521,74 @@ void ForwardDynamicsABM::updateForceSensors()
         sensor->notifyStateChange();
     }
 }
+
+
+void ForwardDynamicsABM::updateTactileSensors()
+{
+    const double EPSILON = 1e-6;
+
+    const DeviceList<TactileSensor>& sensors = sensorHelper.tactileSensors();
+
+    for(size_t i=0; i < sensors.size(); ++i){
+
+        TactileSensor* sensor = sensors[i];
+
+		DyLink* link = static_cast<DyLink*>(sensor->link());
+
+		DyLink::ConstraintForceArray& forces = link->constraintForces();
+		if(!forces.empty()){
+
+		    sensor->forceData().clear();
+		    vector<tuple<double,double,double>> points;
+		    double xRange = sensor->maxX() - sensor->minX();
+			double yRange = sensor->maxY() - sensor->minY();
+			double xCellSize = xRange/sensor->cols();
+			double yCellSize = yRange/sensor->rows();
+			double currentX = sensor->minX();
+			double currentY = sensor->minY();
+
+			for (int j = 0; j<sensor->rows()*sensor->cols(); j++){
+				points.push_back(make_tuple(currentX, currentY, -EPSILON));
+				currentX += xCellSize;
+				if ((j+1)%sensor->cols() == 0){
+					currentX = sensor->minX();
+					currentY += yCellSize;
+				}
+			}
+			for(size_t j=0; j < forces.size(); ++j){
+		        const DyLink::ConstraintForce& force_surf = forces[j];
+				Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p());
+				Vector3 f_surf = link->R().transpose() * force_surf.force;
+				if (p_surf.z() < sensor->p_local().z() + EPSILON) {
+					double x = p_surf.x();
+					double y = p_surf.y();
+					//saturate
+					if (x>sensor->maxX()) x = sensor->maxX();
+					if (y>sensor->maxY()) y = sensor->maxY();
+					if (x<sensor->minX()) x = sensor->minX();
+					if (y<sensor->minY()) y = sensor->minY();
+					double absX = x - sensor->minX();
+					double absY = y - sensor->minY();
+					int matrixX = floor(absX/xCellSize);
+					int matrixY = floor(absY/yCellSize);
+					if (matrixX >= sensor->cols()) matrixX = sensor->cols()-1;
+					if (matrixY >= sensor->rows()) matrixY = sensor->rows()-1;
+
+					if (get<2>(points.at(matrixY*sensor->cols() + matrixX)) == -EPSILON){
+						get<2>(points.at(matrixY*sensor->cols() + matrixX)) = f_surf.z();
+					}
+					else{
+						get<2>(points.at(matrixY*sensor->cols() + matrixX)) = get<2>(points.at(matrixY*sensor->cols() + matrixX)) + f_surf.z();
+					}
+				}
+			}
+			for (int j=0; j<points.size(); j++){
+				if (get<2>(points.at(j)) > -EPSILON){
+					std::pair<Vector2, Vector3> xy_f = std::make_pair( Vector2(get<0>(points.at(j)), get<1>(points.at(j))) , Vector3(0,0, get<2>(points.at(j))) );
+					sensor->forceData().push_back(xy_f);
+					sensor->notifyStateChange();
+				}
+			}
+		}
+    }
+}

src/Body/ForwardDynamicsABM.h

@@ -52,6 +52,7 @@ class CNOID_EXPORT ForwardDynamicsABM : public ForwardDynamics
     inline void calcABMLastHalf();
 
     void updateForceSensors();
+    void updateTactileSensors();
 
     // Buffers for the Runge Kutta Method
     Position T0;

src/Body/TactileSensor.cpp

@@ -0,0 +1,112 @@
+/**
+   \file
+   \author Rafael Cisneros
+*/
+
+#include "TactileSensor.h"
+
+using namespace cnoid;
+
+
+TactileSensor::TactileSensor() //: maxX_(0), maxY_(0), minX_(0), minY_(0), rows_(0), cols_(0)
+{
+  forceData_ = std::make_shared<ForceData>();
+  maxX_ = maxY_ = minX_ = minY_ = rows_ = cols_ = 0;
+}
+
+
+TactileSensor::TactileSensor(const TactileSensor& org, bool copyStateOnly)
+  : Device(org, copyStateOnly)
+{
+  copyStateFrom(org);
+}
+
+
+const char* TactileSensor::typeName()
+{
+  return "TactileSensor";
+}
+
+
+void TactileSensor::copyStateFrom(const TactileSensor& other)
+{
+  forceData_ = other.forceData_;
+  maxX_ = other.maxX_;
+  maxY_ = other.maxY_;
+  minX_ = other.minX_;
+  minY_ = other.minY_;
+  rows_ = other.rows_;
+  cols_ = other.cols_;
+}
+
+
+void TactileSensor::copyStateFrom(const DeviceState& other)
+{
+  if (typeid(other) != typeid(TactileSensor))
+    throw std::invalid_argument("Type mismatch in the Device::copyStateFrom function");
+
+  copyStateFrom(static_cast<const TactileSensor&>(other));
+}
+
+Referenced* TactileSensor::doClone(CloneMap*) const
+{
+  return new TactileSensor(*this, false);
+}
+
+
+DeviceState* TactileSensor::cloneState() const
+{
+  return new TactileSensor(*this, true);
+}
+
+
+void TactileSensor::forEachActualType(std::function<bool(const std::type_info& type)> func)
+{
+  if (!func(typeid(TactileSensor)))
+    Device::forEachActualType(func);
+}
+
+
+void TactileSensor::clearState()
+{
+  forceData_->clear();
+}
+
+
+int TactileSensor::stateSize() const
+{
+  return forceData_->size() * 5;
+}
+
+
+const double* TactileSensor::readState(const double* buf)
+{
+  return buf;
+}
+
+
+double* TactileSensor::writeState(double* out_buf) const
+{
+  for (size_t i = 0; i < forceData_->size(); i++) {
+    Eigen::Map<Vector2>(out_buf) << (*forceData_)[i].first;
+    out_buf = out_buf + 2;
+    Eigen::Map<Vector3>(out_buf) << (*forceData_)[i].second;
+    out_buf = out_buf + 3;
+  }
+
+  return out_buf;
+}
+
+void TactileSensor::setMaxX(double maxX) {maxX_ = maxX;}
+void TactileSensor::setMaxY(double maxY) {maxY_ = maxY;}
+void TactileSensor::setMinX(double minX) {minX_ = minX;}
+void TactileSensor::setMinY(double minY) {minY_ = minY;}
+void TactileSensor::setRows(int rows) {rows_ = rows;}
+void TactileSensor::setCols(int cols) {cols_ = cols;}
+
+double TactileSensor::maxX() {return maxX_;}
+double TactileSensor::maxY() {return maxY_;}
+double TactileSensor::minY() {return minY_;}
+double TactileSensor::minX() {return minX_;}
+int TactileSensor::rows() {return rows_;}
+int TactileSensor::cols() {return cols_;}

src/Body/TactileSensor.h

@@ -0,0 +1,69 @@
+/**
+   \file
+   \author Rafael Cisneros
+*/
+
+#ifndef CNOID_BODY_TACTILE_SENSOR_H
+#define CNOID_BODY_TACTILE_SENSOR_H
+
+#include "Device.h"
+#include <memory>
+#include "exportdecl.h"
+#include <iostream>
+namespace cnoid {
+
+class CNOID_EXPORT TactileSensor : public Device
+{
+ public:
+
+  TactileSensor();
+  TactileSensor(const TactileSensor& org, bool copyStateOnly = false);
+
+  virtual const char* typeName() override;
+  void copyStateFrom(const TactileSensor& other);
+  virtual void copyStateFrom(const DeviceState& other) override;
+  virtual DeviceState* cloneState() const override;
+  virtual void forEachActualType(std::function<bool(const std::type_info& type)> func) override;
+  virtual void clearState() override;
+  virtual int stateSize() const override;
+  virtual const double* readState(const double* buf) override;
+  virtual double* writeState(double* out_buf) const override;
+
+  typedef std::vector< std::pair<Vector2, Vector3> > ForceData;
+
+  const ForceData& forceData() const { return *forceData_; }
+  ForceData& forceData() { return *forceData_; }
+
+  void setMaxX(double maxX);
+  void setMaxY(double maxY); 
+  void setMinX(double minX); 
+  void setMinY(double minY); 
+  void setRows(int rows); 
+  void setCols(int cols); 
+
+  double maxX();
+  double maxY();
+  double minY();
+  double minX();
+  int rows();
+  int cols();
+
+ protected:
+  virtual Referenced* doClone(CloneMap* cloneMap) const override;
+
+ private:
+
+  std::shared_ptr<ForceData> forceData_;
+  double maxX_;
+  double maxY_;
+  double minX_;
+  double minY_;
+  int rows_;
+  int cols_;
+};
+
+ typedef ref_ptr<TactileSensor> TactileSensorPtr;
+
+}
+
+#endif