fix: improve RH56 tactile data read speed

cmvr-es/applications/include/touch_screen_app.h

@@ -57,14 +57,6 @@ public:
         ROBOT_COMMAND_FAILED      // 向机器人下发控制命令失败。
     };
 
-    enum class TactileRegion {
-        TIP = 0,
-        FINGER,
-        PAD,
-        TIP_AND_FINGER,
-        THUMB_MIDDLE
-    };
-
     enum class AlignMode {
         POSE_AND_POSITION = 0,  // 使用配置里的固定 rx/ry/rz 与位置一起对齐。
         RX_RY_AND_POSITION,     // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。
@@ -202,11 +194,14 @@ public:
         // 使用哪根手指的触觉阵列判断是否接触。
         device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX};
         // 使用该手指的哪个触觉区域。
-        TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER};
+        device::AbstractDexHand::TactileRegion tactile_region{
+            device::AbstractDexHand::TactileRegion::TIP};
         // 触觉压力和阈值；总和超过该值认为已经接触。
         double tactile_pressure_sum_threshold{100.0};
         // 触觉峰值阈值；为 0 时表示不使用峰值判据。
         double tactile_pressure_peak_threshold{0.0};
+        // 当前帧非 0 触觉点数量阈值；只有达到该数量后，才进一步判断 sum/peak 阈值。
+        int tactile_nonzero_count_threshold{1};
     };
 
     TouchScreenApp();
@@ -244,6 +239,7 @@ public:
     int targetV() const { return target_v_; }
     double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
     double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
+    int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; }
     int lastActiveTagId() const { return last_active_tag_id_; }
     const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
 
@@ -306,6 +302,7 @@ private:
 
     double last_touch_pressure_sum_{0.0};
     double last_touch_pressure_peak_{0.0};
+    int last_touch_nonzero_count_{0};
     Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
     bool locked_target_rotation_valid_{false};
     Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};

cmvr-es/applications/src/touch_screen_app.cpp

@@ -45,14 +45,20 @@ bool computeLinearMoveDeltaTool(const Eigen::Matrix<double, 6, 1>& twist_base,
 }
 
 bool isTouchTriggered(const TouchScreenApp::Options& options,
+                      const int nonzero_count,
                       const double pressure_sum,
                       const double pressure_peak) {
+    if (options.tactile_nonzero_count_threshold > 0 &&
+        nonzero_count < options.tactile_nonzero_count_threshold) {
+        return false;
+    }
     return pressure_sum >= options.tactile_pressure_sum_threshold ||
            (options.tactile_pressure_peak_threshold > 0.0 &&
             pressure_peak >= options.tactile_pressure_peak_threshold);
 }
 
 void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& matrix,
+                           int& nonzero_count_out,
                            double& sum_out,
                            double& peak_out) {
     if (!matrix.valid()) {
@@ -63,6 +69,9 @@ void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& mat
         const auto* row_data = matrix.rowData(row);
         for (int col = 0; col < matrix.cols; ++col) {
             const double value = static_cast<double>(row_data[col]);
+            if (row_data[col] != 0) {
+                ++nonzero_count_out;
+            }
             sum_out += value;
             peak_out = std::max(peak_out, value);
         }
@@ -127,27 +136,48 @@ Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
     return aa.axis() * aa.angle();
 }
 
+bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target,
+                                          const Eigen::Matrix3d& R_current,
+                                          double& yaw_rad_out) {
+    if (!R_target.allFinite() || !R_current.allFinite()) {
+        return false;
+    }
+
+    const Eigen::Vector3d z_ref = R_target.col(2);
+    const Eigen::Vector3d x_ref = R_target.col(0);
+    const Eigen::Vector3d y_ref = R_target.col(1);
+
+    Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0));
+    if (in_plane.norm() <= 1e-9) {
+        in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1));
+    }
+    const double in_plane_norm = in_plane.norm();
+    if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) {
+        return false;
+    }
+
+    in_plane /= in_plane_norm;
+    yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane));
+    return std::isfinite(yaw_rad_out);
+}
+
 bool appendRequestedTactileRegions(
     const device::AbstractDexHand::FingerType finger,
-    const TouchScreenApp::TactileRegion region,
+    const device::AbstractDexHand::TactileRegion region,
     std::vector<device::AbstractDexHand::TactileRegionKey>& regions_out) {
     using DeviceTactileRegion = device::AbstractDexHand::TactileRegion;
 
     switch (region) {
-        case TouchScreenApp::TactileRegion::TIP:
+        case device::AbstractDexHand::TactileRegion::TIP:
             regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
             return true;
-        case TouchScreenApp::TactileRegion::FINGER:
+        case device::AbstractDexHand::TactileRegion::FINGER:
             regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
             return true;
-        case TouchScreenApp::TactileRegion::PAD:
+        case device::AbstractDexHand::TactileRegion::PAD:
             regions_out.emplace_back(finger, DeviceTactileRegion::PAD);
             return true;
-        case TouchScreenApp::TactileRegion::TIP_AND_FINGER:
+        case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE:
-            regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
-            regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
-            return true;
-        case TouchScreenApp::TactileRegion::THUMB_MIDDLE:
             if (finger != device::AbstractDexHand::FingerType::THUMB) {
                 return false;
             }
@@ -197,21 +227,42 @@ device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreen
     return device::AbstractDexHand::FingerType::INDEX;
 }
 
-TouchScreenApp::TactileRegion toTactileRegion(
+const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) {
+    switch (finger) {
+        case device::AbstractDexHand::FingerType::PINKY: return "PINKY";
+        case device::AbstractDexHand::FingerType::RING: return "RING";
+        case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE";
+        case device::AbstractDexHand::FingerType::INDEX: return "INDEX";
+        case device::AbstractDexHand::FingerType::THUMB: return "THUMB";
+        case device::AbstractDexHand::FingerType::PALM: return "PALM";
+    }
+    return "UNKNOWN";
+}
+
+const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) {
+    switch (region) {
+        case device::AbstractDexHand::TactileRegion::TIP: return "TIP";
+        case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER";
+        case device::AbstractDexHand::TactileRegion::PAD: return "PAD";
+        case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
+        case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
+    }
+    return "UNKNOWN";
+}
+
+device::AbstractDexHand::TactileRegion toTactileRegion(
     const cmvr::config::TouchScreenTactileRegion region) {
     switch (region) {
         case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP:
-            return TouchScreenApp::TactileRegion::TIP;
+            return device::AbstractDexHand::TactileRegion::TIP;
         case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER:
-            return TouchScreenApp::TactileRegion::FINGER;
+            return device::AbstractDexHand::TactileRegion::FINGER;
         case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD:
-            return TouchScreenApp::TactileRegion::PAD;
+            return device::AbstractDexHand::TactileRegion::PAD;
-        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER:
-            return TouchScreenApp::TactileRegion::TIP_AND_FINGER;
         case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE:
-            return TouchScreenApp::TactileRegion::THUMB_MIDDLE;
+            return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE;
     }
-    return TouchScreenApp::TactileRegion::TIP;
+    return device::AbstractDexHand::TactileRegion::TIP;
 }
 
 TouchScreenApp::AlignMode toAlignMode(const cmvr::config::TouchScreenAlignMode mode) {
@@ -445,6 +496,7 @@ bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
         last_active_tag_id_ = -1;
         last_touch_pressure_sum_ = 0.0;
         last_touch_pressure_peak_ = 0.0;
+        last_touch_nonzero_count_ = 0;
         last_align_error_camera_.setZero();
         last_status_ = Status::IDLE;
     } else {
@@ -539,6 +591,7 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
     align_stable_count_ = 0;
     last_touch_pressure_sum_ = 0.0;
     last_touch_pressure_peak_ = 0.0;
+    last_touch_nonzero_count_ = 0;
     last_active_tag_id_ = -1;
     last_align_error_camera_.setZero();
     locked_target_rotation_valid_ = false;
@@ -560,7 +613,15 @@ bool TouchScreenApp::step() {
     }
 
     if (dexhand_) {
-        updateTouchPressure();
+        const bool tactile_ok = updateTouchPressure();
+        // std::cout << "[TouchScreenApp][TACTILE] finger="
+        //           << fingerTypeToString(options_.tactile_finger)
+        //           << ", region=" << tactileRegionToString(options_.tactile_region)
+        //           << ", ok=" << (tactile_ok ? 1 : 0)
+        //           << ", nonzero_count=" << last_touch_nonzero_count_
+        //           << ", pressure_sum=" << last_touch_pressure_sum_
+        //           << ", pressure_peak=" << last_touch_pressure_peak_
+        //           << std::endl;
     }
 
     switch (phase_) {
@@ -621,6 +682,7 @@ void TouchScreenApp::stop() {
     last_active_tag_id_ = -1;
     last_touch_pressure_sum_ = 0.0;
     last_touch_pressure_peak_ = 0.0;
+    last_touch_nonzero_count_ = 0;
     last_align_error_camera_.setZero();
     locked_target_rotation_valid_ = false;
     locked_target_rotation_.setIdentity();
@@ -874,6 +936,10 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
     if (config.has_tactile_pressure_peak_threshold()) {
         options.tactile_pressure_peak_threshold = config.tactile_pressure_peak_threshold();
     }
+    if (config.has_tactile_nonzero_count_threshold()) {
+        options.tactile_nonzero_count_threshold =
+            std::max(0, config.tactile_nonzero_count_threshold());
+    }
 
     options_out = options;
     return true;
@@ -1042,9 +1108,8 @@ bool TouchScreenApp::stepAligning() {
             break;
         case AlignMode::RX_RY_AND_POSITION:
             if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) {
-                const Eigen::Matrix3d R_delta = R_target.transpose() * R_current;
+                double locked_yaw_rad = 0.0;
-                const double locked_yaw_rad = std::atan2(R_delta(1, 0), R_delta(0, 0));
+                if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) {
-                if (!std::isfinite(locked_yaw_rad)) {
                     enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
                     return false;
                 }
@@ -1170,7 +1235,10 @@ bool TouchScreenApp::stepTouching() {
         return false;
     }
 
-    if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) {
+    if (isTouchTriggered(options_,
+                         last_touch_nonzero_count_,
+                         last_touch_pressure_sum_,
+                         last_touch_pressure_peak_)) {
         if (options_.touch_use_speedl) {
             logTouchingSpeedLState();
         }
@@ -1206,7 +1274,10 @@ bool TouchScreenApp::stepTouching() {
                 enterFailed(Status::TACTILE_UNAVAILABLE);
                 return false;
             }
-            if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) {
+            if (isTouchTriggered(options_,
+                                 last_touch_nonzero_count_,
+                                 last_touch_pressure_sum_,
+                                 last_touch_pressure_peak_)) {
                 if (!handleTouchTriggered(true)) {
                     enterFailed(Status::ROBOT_COMMAND_FAILED);
                     return false;
@@ -1562,6 +1633,7 @@ void TouchScreenApp::enterFailed(const Status status) {
 bool TouchScreenApp::updateTouchPressure() {
     last_touch_pressure_sum_ = 0.0;
     last_touch_pressure_peak_ = 0.0;
+    last_touch_nonzero_count_ = 0;
     if (!dexhand_) {
         return false;
     }
@@ -1573,6 +1645,7 @@ bool TouchScreenApp::updateTouchPressure() {
         return false;
     }
 
+    int nonzero_count = 0;
     double sum = 0.0;
     double peak = 0.0;
     try {
@@ -1581,12 +1654,13 @@ bool TouchScreenApp::updateTouchPressure() {
             if (!sensor.valid()) {
                 return false;
             }
-            accumulateMatrixStats(sensor.view, sum, peak);
+            accumulateMatrixStats(sensor.view, nonzero_count, sum, peak);
         }
     } catch (...) {
         return false;
     }
 
+    last_touch_nonzero_count_ = nonzero_count;
     last_touch_pressure_sum_ = sum;
     last_touch_pressure_peak_ = peak;
     return true;

cmvr-es/applications/src/touch_screen_app_test.cpp

@@ -45,6 +45,7 @@ void run_touch_once(int u, int v) {
                   << ", target_c=[" << p_c_target.x() << ", "
                   << p_c_target.y() << ", "
                   << p_c_target.z() << "]"
+                  << ", nonzero_count=" << app.lastTouchNonzeroCount()
                   << ", pressure_sum=" << app.lastTouchPressureSum()
                   << ", pressure_peak=" << app.lastTouchPressurePeak()
                   << ", err_c=[" << app.lastAlignErrorCamera().x() << ", "
@@ -58,7 +59,8 @@ void run_touch_once(int u, int v) {
             align_reached = true;
         }
         if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) {
-            std::cout << "touch triggered, pressure_sum=" << app.lastTouchPressureSum()
+            std::cout << "touch triggered, nonzero_count=" << app.lastTouchNonzeroCount()
+                      << ", pressure_sum=" << app.lastTouchPressureSum()
                       << ", pressure_peak=" << app.lastTouchPressurePeak() << "\n";
             touch_triggered = true;
         }

cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt

@@ -135,7 +135,8 @@ retract_twist_base {
 retract_acceleration: 3.0
 retract_duration_s: 0.45
 
-tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
+tactile_finger: TOUCH_SCREEN_FINGER_TYPE_RING
 tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP
-tactile_pressure_sum_threshold: 300000.0
+tactile_pressure_sum_threshold: 200.0
-tactile_pressure_peak_threshold: 0.0
+tactile_pressure_peak_threshold: 0
+tactile_nonzero_count_threshold: 4

cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h

@@ -35,7 +35,7 @@ namespace cmvr::device {
         bool isOpen() const;
 
         void writeRegisters(int address, const uint16_t* values, int count);
-        void readRegisterBlock(int start_addr, int count, std::vector<uint16_t>& values);
+        void readRegisterBlock(int start_address, int count, std::vector<uint16_t>& values);
 
     private:
         void closeUnlocked();
@@ -121,7 +121,7 @@ namespace cmvr::device {
         std::thread tactile_thread_;
         std::atomic<bool> tactile_thread_running_{false};
         std::atomic<int> active_buffer_index_{0};
-        std::chrono::milliseconds tactile_poll_interval_{20};
+        std::chrono::milliseconds tactile_poll_interval_{10};
     };
 }
 

cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h

@@ -12,6 +12,7 @@ namespace cmvr::device {
 
     enum class RH56TactileFillOrder {
         ROW_MAJOR,
+        COLUMN_MAJOR_TOP_TO_BOTTOM,
         COLUMN_MAJOR_BOTTOM_TO_TOP
     };
 
@@ -57,8 +58,10 @@ namespace cmvr::device {
                 return;
             }
 
+            const bool reverse_rows = layout.fill_order == RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP;
             for (int col = 0; col < layout.cols; ++col) {
-                for (int row = layout.rows - 1; row >= 0 && input_index < valueCount; --row, ++input_index) {
+                for (int row_index = 0; row_index < layout.rows && input_index < valueCount; ++row_index, ++input_index) {
+                    const int row = reverse_rows ? (layout.rows - 1 - row_index) : row_index;
                     destination[static_cast<size_t>(row * layout.cols + col)] = sanitize_point(input[input_index]);
                 }
             }
@@ -114,7 +117,7 @@ namespace cmvr::device {
                  14,
                  950,
                  112,
-                 RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP}
+                 RH56TactileFillOrder::COLUMN_MAJOR_TOP_TO_BOTTOM}
             }};
             return layouts;
         }

cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp

@@ -23,8 +23,10 @@ namespace {
     constexpr int kDefaultPort = 6000;
     constexpr int kMaxRegistersPerRead = 125;
 
-    // The RH56 protocol documents addresses and lengths in bytes, while libmodbus
+    // The RH56 tactile map uses byte-style protocol addresses (3000, 3370, ...)
-    // expects holding-register indexes (16-bit words).
+    // while each Modbus read still returns 16-bit registers. Keep addresses in
+    // the protocol units that the datasheet/Python script use, and only convert
+    // byte lengths or address deltas to register counts when slicing buffers.
     constexpr int kRegisterByteWidth = 2;
     constexpr int kFingerTipByteLength = 18;      // 3 x 3
     constexpr int kFingerSurfaceByteLength = 192; // 12 x 8
@@ -37,10 +39,6 @@ namespace {
         return byte_length / kRegisterByteWidth;
     }
 
-    constexpr int toRegisterAddress(const int byte_address) {
-        return byte_address / kRegisterByteWidth;
-    }
-
     constexpr int nextByteAddress(const int start_byte_addr, const int byte_length) {
         return start_byte_addr + byte_length;
     }
@@ -57,12 +55,12 @@ namespace {
     struct TactileRegionConfig {
         FingerType finger;
         TactileRegion region;
-        int start_register;
+        int start_address;
         int register_count;
     };
 
     struct TactileReadSegment {
-        int start_register;
+        int start_address;
         int register_count;
         std::vector<size_t> region_indexes;
     };
@@ -72,84 +70,84 @@ namespace {
     const std::array<TactileRegionConfig, RH56DFTPDexhand::TACTILE_REGION_SLOT_COUNT> kTactileRegionConfigs = {{
         {FingerType::PINKY,
          TactileRegion::TIP,
-         toRegisterAddress(kTouchSensorBaseByteAddrPinky),
+         kTouchSensorBaseByteAddrPinky,
          toRegisterCount(kFingerTipByteLength)},
         {FingerType::PINKY,
          TactileRegion::FINGER,
-         toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength)),
+         nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength),
          toRegisterCount(kFingerSurfaceByteLength)},
         {FingerType::PINKY,
          TactileRegion::PAD,
-         toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength),
+         nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength),
-                                           kFingerSurfaceByteLength)),
+                         kFingerSurfaceByteLength),
          toRegisterCount(kFingerPadByteLength)},
 
         {FingerType::RING,
          TactileRegion::TIP,
-         toRegisterAddress(kTouchSensorBaseByteAddrRing),
+         kTouchSensorBaseByteAddrRing,
          toRegisterCount(kFingerTipByteLength)},
         {FingerType::RING,
          TactileRegion::FINGER,
-         toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength)),
+         nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength),
          toRegisterCount(kFingerSurfaceByteLength)},
         {FingerType::RING,
          TactileRegion::PAD,
-         toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength),
+         nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength),
-                                           kFingerSurfaceByteLength)),
+                         kFingerSurfaceByteLength),
          toRegisterCount(kFingerPadByteLength)},
 
         {FingerType::MIDDLE,
          TactileRegion::TIP,
-         toRegisterAddress(kTouchSensorBaseByteAddrMiddle),
+         kTouchSensorBaseByteAddrMiddle,
          toRegisterCount(kFingerTipByteLength)},
         {FingerType::MIDDLE,
          TactileRegion::FINGER,
-         toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength)),
+         nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength),
          toRegisterCount(kFingerSurfaceByteLength)},
         {FingerType::MIDDLE,
          TactileRegion::PAD,
-         toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength),
+         nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength),
-                                           kFingerSurfaceByteLength)),
+                         kFingerSurfaceByteLength),
          toRegisterCount(kFingerPadByteLength)},
 
         {FingerType::INDEX,
          TactileRegion::TIP,
-         toRegisterAddress(kTouchSensorBaseByteAddrIndex),
+         kTouchSensorBaseByteAddrIndex,
          toRegisterCount(kFingerTipByteLength)},
         {FingerType::INDEX,
          TactileRegion::FINGER,
-         toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength)),
+         nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength),
          toRegisterCount(kFingerSurfaceByteLength)},
         {FingerType::INDEX,
          TactileRegion::PAD,
-         toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength),
+         nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength),
-                                           kFingerSurfaceByteLength)),
+                         kFingerSurfaceByteLength),
          toRegisterCount(kFingerPadByteLength)},
 
         {FingerType::THUMB,
          TactileRegion::TIP,
-         toRegisterAddress(kTouchSensorBaseByteAddrThumb),
+         kTouchSensorBaseByteAddrThumb,
          toRegisterCount(kFingerTipByteLength)},
         {FingerType::THUMB,
          TactileRegion::FINGER,
-         toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength)),
+         nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength),
          toRegisterCount(kFingerSurfaceByteLength)},
         {FingerType::THUMB,
          TactileRegion::THUMB_MIDDLE,
-         toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength),
+         nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength),
-                                           kFingerSurfaceByteLength)),
+                         kFingerSurfaceByteLength),
          toRegisterCount(kThumbMiddleByteLength)},
         {FingerType::THUMB,
          TactileRegion::PAD,
-         toRegisterAddress(nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb,
+         nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb,
-                                                                           kFingerTipByteLength),
+                                                         kFingerTipByteLength),
-                                                           kFingerSurfaceByteLength),
+                                         kFingerSurfaceByteLength),
-                                           kThumbMiddleByteLength)),
+                         kThumbMiddleByteLength),
          toRegisterCount(kThumbPadByteLength)},
 
         {FingerType::PALM,
          TactileRegion::PALM_PAD,
-         toRegisterAddress(kTouchSensorBaseByteAddrPalm),
+         kTouchSensorBaseByteAddrPalm,
          kTouchSensorRegCountPalm}
     }};
 
@@ -201,8 +199,9 @@ namespace {
             const auto& config = kTactileRegionConfigs[i];
             if (!segments.empty()) {
                 auto& last_segment = segments.back();
-                const int last_end_register = last_segment.start_register + last_segment.register_count;
+                const int last_end_address =
-                if (last_end_register == config.start_register) {
+                    last_segment.start_address + last_segment.register_count * kRegisterByteWidth;
+                if (last_end_address == config.start_address) {
                     last_segment.register_count += config.register_count;
                     last_segment.region_indexes.push_back(i);
                     continue;
@@ -210,7 +209,7 @@ namespace {
             }
 
             TactileReadSegment segment;
-            segment.start_register = config.start_register;
+            segment.start_address = config.start_address;
             segment.register_count = config.register_count;
             segment.region_indexes.push_back(i);
             segments.push_back(std::move(segment));
@@ -281,7 +280,7 @@ void ModbusController::writeRegisters(const int address, const uint16_t* values,
     }
 }
 
-void ModbusController::readRegisterBlock(const int start_addr, const int count, std::vector<uint16_t>& values) {
+void ModbusController::readRegisterBlock(const int start_address, const int count, std::vector<uint16_t>& values) {
     values.assign(static_cast<size_t>(count), 0);
 
     std::lock_guard<std::mutex> lock(io_mutex_);
@@ -291,15 +290,16 @@ void ModbusController::readRegisterBlock(const int start_addr, const int count,
 
     for (int offset = 0; offset < count; offset += kMaxRegistersPerRead) {
         const int current_count = std::min(kMaxRegistersPerRead, count - offset);
-        const int rc = modbus_read_registers(ctx_, start_addr + offset, current_count, values.data() + offset);
+        const int current_address = start_address + offset * kRegisterByteWidth;
+        const int rc = modbus_read_registers(ctx_, current_address, current_count, values.data() + offset);
         if (rc == -1) {
             throw std::runtime_error(
-                "Failed to read register block at " + std::to_string(start_addr + offset) + ": " +
+                "Failed to read register block at " + std::to_string(current_address) + ": " +
                 modbus_strerror(errno));
         }
         if (rc != current_count) {
             throw std::runtime_error(
-                "Incomplete register read at " + std::to_string(start_addr + offset) + ", expected " +
+                "Incomplete register read at " + std::to_string(current_address) + ", expected " +
                 std::to_string(current_count) + ", got " + std::to_string(rc));
         }
     }
@@ -428,7 +428,7 @@ void RH56DFTPDexhand::setAngles(const std::vector<int>& finger_joint_angles) {
     try {
         ensureConnected();
         controller_->writeRegisters(
-            toRegisterAddress(kAngleSetByteAddress),
+            kAngleSetByteAddress,
             registers.data(),
             static_cast<int>(registers.size()));
 
@@ -518,10 +518,10 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) {
         std::vector<uint16_t> segment_values;
 
         for (const auto& segment : read_plan) {
-            controller_->readRegisterBlock(segment.start_register, segment.register_count, segment_values);
+            controller_->readRegisterBlock(segment.start_address, segment.register_count, segment_values);
             for (const size_t region_index : segment.region_indexes) {
                 const auto& config = kTactileRegionConfigs[region_index];
-                const int offset = config.start_register - segment.start_register;
+                const int offset = toRegisterCount(config.start_address - segment.start_address);
                 write_buffer.assignRegionData(
                     config.finger,
                     config.region,
@@ -541,12 +541,14 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) {
 }
 
 void RH56DFTPDexhand::tactilePollingLoop() {
+    auto next_poll_deadline = std::chrono::steady_clock::now();
     std::unique_lock<std::mutex> lock(polling_mutex_);
     while (tactile_thread_running_.load(std::memory_order_acquire)) {
         if (requested_polling_mask_.none()) {
             polling_cv_.wait(lock, [this]() {
                 return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any();
             });
+            next_poll_deadline = std::chrono::steady_clock::now();
             continue;
         }
 
@@ -559,7 +561,14 @@ void RH56DFTPDexhand::tactilePollingLoop() {
         }
         lock.lock();
 
-        polling_cv_.wait_for(lock, tactile_poll_interval_, [this, mask]() {
+        next_poll_deadline += tactile_poll_interval_;
+        const auto now = std::chrono::steady_clock::now();
+        if (next_poll_deadline <= now) {
+            next_poll_deadline = now;
+            continue;
+        }
+
+        polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() {
             return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask;
         });
     }

cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp

@@ -61,8 +61,8 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
 
     ASSERT_NE(hand_config, nullptr);
 
-    const auto finger = DexHand::FingerType::INDEX;
+    const auto finger = DexHand::FingerType::RING;
-    const auto region = DexHand::TactileRegion::FINGER;
+    const auto region = DexHand::TactileRegion::TIP;
     const int iterations = 20000;
     const int warmup_ms = 200;
     const int read_interval_ms = 10;

cmvr-es/service/grpc/src/grpc_hlc_service.cpp

@@ -118,6 +118,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
                           << ", phase="
                           << cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase())
                           << ", active_tag=" << touch_app_.lastActiveTagId()
+                          << ", nonzero_count=" << touch_app_.lastTouchNonzeroCount()
                           << ", pressure_sum=" << touch_app_.lastTouchPressureSum()
                           << ", pressure_peak=" << touch_app_.lastTouchPressurePeak();
                 last_logged_status = touch_app_.lastStatus();

protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto

@@ -66,7 +66,6 @@ enum TouchScreenTactileRegion {
   TOUCH_SCREEN_TACTILE_REGION_TIP = 0;
   TOUCH_SCREEN_TACTILE_REGION_FINGER = 1;
   TOUCH_SCREEN_TACTILE_REGION_PAD = 2;
-  TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER = 3;
   TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4;
 }
 
@@ -142,4 +141,5 @@ message TouchScreenAppConfig {
   optional TouchScreenTactileRegion tactile_region = 37;
   optional double tactile_pressure_sum_threshold = 38;
   optional double tactile_pressure_peak_threshold = 39;
+  optional int32 tactile_nonzero_count_threshold = 56;
 }

script/touch_data.py

@@ -0,0 +1,243 @@
+import time
+from pymodbus.client import ModbusTcpClient
+from pymodbus.pdu import ExceptionResponse
+
+# 定义 Modbus TCP 相关参数
+MODBUS_IP = "192.168.11.210"
+MODBUS_PORT = 6000
+
+# 定义各部分数据地址范围
+TOUCH_SENSOR_BASE_ADDR_PINKY = 3000  # 小拇指
+TOUCH_SENSOR_END_ADDR_PINKY = 3369
+
+TOUCH_SENSOR_BASE_ADDR_RING = 3370  # 无名指
+TOUCH_SENSOR_END_ADDR_RING = 3739
+
+TOUCH_SENSOR_BASE_ADDR_MIDDLE = 3740  # 中指
+TOUCH_SENSOR_END_ADDR_MIDDLE = 4109
+
+TOUCH_SENSOR_BASE_ADDR_INDEX = 4110  # 食指
+TOUCH_SENSOR_END_ADDR_INDEX = 4479
+
+TOUCH_SENSOR_BASE_ADDR_THUMB = 4480  # 大拇指
+TOUCH_SENSOR_END_ADDR_THUMB = 4899
+
+TOUCH_SENSOR_BASE_ADDR_PALM = 4900  # 掌心
+TOUCH_SENSOR_END_ADDR_PALM = 5123
+
+# Modbus 每次最多读取寄存器的数量
+MAX_REGISTERS_PER_READ = 125
+
+
+def read_register_range(client, start_addr, end_addr):
+    """
+    批量读取指定地址范围内的寄存器数据。
+    """
+    register_values = []
+    # 分段读取寄存器
+    for addr in range(start_addr, end_addr + 1, MAX_REGISTERS_PER_READ * 2):
+        current_count = min(MAX_REGISTERS_PER_READ, (end_addr - addr) // 2 + 1)
+        response = client.read_holding_registers(address=addr, count=current_count)
+
+        if isinstance(response, ExceptionResponse) or response.isError():
+            print(f"读取寄存器 {addr} 失败: {response}")
+            register_values.extend([0] * current_count)
+        else:
+            register_values.extend(response.registers)
+
+    return register_values
+
+def format_finger_data(finger_name, data):
+    """
+    格式化四指触觉数据。
+    """
+    result = {}
+
+    if finger_name != "大拇指":
+        # 四指格式
+        if len(data) < 185:
+            print(f"{finger_name} 数据长度不足，至少185个数据，实际：{len(data)}")
+            return None
+
+        idx = 0
+        # 指端数据 3x3
+        result['tip_end'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)]
+        idx += 9
+
+        # 指尖触觉数据 12x8
+        result['tip_touch'] = [data[idx + i*8: idx + (i+1)*8] for i in range(12)]
+        idx += 96
+
+        # 指腹触觉数据 10x8
+        result['finger_pad'] = [data[idx + i*8: idx + (i+1)*8] for i in range(10)]
+        idx += 80
+    else:
+        # 大拇指格式
+        if len(data) < 210:
+            print(f"{finger_name} 数据长度不足，至少210个数据，实际：{len(data)}")
+            return None
+
+        idx = 0
+        # 指端数据 3x3
+        result['tip_end'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)]
+        idx += 9
+
+        # 指尖触觉数据 12x8
+        result['tip_touch'] = [data[idx + i*8: idx + (i+1)*8] for i in range(12)]
+        idx += 96
+
+        # 指中触觉数据 3x3
+        result['middle_touch'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)]
+        idx += 9
+
+        # 指腹触觉数据 12x8
+        finger_pad = [data[idx + i*8: idx + (i+1)*8] for i in range(12)]
+        idx += 96
+
+        # 指腹触觉数据行元素反转
+       # finger_pad = [row[::-1] for row in finger_pad]
+        # 指腹触觉数据行顺序反转
+        #finger_pad.reverse()
+
+        result['finger_pad'] = finger_pad
+
+    return result
+
+def format_palm_data(data):
+    """
+    格式化掌心数据为14x8矩阵，然后转置为8x14矩阵。
+    """
+    expected_len = 14 * 8
+    if len(data) < expected_len:
+        print(f"掌心数据长度不足，至少{expected_len}个数据，实际：{len(data)}")
+        return None
+
+    # 生成原始矩阵（14行8列）
+    palm_matrix = [data[i*8:(i+1)*8] for i in range(14)]
+
+    # 转置矩阵
+    transposed = list(map(list, zip(*palm_matrix)))
+
+    return transposed
+
+
+def print_formatted_finger_data(finger_name, formatted_data):
+    if formatted_data is None:
+        print(f"{finger_name} 数据格式化失败")
+        return
+
+    print(f"--- {finger_name} 指端指端数据 (3x3) ---")
+    for row in formatted_data.get('tip_end', []):
+        print(row)
+
+    print(f"--- {finger_name} 指尖触觉数据 (12x8) ---")
+    for row in formatted_data.get('tip_touch', []):
+        print(row)
+
+    if finger_name == "大拇指":
+        if 'middle_touch' in formatted_data:
+            print(f"--- {finger_name} 指中触觉数据 (3x3) ---")
+            for row in formatted_data['middle_touch']:
+                print(row)
+        else:
+            print(f"{finger_name} 指中触觉数据缺失")
+
+    print(f"--- {finger_name} 指腹触觉数据 ({'12x8' if finger_name == '大拇指' else '10x8'}) ---")
+    for row in formatted_data.get('finger_pad', []):
+        print(row)
+
+def print_formatted_palm_data(palm_data):
+    if palm_data is None:
+        print("掌心数据格式化失败")
+        return
+
+    print("--- 掌心数据 (8x14) ---")
+    for row in palm_data:
+        print(row)
+
+def read_firmware_version(client):
+    """
+    读取固件版本，返回十六进制字符串 地址14
+    """
+    response = client.read_holding_registers(address=14, count=1)
+    if isinstance(response, ExceptionResponse) or response.isError():
+        print(f"读取固件版本失败：{response}")
+        return None
+    else:
+        reg_value = response.registers[0]
+        # 转换为16进制字符串
+        hex_str = format(reg_value, '04X')  # 4位，补零
+        return hex_str
+
+def read_multiple_registers():
+    client = ModbusTcpClient(MODBUS_IP, port=MODBUS_PORT)
+    client.connect()
+
+    try:
+        while True:
+            start_time = time.time()
+
+            # 读取各部分数据
+            pinky_register_values = read_register_range(
+                client,
+                TOUCH_SENSOR_BASE_ADDR_PINKY,
+                TOUCH_SENSOR_END_ADDR_PINKY
+            )
+            ring_register_values = read_register_range(
+                client,
+                TOUCH_SENSOR_BASE_ADDR_RING,
+                TOUCH_SENSOR_END_ADDR_RING
+            )
+            middle_register_values = read_register_range(
+                client,
+                TOUCH_SENSOR_BASE_ADDR_MIDDLE,
+                TOUCH_SENSOR_END_ADDR_MIDDLE
+            )
+            index_register_values = read_register_range(
+                client,
+                TOUCH_SENSOR_BASE_ADDR_INDEX,
+                TOUCH_SENSOR_END_ADDR_INDEX
+            )
+            thumb_register_values = read_register_range(
+                client,
+                TOUCH_SENSOR_BASE_ADDR_THUMB,
+                TOUCH_SENSOR_END_ADDR_THUMB
+            )
+            palm_register_values = read_register_range(
+                client,
+                TOUCH_SENSOR_BASE_ADDR_PALM,
+                TOUCH_SENSOR_END_ADDR_PALM
+            )
+
+            end_time = time.time()
+            frequency = 1 / (end_time - start_time) if end_time > start_time else float('inf')
+
+            # 格式化数据
+            pinky_formatted = format_finger_data("小拇指", pinky_register_values)
+            ring_formatted = format_finger_data("无名指", ring_register_values)
+            middle_formatted = format_finger_data("中指", middle_register_values)
+            index_formatted = format_finger_data("食指", index_register_values)
+            thumb_formatted = format_finger_data("大拇指", thumb_register_values)
+            palm_formatted = format_palm_data(palm_register_values)
+
+            # 打印格式化数据
+            print_formatted_finger_data("小拇指", pinky_formatted)
+            print_formatted_finger_data("无名指", ring_formatted)
+            print_formatted_finger_data("中指", middle_formatted)
+            print_formatted_finger_data("食指", index_formatted)
+            print_formatted_finger_data("大拇指", thumb_formatted)
+            print_formatted_palm_data(palm_formatted)
+
+            print(f"读取频率：{frequency:.2f} Hz")
+            print("\n" + "="*40 + "\n")
+
+            firmware_version_int = read_firmware_version(client)
+            if firmware_version_int is not None:
+                print(f"固件版本：{firmware_version_int}")
+
+    finally:
+        client.close()
+
+
+if __name__ == "__main__":
+    read_multiple_registers()