diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 6bbd6cd6..f3a7d654 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/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 } } diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index b9261829..3d507b94 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/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" diff --git a/cmvr-es/devices/dexhand/oymotion_ap001/include/oymotion_ap001.h b/cmvr-es/devices/dexhand/oymotion_ap001/include/oymotion_ap001.h index 2597949c..ca9bfaa2 100644 --- a/cmvr-es/devices/dexhand/oymotion_ap001/include/oymotion_ap001.h +++ b/cmvr-es/devices/dexhand/oymotion_ap001/include/oymotion_ap001.h @@ -2,8 +2,10 @@ #define CMVR_ES_OYMOTION_AP001_H #include +#include #include #include +#include #include #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 normalizedPositions(const std::vector& values, int maximum) const; @@ -57,7 +62,16 @@ private: std::string last_error_; std::array speeds_{{255, 255, 255, 255, 255, 255}}; std::array current_positions_{}; + // OHand SDK reports the physical joint angle in degrees * 100. + std::array current_angles_{}; std::vector tactile_regions_; + static constexpr size_t INDEX_TACTILE_COUNT = 60; + std::atomic tactile_running_{false}; + std::thread tactile_thread_; + mutable std::mutex tactile_mutex_; + std::array index_tactile_{}; + bool index_tactile_valid_{false}; + uint64_t index_tactile_sequence_{0}; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/dexhand/oymotion_ap001/src/oymotion_ap001.cpp b/cmvr-es/devices/dexhand/oymotion_ap001/src/oymotion_ap001.cpp index 8496bf98..4eb24162 100644 --- a/cmvr-es/devices/dexhand/oymotion_ap001/src/oymotion_ap001.cpp +++ b/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 kApiToSdkMotor{ + 4, 3, 2, 1, 0, 5}; + +template +std::array apiToSdkMotorOrder( + const std::array& api_values) { + std::array 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 +std::array sdkToApiMotorOrder( + const std::array& sdk_values) { + std::array 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(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(hand_id_) << ", protocol=" << static_cast(major) << "." << static_cast(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 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 io_lock(io_mutex_); if (sdk_context_) { - std::array targets{}, positions{}; - uint8_t count = 0, remote_error = 0; - if (HAND_GetFingerPosAll(sdk_context_, hand_id_, targets.data(), positions.data(), &count, &remote_error) == HAND_RESP_SUCCESS) current_positions_ = positions; + std::array target_positions{}; + std::array current_positions{}; + // The SDK treats motor_cnt as an in/out buffer-capacity parameter. + uint8_t position_count = static_cast(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(position_result) + << ", remote_error=" << static_cast(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(position_count); + } else { + current_positions_ = sdkToApiMotorOrder(current_positions); + } + + std::array target_angles{}; + std::array current_angles{}; + // The SDK treats motor_cnt as an in/out buffer-capacity parameter. + uint8_t angle_count = static_cast(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(angle_result) + << ", remote_error=" << static_cast(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(angle_count); + } else { + current_angles_ = sdkToApiMotorOrder(current_angles); + } } for (size_t i = 0; i < MOTOR_COUNT; ++i) { next.hands[i].position = static_cast(current_positions_[i]) * kPositionInputMaximum / 65535; - next.hands[i].angle = static_cast(current_positions_[i]) * kAngleInputMaximum / 65535; + next.hands[i].angle = current_angles_[i]; next.hands[i].speed = static_cast(speeds_[i]) * kVelocityInputMaximum / 255; } const auto error = lastError(); @@ -130,14 +213,20 @@ std::array OYMotionAP001::normalizedPositi } void OYMotionAP001::setPositions(const std::vector& values) { - auto positions = normalizedPositions(values, kPositionInputMaximum); - std::lock_guard lock(io_mutex_); uint8_t remote_error = 0; - checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds_.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll"); + auto positions = apiToSdkMotorOrder( + normalizedPositions(values, kPositionInputMaximum)); + std::lock_guard 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& values) { - auto positions = normalizedPositions(values, kAngleInputMaximum); - std::lock_guard lock(io_mutex_); 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)"); + auto positions = apiToSdkMotorOrder( + normalizedPositions(values, kAngleInputMaximum)); + std::lock_guard 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& 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& values) { void OYMotionAP001::setForce(const std::vector& values) { if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 force values"); std::lock_guard 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(i), static_cast(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(kApiToSdkMotor[api_index]), + static_cast(std::clamp( + values[api_index], 0, kForceInputMaximum)), + &remote_error), + remote_error, "HAND_SetFingerForceTarget")) { + return; + } + } } -void OYMotionAP001::setTactilePollingRegions(const std::vector& regions) { tactile_regions_ = regions; } -std::vector OYMotionAP001::getSensorData() { return {}; } -AbstractDexHand::TactileRegionData OYMotionAP001::getSensorData(FingerType, TactileRegion) { return {}; } -AbstractDexHand::ResultantForce OYMotionAP001::getResultantForce(FingerType, TactileRegion) { return {}; } -AbstractDexHand::ForceNewtons OYMotionAP001::getResultantForceNewtons(FingerType, TactileRegion) { return {}; } +void OYMotionAP001::setTactilePollingRegions( + const std::vector& regions) { + std::lock_guard lock(io_mutex_); + tactile_regions_ = regions; +} + +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 raw_force{}; + uint8_t entry_count = static_cast(raw_force.size()); + uint8_t remote_error = 0; + uint8_t result = HAND_RESP_TIMEOUT; + + { + std::lock_guard 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 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(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 OYMotionAP001::getSensorData() { + auto snapshot = std::make_shared(); + { + std::lock_guard 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; diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index d7db70c8..31524a06 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/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 diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index b672e233..dfe59861 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -11,6 +11,7 @@ #include #include +#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(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;