add dexhand sensordata

cmvr-es/config/manager/device_manager.pb.txt

@@ -203,21 +203,21 @@ device_manager {
     id: "dual_arm_ethercat_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/dual_arm_ethercat_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "right_ethercat_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/ethercat_dual_arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "left_ethercat_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/ethercat_dual_arm.pb.txt"
-    enable: true
+    enable: false
   }
 
 }

cmvr-es/config/manager/task_manager.pb.txt

@@ -5,7 +5,7 @@ task_manager {
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
     config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: true
+    enable: false
   }
   tasks {
     id: "grpc_server"

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

@@ -2,8 +2,10 @@
 #define CMVR_ES_OYMOTION_AP001_H
 
 #include <array>
+#include <atomic>
 #include <mutex>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include "cmvr/config/dexhand_config/dexhand_config.pb.h"
@@ -41,6 +43,9 @@ private:
     static OYMotionAP001* fromSdkContext(void* context);
     bool openSocket();
     bool checkSdkResult(uint8_t result, uint8_t remote_error, const char* operation);
+    void startTactilePolling();
+    void stopTactilePolling();
+    void tactilePollingLoop();
     void setFault(const std::string& message);
     void setState(Status state);
     std::array<uint16_t, MOTOR_COUNT> normalizedPositions(const std::vector<int>& values, int maximum) const;
@@ -57,7 +62,16 @@ private:
     std::string last_error_;
     std::array<uint8_t, MOTOR_COUNT> speeds_{{255, 255, 255, 255, 255, 255}};
     std::array<uint16_t, MOTOR_COUNT> current_positions_{};
+    // OHand SDK reports the physical joint angle in degrees * 100.
+    std::array<int16_t, MOTOR_COUNT> current_angles_{};
     std::vector<TactileRegionKey> tactile_regions_;
+    static constexpr size_t INDEX_TACTILE_COUNT = 60;
+    std::atomic<bool> tactile_running_{false};
+    std::thread tactile_thread_;
+    mutable std::mutex tactile_mutex_;
+    std::array<TactilePoint, INDEX_TACTILE_COUNT> index_tactile_{};
+    bool index_tactile_valid_{false};
+    uint64_t index_tactile_sequence_{0};
 };
 
 } // namespace cmvr::device

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

@@ -21,6 +21,43 @@ constexpr int kPositionInputMaximum = 2000;
 constexpr int kAngleInputMaximum = 1000;
 constexpr int kVelocityInputMaximum = 1000;
 constexpr int kForceInputMaximum = 3000;
+constexpr uint8_t kIndexTactileSensorId = 1;
+constexpr int kIndexTactileRows = 12;
+constexpr int kIndexTactileCols = 5;
+constexpr auto kTactilePollingPeriod = std::chrono::microseconds(6667);
+
+// cmvr-es exposes dex-hand joints as:
+// pinky, ring, middle, index, thumb flexion, thumb rotation.
+// OHand SDK uses:
+// thumb flexion, index, middle, ring, pinky, thumb rotation.
+constexpr std::array<size_t, OYMotionAP001::MOTOR_COUNT> kApiToSdkMotor{
+    4, 3, 2, 1, 0, 5};
+
+template <typename T>
+std::array<T, OYMotionAP001::MOTOR_COUNT> apiToSdkMotorOrder(
+    const std::array<T, OYMotionAP001::MOTOR_COUNT>& api_values) {
+    std::array<T, OYMotionAP001::MOTOR_COUNT> sdk_values{};
+    for (size_t api_index = 0; api_index < kApiToSdkMotor.size(); ++api_index) {
+        sdk_values[kApiToSdkMotor[api_index]] = api_values[api_index];
+    }
+    return sdk_values;
+}
+
+template <typename T>
+std::array<T, OYMotionAP001::MOTOR_COUNT> sdkToApiMotorOrder(
+    const std::array<T, OYMotionAP001::MOTOR_COUNT>& sdk_values) {
+    std::array<T, OYMotionAP001::MOTOR_COUNT> api_values{};
+    for (size_t api_index = 0; api_index < kApiToSdkMotor.size(); ++api_index) {
+        api_values[api_index] = sdk_values[kApiToSdkMotor[api_index]];
+    }
+    return api_values;
+}
+
+struct TactileSnapshot {
+    std::array<AbstractDexHand::TactilePoint,
+               static_cast<size_t>(kIndexTactileRows * kIndexTactileCols)>
+        values{};
+};
 }
 
 OYMotionAP001::OYMotionAP001(const config::OYMotionAP001Config& config) : config_(config) {
@@ -89,11 +126,21 @@ bool OYMotionAP001::init() {
     if (!checkSdkResult(HAND_GetProtocolVersion(sdk_context_, hand_id_, &major, &minor, &remote_error), remote_error, "HAND_GetProtocolVersion")) return false;
     CMVR_LOG(INFO) << "[OYMotionAP001] connected: id=" << id_ << ", interface=" << can_interface_ << ", hand_id=" << static_cast<int>(hand_id_) << ", protocol=" << static_cast<int>(major) << "." << static_cast<int>(minor);
     setState(Status::INITIALIZED);
+    // DeviceManager constructs and initializes devices during startup, but the
+    // application does not call DeviceManager::start(). Start the independent
+    // tactile sampler as soon as the AP001 connection is ready.
+    startTactilePolling();
     return true;
 }
 
-bool OYMotionAP001::start() { if (!init()) return false; setState(Status::STREAMING); return true; }
+bool OYMotionAP001::start() {
+    if (!init()) return false;
+    setState(Status::STREAMING);
+    startTactilePolling();
+    return true;
+}
 bool OYMotionAP001::stop() {
+    stopTactilePolling();
     std::lock_guard<std::mutex> io_lock(io_mutex_);
     if (sdk_context_) { HAND_FreeContext(sdk_context_); sdk_context_ = nullptr; }
     if (socket_fd_ >= 0) { close(socket_fd_); socket_fd_ = -1; }
@@ -108,13 +155,49 @@ void OYMotionAP001::getState(DexHandState& output) {
     next.is_initialized = state() == Status::INITIALIZED || state() == Status::STREAMING;
     std::lock_guard<std::mutex> io_lock(io_mutex_);
     if (sdk_context_) {
-        std::array<uint16_t, MOTOR_COUNT> targets{}, positions{};
+        std::array<uint16_t, MOTOR_COUNT> target_positions{};
-        uint8_t count = 0, remote_error = 0;
+        std::array<uint16_t, MOTOR_COUNT> current_positions{};
-        if (HAND_GetFingerPosAll(sdk_context_, hand_id_, targets.data(), positions.data(), &count, &remote_error) == HAND_RESP_SUCCESS) current_positions_ = positions;
+        // The SDK treats motor_cnt as an in/out buffer-capacity parameter.
+        uint8_t position_count = static_cast<uint8_t>(MOTOR_COUNT);
+        uint8_t position_remote_error = 0;
+        const uint8_t position_result = HAND_GetFingerPosAll(
+            sdk_context_, hand_id_, target_positions.data(), current_positions.data(),
+            &position_count, &position_remote_error);
+
+        if (position_result != HAND_RESP_SUCCESS) {
+            CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerPosAll failed: result="
+                            << static_cast<int>(position_result)
+                            << ", remote_error=" << static_cast<int>(position_remote_error);
+        } else if (position_count != MOTOR_COUNT) {
+            CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerPosAll returned unexpected motor count: expected="
+                            << MOTOR_COUNT << ", actual=" << static_cast<int>(position_count);
+        } else {
+            current_positions_ = sdkToApiMotorOrder(current_positions);
+        }
+
+        std::array<int16_t, MOTOR_COUNT> target_angles{};
+        std::array<int16_t, MOTOR_COUNT> current_angles{};
+        // The SDK treats motor_cnt as an in/out buffer-capacity parameter.
+        uint8_t angle_count = static_cast<uint8_t>(MOTOR_COUNT);
+        uint8_t angle_remote_error = 0;
+        const uint8_t angle_result = HAND_GetFingerAngleAll(
+            sdk_context_, hand_id_, target_angles.data(), current_angles.data(),
+            &angle_count, &angle_remote_error);
+
+        if (angle_result != HAND_RESP_SUCCESS) {
+            CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerAngleAll failed: result="
+                            << static_cast<int>(angle_result)
+                            << ", remote_error=" << static_cast<int>(angle_remote_error);
+        } else if (angle_count != MOTOR_COUNT) {
+            CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerAngleAll returned unexpected motor count: expected="
+                            << MOTOR_COUNT << ", actual=" << static_cast<int>(angle_count);
+        } else {
+            current_angles_ = sdkToApiMotorOrder(current_angles);
+        }
     }
     for (size_t i = 0; i < MOTOR_COUNT; ++i) {
         next.hands[i].position = static_cast<int>(current_positions_[i]) * kPositionInputMaximum / 65535;
-        next.hands[i].angle = static_cast<int>(current_positions_[i]) * kAngleInputMaximum / 65535;
+        next.hands[i].angle = current_angles_[i];
         next.hands[i].speed = static_cast<int>(speeds_[i]) * kVelocityInputMaximum / 255;
     }
     const auto error = lastError();
@@ -130,14 +213,20 @@ std::array<uint16_t, OYMotionAP001::MOTOR_COUNT> OYMotionAP001::normalizedPositi
 }
 
 void OYMotionAP001::setPositions(const std::vector<int>& values) {
-    auto positions = normalizedPositions(values, kPositionInputMaximum);
+    auto positions = apiToSdkMotorOrder(
-    std::lock_guard<std::mutex> lock(io_mutex_); uint8_t remote_error = 0;
+        normalizedPositions(values, kPositionInputMaximum));
-    checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds_.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll");
+    std::lock_guard<std::mutex> lock(io_mutex_);
+    auto speeds = apiToSdkMotorOrder(speeds_);
+    uint8_t remote_error = 0;
+    checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll");
 }
 void OYMotionAP001::setAngles(const std::vector<int>& values) {
-    auto positions = normalizedPositions(values, kAngleInputMaximum);
+    auto positions = apiToSdkMotorOrder(
-    std::lock_guard<std::mutex> lock(io_mutex_); uint8_t remote_error = 0;
+        normalizedPositions(values, kAngleInputMaximum));
-    checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds_.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll(angle normalization)");
+    std::lock_guard<std::mutex> lock(io_mutex_);
+    auto speeds = apiToSdkMotorOrder(speeds_);
+    uint8_t remote_error = 0;
+    checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll(angle normalization)");
 }
 void OYMotionAP001::setVelocities(const std::vector<int>& values) {
     if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 velocities");
@@ -147,14 +236,136 @@ void OYMotionAP001::setVelocities(const std::vector<int>& values) {
 void OYMotionAP001::setForce(const std::vector<int>& values) {
     if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 force values");
     std::lock_guard<std::mutex> lock(io_mutex_);
-    for (size_t i = 0; i < MOTOR_COUNT; ++i) { uint8_t remote_error = 0; if (!checkSdkResult(HAND_SetFingerForceTarget(sdk_context_, hand_id_, static_cast<uint8_t>(i), static_cast<uint16_t>(std::clamp(values[i], 0, kForceInputMaximum)), &remote_error), remote_error, "HAND_SetFingerForceTarget")) return; }
+    for (size_t api_index = 0; api_index < MOTOR_COUNT; ++api_index) {
+        uint8_t remote_error = 0;
+        if (!checkSdkResult(
+                HAND_SetFingerForceTarget(
+                    sdk_context_, hand_id_,
+                    static_cast<uint8_t>(kApiToSdkMotor[api_index]),
+                    static_cast<uint16_t>(std::clamp(
+                        values[api_index], 0, kForceInputMaximum)),
+                    &remote_error),
+                remote_error, "HAND_SetFingerForceTarget")) {
+            return;
+        }
+    }
 }
 
-void OYMotionAP001::setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) { tactile_regions_ = regions; }
+void OYMotionAP001::setTactilePollingRegions(
-std::vector<AbstractDexHand::TactileRegionData> OYMotionAP001::getSensorData() { return {}; }
+    const std::vector<TactileRegionKey>& regions) {
-AbstractDexHand::TactileRegionData OYMotionAP001::getSensorData(FingerType, TactileRegion) { return {}; }
+    std::lock_guard<std::mutex> lock(io_mutex_);
-AbstractDexHand::ResultantForce OYMotionAP001::getResultantForce(FingerType, TactileRegion) { return {}; }
+    tactile_regions_ = regions;
-AbstractDexHand::ForceNewtons OYMotionAP001::getResultantForceNewtons(FingerType, TactileRegion) { return {}; }
+}
+
+void OYMotionAP001::startTactilePolling() {
+    bool expected = false;
+    if (!tactile_running_.compare_exchange_strong(expected, true)) return;
+    tactile_thread_ = std::thread(&OYMotionAP001::tactilePollingLoop, this);
+}
+
+void OYMotionAP001::stopTactilePolling() {
+    tactile_running_.store(false);
+    if (tactile_thread_.joinable()) tactile_thread_.join();
+}
+
+void OYMotionAP001::tactilePollingLoop() {
+    auto next_poll = std::chrono::steady_clock::now();
+    auto stats_start = next_poll;
+    uint64_t successful_reads = 0;
+    uint64_t failed_reads = 0;
+
+    while (tactile_running_.load()) {
+        std::array<uint8_t, INDEX_TACTILE_COUNT> raw_force{};
+        uint8_t entry_count = static_cast<uint8_t>(raw_force.size());
+        uint8_t remote_error = 0;
+        uint8_t result = HAND_RESP_TIMEOUT;
+
+        {
+            std::lock_guard<std::mutex> lock(io_mutex_);
+            if (sdk_context_) {
+                result = HAND_GetFingerForce(
+                    sdk_context_, hand_id_, kIndexTactileSensorId, &entry_count,
+                    raw_force.data(), &remote_error);
+            }
+        }
+
+        if (result == HAND_RESP_SUCCESS && entry_count == raw_force.size()) {
+            std::lock_guard<std::mutex> lock(tactile_mutex_);
+            for (size_t point = 0; point < raw_force.size(); ++point) {
+                index_tactile_[point] = TactilePoint::fromFz(raw_force[point]);
+            }
+            index_tactile_valid_ = true;
+            ++index_tactile_sequence_;
+            ++successful_reads;
+        } else {
+            ++failed_reads;
+        }
+
+        const auto now = std::chrono::steady_clock::now();
+        const auto stats_elapsed = now - stats_start;
+        if (stats_elapsed >= std::chrono::seconds(5)) {
+            const double seconds =
+                std::chrono::duration<double>(stats_elapsed).count();
+            CMVR_LOG(INFO) << "[OYMotionAP001] index tactile acquisition: rate="
+                           << successful_reads / seconds << " Hz, successful="
+                           << successful_reads << ", failed=" << failed_reads;
+            stats_start = now;
+            successful_reads = 0;
+            failed_reads = 0;
+        }
+
+        next_poll += kTactilePollingPeriod;
+        if (next_poll > now) {
+            std::this_thread::sleep_until(next_poll);
+        } else {
+            next_poll = now;
+        }
+    }
+}
+
+std::vector<AbstractDexHand::TactileRegionData> OYMotionAP001::getSensorData() {
+    auto snapshot = std::make_shared<TactileSnapshot>();
+    {
+        std::lock_guard<std::mutex> lock(tactile_mutex_);
+        if (!index_tactile_valid_) return {};
+        snapshot->values = index_tactile_;
+    }
+    return {TactileRegionData{
+        FingerType::INDEX, TactileRegion::PAD,
+        TactileMatrixView{snapshot->values.data(), kIndexTactileRows,
+                          kIndexTactileCols},
+        "食指触觉", snapshot}};
+}
+
+AbstractDexHand::TactileRegionData OYMotionAP001::getSensorData(
+    FingerType finger, TactileRegion region) {
+    if (finger != FingerType::INDEX || region != TactileRegion::PAD) return {};
+    auto regions = getSensorData();
+    return regions.empty() ? TactileRegionData{} : regions.front();
+}
+
+AbstractDexHand::ResultantForce OYMotionAP001::getResultantForce(
+    FingerType finger, TactileRegion region) {
+    const auto tactile_data = getSensorData(finger, region);
+    if (!tactile_data.valid()) return {};
+
+    ResultantForce resultant{};
+    for (int row = 0; row < tactile_data.view.rows; ++row) {
+        const auto* values = tactile_data.view.rowData(row);
+        for (int col = 0; col < tactile_data.view.cols; ++col) {
+            resultant.fx += values[col].fx;
+            resultant.fy += values[col].fy;
+            resultant.fz += values[col].fz;
+        }
+    }
+    return resultant;
+}
+
+AbstractDexHand::ForceNewtons OYMotionAP001::getResultantForceNewtons(
+    FingerType, TactileRegion) {
+    throw std::runtime_error(
+        "OYMotionAP001 tactile values are raw sensor counts; no newton calibration is available.");
+}
 
 bool OYMotionAP001::checkSdkResult(uint8_t result, uint8_t remote_error, const char* operation) {
     if (result == HAND_RESP_SUCCESS) return true;

cmvr-es/service/CMakeLists.txt

@@ -20,6 +20,7 @@ target_link_libraries(service PRIVATE
         cmvr_es::proto
         osqp
         cmvr_es::device_manager
+        cmvr_es::device::oymotion_ap001
         cmvr_es::task_manager
         cmvr_es::algorithms::controller
         cmvr_es::task

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

@@ -11,6 +11,7 @@
 #include <thread>
 #include <vector>
 
+#include "devices/dexhand/oymotion_ap001/include/oymotion_ap001.h"
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
 #include "service/grpc/action/include/control_command_arbiter.h"
 #include "service/grpc/action/include/control_command_guard.h"
@@ -470,6 +471,10 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
         }
         maybeConfigureRh56FullTactilePolling(dev);
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
+        const auto stream_interval =
+            std::dynamic_pointer_cast<OYMotionAP001>(dev)
+                ? std::chrono::microseconds(6667)
+                : std::chrono::microseconds(33000);
 
         while (!context->IsCancelled())
         {
@@ -483,7 +488,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
                 break;
             }
 
-            std::this_thread::sleep_for(std::chrono::milliseconds(33));
+            std::this_thread::sleep_for(stream_interval);
         }
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id;
         return grpc::Status::OK;