Add calibrated skin

.github/workflows/ci.yml

@@ -8,6 +8,11 @@ on:
   # Execute a "nightly" build at 2 AM UTC
   - cron:  '0 2 * * *'
 
+env:
+  vcpkg_robotology_TAG: v0.10.1
+
+  # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
+  VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg
 
 jobs:
   build:
@@ -40,21 +45,27 @@ jobs:
     - name: Dependencies [Windows]
       if: matrix.os == 'windows-latest'
       run: |
-        git clone https://github.com/robotology-dependencies/robotology-vcpkg-binary-ports C:/robotology-vcpkg-binary-ports
-        vcpkg.exe --overlay-ports=C:/robotology-vcpkg-binary-ports install --triplet x64-windows ace libxml2 eigen3 ipopt-binary catch2
+        # To avoid spending a huge time compiling vcpkg dependencies, we download a root that comes precompiled with all the ports that we need
+        choco install -y wget unzip
+        # To avoid problems with non-relocatable packages, we unzip the archive exactly in the same C:/robotology/vcpkg
+        # that has been used to create the pre-compiled archive
+        cd C:/
+        wget https://github.com/robotology/robotology-superbuild-dependencies-vcpkg/releases/download/${env:vcpkg_robotology_TAG}/vcpkg-robotology-with-gazebo.zip
+        unzip vcpkg-robotology-with-gazebo.zip -d C:/
+        rm vcpkg-robotology-with-gazebo.zip
 
     - name: Dependencies [macOS]
       if: matrix.os == 'macOS-latest'
       run: |
-        brew install ace boost eigen swig qt5 orocos-kdl catch2
+        brew install ace boost eigen swig qt5 orocos-kdl catch2 nlohmann-json
 
     - name: Dependencies [Ubuntu]
       if: matrix.os == 'ubuntu-latest'
       run: |
         sudo apt-get update
         sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev  libboost-system-dev libboost-filesystem-dev \
                              libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \
-                             libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind
+                             libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python3-dev valgrind nlohmann-json3-dev
 
     - name: Source-based Dependencies [Windows]
       if: matrix.os == 'windows-latest'

app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini

@@ -1,9 +1,12 @@
 ###############
 ### ROBOT
 ###############
-remote_control_boards   ("left_arm")
+remote_control_boards           ("left_arm")
 
-remote_sensor_boards    "left_hand"
+remote_sensor_boards            "left_hand"
+
+connect_to_calibrated_skin      1
+remote_calibrated_sensor_board  "left_hand_filtered"
 
 axis_list             ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index_distal", "l_middle_proximal", "l_middle_distal", "l_pinky" )
 
@@ -149,7 +152,7 @@ axesJointsCoupled     1
 
 # in case a calibration phase necessary before starting teleoperating the robot,
 # otherwise read the calibration matrix from the config file
-doCalibration           1
+doCalibration           0
 
 #robot controller exponential filter gain
 exponentialFilterGain           0.9
@@ -200,7 +203,12 @@ l_ring_finger_tactile_info           ( 24                      35
 l_little_finger_tactile_info         ( 36                      47                    0.14                1.0                    200.0                   0.5                    3.0                   40.0)
 
 
-net_model_name                       "left_palm_net.json"
+net_model_name                       "left_palm_model_alexnet32_25ep.json"
+use_calibrated_skin_for_palm         1
+palm_skin_index_offset               96
+number_of_palm_skin_taxels           48
+palm_skin_activation_norm_raw        1630
+palm_skin_activation_norm_calibrated 15
 palm_skin_mapping                    ((29 0 0), (28 0 1), (30 0 2), (31 0 3), (33 0 4), (27 1 1), (26 1 2), (32 1 3), (34 1 4), (35 2 2), (25 2 3), (24 2 4), (6 0 5), (7 0 6), (8 0 7), (9 0 8), (10 0 9), (11 0 10), (3 1 5), (1 1 6), (4 1 7), (5 1 8), (2 2 5), (0 2 6), (17 2 7), (20 2 8), (21 2 9), (15 3 5), (14 3 6), (16 3 7), (19 3 8), (13 4 7), (18 4 8), (22 3 9), (12 4 9), (23 5 8), (45 5 5), (42 5 6), (36 5 7), (44 6 5), (40 6 6), (38 6 7), (37 6 8), (41 7 6), (39 7 7), (43 8 6), (47 4 6), (46 4 5))
 
 

app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini

@@ -145,7 +145,7 @@ analog_sensors_raw_max_boundary ( 25.0  0.0   37.0    15.0  24.0  12.0     0.0
 # in case each joint does not have independant motor to actuate, they are coupled
 axesJointsCoupled     1
 
-doCalibration           1
+doCalibration           0
 
 #robot controller exponential filter gain
 exponentialFilterGain           0.9

modules/HapticGlove_module/include/RobotSkin.hpp

@@ -118,14 +118,16 @@ struct HapticGlove::FingertipTactileData
 
     double contactThreshold()
     {
-        return contactThresholdValue 
-               + contactThresholdMultiplier * stdTactileSensor[this->maxTactileFeedbackAbsoluteElement()];
+        return contactThresholdValue
+               + contactThresholdMultiplier
+                     * stdTactileSensor[this->maxTactileFeedbackAbsoluteElement()];
     }
 
     double contactDerivativeThreshold()
     {
         return contactDerivativeThresholdValue
-               + contactDerivativeThresholdMultiplier * stdTactileSensorDerivative[this->maxTactileFeedbackDerivativeElement()];
+               + contactDerivativeThresholdMultiplier
+                     * stdTactileSensorDerivative[this->maxTactileFeedbackDerivativeElement()];
     }
 
     void printInfo() const
@@ -190,11 +192,30 @@ class HapticGlove::RobotSkin : RobotSkinService
         m_fingertipRawTactileFeedbacksStdVector; /**< fingertip raw tactile feedbacks, `0`
                                         means high pressure, `255` means low pressure */
 
+    yarp::sig::Vector
+        m_calibratedTactileFeedbacksYarpVector; /**<  calibrated
+                                                        tactile feedbacks, `255` means high
+                                                        pressure, `0` means low pressure */
+    std::vector<double> m_calibratedTactileFeedbacksStdVector; /**<  calibrated
+                                                        tactile feedbacks, `255` means high
+                                                        pressure, `0` means low pressure */
+
     yarp::dev::PolyDriver m_tactileSensorDevice; /**< Analog device for the skin. */
 
     yarp::dev::IAnalogSensor* m_tactileSensorInterface{
         nullptr}; /**< skin ananlog sensor interface */
 
+    // Differently for the row data the calibrated tactile data are provided by the skinmanager as
+    // vector streamed in a yarp port
+    yarp::os::BufferedPort<yarp::sig::Vector> m_tactileCalibratedSensorPort;
+    bool m_isTactileCalibrateConnected{false};
+
+    bool m_useCalibratedSkinForPalm{false};
+    int m_palmSkinIndexOffset{96};
+    int m_numberOfPalmSkinTaxels{48};
+    int m_palmSkinActivationNormRaw{1630};
+    int m_palmSkinActivationNormCalibrated{15};
+
     std::vector<double>
         m_fbParams; /**< # absolute vibrotactile feedback nonlinear function parameters; reference
                      to
@@ -227,6 +248,8 @@ class HapticGlove::RobotSkin : RobotSkinService
 
     bool getRawTactileFeedbackFromRobot();
 
+    bool getCalibratedTactileFeedbackFromRobot();
+
     Eigen::MatrixXf m_skinMatrix;
 
 public:
@@ -280,6 +303,9 @@ class HapticGlove::RobotSkin : RobotSkinService
      */
     const yarp::sig::Vector& fingerRawTactileFeedbacks() const;
 
+
+    const yarp::sig::Vector& fingerCalibratedTactileFeedbacks() const;
+
     /**
      * Get the fingertip calibrated tactile feedbacks
      * @param fingertipTactileFeedbacks the tactile feedbacks of all the links
@@ -318,7 +344,13 @@ class HapticGlove::RobotSkin : RobotSkinService
 
     const Eigen::MatrixXf& getPalmSkinMatrix(const yarp::sig::Vector& rawData);
 
-    bool isPalmSkinActive(const yarp::sig::Vector& rawData);
+    bool isPalmRawSkinActive() const;
+
+    bool isPalmCalibratedSkinActive() const;
+
+    bool isPalmSkinActive() const;
+
+    bool getUseCalibratedSkinForPalm() const;
 };
 
 #endif // ROBOT_SKIN_HPP

modules/HapticGlove_module/src/GloveWearable.cpp

@@ -334,6 +334,7 @@ bool GloveWearableImpl::getFingertipPoseValues(Eigen::MatrixXd& values)
 
 bool GloveWearableImpl::setFingertipForceFeedbackValues(const std::vector<int>& values)
 {
+    return true;
     if (values.size() != m_numForceFeedback)
     {
         yError() << m_logPrefix
@@ -362,6 +363,7 @@ bool GloveWearableImpl::setFingertipForceFeedbackValues(const std::vector<int>&
 
 bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector<int>& values)
 {
+    return true;
     if (values.size() != m_numVibrotactileFeedback)
     {
         yError()

modules/HapticGlove_module/src/RobotController.cpp

@@ -62,10 +62,10 @@ bool RobotController::configure(const yarp::os::Searchable& config,
 
     if (m_axesJointsCoupled)
     {
-        m_A.resize(m_numActuatedJoints, m_numActuatedAxis);
-        m_Bias.resize(m_numActuatedJoints, 1);
+        m_A = Eigen::MatrixXd::Identity(m_numActuatedAxis, m_numActuatedAxis);
+        m_Bias = Eigen::MatrixXd::Zero(m_numActuatedJoints, 1);
 
-        if (!m_doCalibration)
+        if (false && !m_doCalibration)
         {
             if (m_numActuatedAxis != m_numAllAxis && m_numActuatedJoints != m_numAllJoints)
             {
@@ -77,8 +77,7 @@ bool RobotController::configure(const yarp::os::Searchable& config,
                 return false;
             }
             std::vector<double> A_vector, Bias_vector;
-            A_vector.resize(m_numActuatedJoints * m_numActuatedAxis);
-            Bias_vector.resize(m_numActuatedJoints);
+
             if (!YarpHelper::getVectorFromSearchable(config, "CouplingMatrix", A_vector))
             {
                 yError() << m_logPrefix
@@ -117,11 +116,6 @@ bool RobotController::configure(const yarp::os::Searchable& config,
                 m_Bias(i) = Bias_vector[i];
             }
         }
-    } else
-    {
-        // if not coupled., the mapping between the motors and joints are identity matrix
-        m_A = Eigen::MatrixXd::Identity(m_numActuatedAxis, m_numActuatedAxis);
-        m_Bias = Eigen::MatrixXd::Zero(m_numActuatedJoints, 1);
     }
 
     // get control  gains from configuration files