add oymotion_dexhand

cmvr-es/config/devices/dexhand/dexhand.pb.txt

@@ -1,4 +1,14 @@
 dexhand {
+  dexhands {
+    id: "right_hand_ap001"
+    oymotion_ap001 {
+      can_interface: "can0"
+      hand_id: 2
+      master_id: 1
+      command_timeout_ms: 255
+    }
+  }
+
   dexhands {
     id: "hand1"
     rh56dftp {

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

@@ -59,6 +59,13 @@ device_manager {
     }
 
 
+  devices {
+    id: "right_hand_ap001"
+    type: DEVICE_TYPE_DEXHAND
+    config_file: "devices/dexhand/dexhand.pb.txt"
+    enable: true
+  }
+
   devices {
     id: "hand2"
     type: DEVICE_TYPE_DEXHAND

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

@@ -27,6 +27,6 @@ task_manager {
     type: TASK_TYPE_QUIC_EDGE
     run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
     config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt"
-    enable: true
+    enable: false
   }
 }

cmvr-es/devices/dexhand/CMakeLists.txt

@@ -1,6 +1,7 @@
 add_subdirectory(rh56dftp_dexhand)
 add_subdirectory(px_6ax_gen3)
 add_subdirectory(zero_sim_touch_dexhand)
+add_subdirectory(oymotion_ap001)
 
 add_library(dexhand INTERFACE)
 
@@ -11,6 +12,7 @@ target_link_libraries(dexhand
         cmvr_es::device::rh56dftp_dexhand
         cmvr_es::device::px_6ax_gen3
         cmvr_es::device::zero_sim_touch_dexhand
+        cmvr_es::device::oymotion_ap001
         cmvr_es::proto
 )
 

cmvr-es/devices/dexhand/dexhand_factory.h

@@ -11,6 +11,7 @@
 #include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
 #include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h"
+#include "devices/dexhand/oymotion_ap001/include/oymotion_ap001.h"
 
 namespace cmvr::device {
 
@@ -37,6 +38,10 @@ public:
             return std::make_shared<ZeroSimTouchDexHand>(
                 backendWithId_(cfg.id(), cfg.zero_sim_touch()));
 
+        case config::DexHandDeviceConfig::kOymotionAp001:
+            return std::make_shared<OYMotionAP001>(
+                backendWithId_(cfg.id(), cfg.oymotion_ap001()));
+
         case config::DexHandDeviceConfig::BACKEND_NOT_SET:
         default:
         {

cmvr-es/devices/dexhand/oymotion_ap001/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_library(oymotion_ap001 STATIC src/oymotion_ap001.cpp)
+
+set(OHAND_SDK_ROOT "${PROJECT_SOURCE_DIR}/dependency/x86/third_party/ohand_serial_sdk")
+add_library(ohand_serial_sdk STATIC IMPORTED)
+set_target_properties(ohand_serial_sdk PROPERTIES
+    IMPORTED_LOCATION "${OHAND_SDK_ROOT}/lib/libROHSerialAPIStatic.a"
+    INTERFACE_INCLUDE_DIRECTORIES "${OHAND_SDK_ROOT}/include")
+
+target_include_directories(oymotion_ap001 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(oymotion_ap001 PRIVATE cmvr_es::hardware cmvr_es::proto ohand_serial_sdk pthread)
+
+add_library(cmvr_es::device::oymotion_ap001 ALIAS oymotion_ap001)

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

@@ -0,0 +1,64 @@
+#ifndef CMVR_ES_OYMOTION_AP001_H
+#define CMVR_ES_OYMOTION_AP001_H
+
+#include <array>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
+#include "devices/dexhand/abstract_dexhand.h"
+
+namespace cmvr::device {
+
+class OYMotionAP001 final : public AbstractDexHand {
+public:
+    static constexpr size_t MOTOR_COUNT = 6;
+    explicit OYMotionAP001(const config::OYMotionAP001Config& config);
+    ~OYMotionAP001() override;
+    std::string typeName() const override { return "OYMotionAP001"; }
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+    Status state() const override;
+    std::string lastError() const override;
+    void getState(DexHandState& state) override;
+    void setAngles(const std::vector<int>& values) override;
+    void setPositions(const std::vector<int>& values) override;
+    void setVelocities(const std::vector<int>& values) override;
+    void setForce(const std::vector<int>& values) override;
+    void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
+    std::vector<TactileRegionData> getSensorData() override;
+    TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
+    ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
+    ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
+
+private:
+    static uint32_t sdkMillis();
+    static void sdkDelay(uint32_t milliseconds);
+    static void sdkSend(uint8_t address, uint8_t* data, uint8_t size, void* context);
+    static void sdkReceive(void* context);
+    static OYMotionAP001* fromSdkContext(void* context);
+    bool openSocket();
+    bool checkSdkResult(uint8_t result, uint8_t remote_error, const char* operation);
+    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;
+
+    config::OYMotionAP001Config config_;
+    std::string can_interface_;
+    uint8_t hand_id_{2};
+    uint8_t master_id_{1};
+    int socket_fd_{-1};
+    void* sdk_context_{nullptr};
+    mutable std::mutex state_mutex_;
+    mutable std::mutex io_mutex_;
+    Status lifecycle_state_{Status::CREATED};
+    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_{};
+    std::vector<TactileRegionKey> tactile_regions_;
+};
+
+} // namespace cmvr::device
+#endif

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

@@ -0,0 +1,166 @@
+#include "devices/dexhand/oymotion_ap001/include/oymotion_ap001.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cstring>
+#include <stdexcept>
+#include <thread>
+#include <fcntl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include "OHandSerialAPI.h"
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+namespace {
+constexpr int kPositionInputMaximum = 2000;
+constexpr int kAngleInputMaximum = 1000;
+constexpr int kVelocityInputMaximum = 1000;
+constexpr int kForceInputMaximum = 3000;
+}
+
+OYMotionAP001::OYMotionAP001(const config::OYMotionAP001Config& config) : config_(config) {
+    id_ = config.id();
+    can_interface_ = config.can_interface().empty() ? "can0" : config.can_interface();
+    if (config.hand_id() > 0 && config.hand_id() <= 0xff) hand_id_ = static_cast<uint8_t>(config.hand_id());
+    if (config.master_id() > 0 && config.master_id() <= 0xff) master_id_ = static_cast<uint8_t>(config.master_id());
+}
+
+OYMotionAP001::~OYMotionAP001() { stop(); }
+uint32_t OYMotionAP001::sdkMillis() {
+    static const auto start = std::chrono::steady_clock::now();
+    return static_cast<uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start).count());
+}
+void OYMotionAP001::sdkDelay(uint32_t milliseconds) { std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); }
+OYMotionAP001* OYMotionAP001::fromSdkContext(void* context) { return context ? *static_cast<OYMotionAP001**>(context) : nullptr; }
+
+void OYMotionAP001::sdkSend(uint8_t address, uint8_t* data, uint8_t size, void* context) {
+    auto* self = fromSdkContext(context);
+    if (!self || self->socket_fd_ < 0) return;
+    for (uint8_t offset = 0; offset < size; offset += CAN_MAX_DLEN) {
+        can_frame frame{};
+        frame.can_id = address;
+        frame.can_dlc = static_cast<__u8>(std::min<int>(CAN_MAX_DLEN, size - offset));
+        std::memcpy(frame.data, data + offset, frame.can_dlc);
+        if (write(self->socket_fd_, &frame, sizeof(frame)) != sizeof(frame)) {
+            self->setFault("[OYMotionAP001] failed to write CAN frame on " + self->can_interface_);
+            return;
+        }
+    }
+}
+
+void OYMotionAP001::sdkReceive(void* context) {
+    auto* self = fromSdkContext(context);
+    if (!self || self->socket_fd_ < 0) return;
+    can_frame frame{};
+    while (read(self->socket_fd_, &frame, sizeof(frame)) == sizeof(frame)) {
+        if ((frame.can_id & CAN_EFF_FLAG) || (frame.can_id & CAN_RTR_FLAG)) continue;
+        for (uint8_t i = 0; i < frame.can_dlc; ++i) HAND_OnData(context, frame.data[i]);
+    }
+}
+
+bool OYMotionAP001::openSocket() {
+    socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    if (socket_fd_ < 0) return false;
+    ifreq request{};
+    std::strncpy(request.ifr_name, can_interface_.c_str(), IFNAMSIZ - 1);
+    if (ioctl(socket_fd_, SIOCGIFINDEX, &request) < 0) return false;
+    sockaddr_can address{};
+    address.can_family = AF_CAN;
+    address.can_ifindex = request.ifr_ifindex;
+    if (bind(socket_fd_, reinterpret_cast<sockaddr*>(&address), sizeof(address)) < 0) return false;
+    const int flags = fcntl(socket_fd_, F_GETFL, 0);
+    return flags >= 0 && fcntl(socket_fd_, F_SETFL, flags | O_NONBLOCK) == 0;
+}
+
+bool OYMotionAP001::init() {
+    std::lock_guard<std::mutex> io_lock(io_mutex_);
+    if (sdk_context_) return true;
+    if (!openSocket()) { setFault("[OYMotionAP001] cannot open " + can_interface_ + "; ensure it is UP at 1 Mbps"); return false; }
+    HAND_SetTimerFunction(sdkMillis, sdkDelay);
+    sdk_context_ = HAND_CreateContext(this, HAND_PROTOCOL_UART, master_id_, sdkSend, sdkReceive);
+    if (!sdk_context_) { setFault("[OYMotionAP001] HAND_CreateContext failed"); return false; }
+    HAND_SetCommandTimeOut(sdk_context_, static_cast<uint16_t>(config_.command_timeout_ms() > 0 ? std::min<uint32_t>(config_.command_timeout_ms(), 65535) : 255));
+    uint8_t major = 0, minor = 0, remote_error = 0;
+    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);
+    return true;
+}
+
+bool OYMotionAP001::start() { if (!init()) return false; setState(Status::STREAMING); return true; }
+bool OYMotionAP001::stop() {
+    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; }
+    if (state() != Status::FAULT) setState(Status::STOPPED);
+    return true;
+}
+AbstractDexHand::Status OYMotionAP001::state() const { std::lock_guard<std::mutex> lock(state_mutex_); return lifecycle_state_; }
+std::string OYMotionAP001::lastError() const { std::lock_guard<std::mutex> lock(state_mutex_); return last_error_; }
+
+void OYMotionAP001::getState(DexHandState& output) {
+    DexHandState next{};
+    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{};
+        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;
+    }
+    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].speed = static_cast<int>(speeds_[i]) * kVelocityInputMaximum / 255;
+    }
+    const auto error = lastError();
+    if (!error.empty()) next.hands[0].error_message.push_back(error);
+    output = std::move(next);
+}
+
+std::array<uint16_t, OYMotionAP001::MOTOR_COUNT> OYMotionAP001::normalizedPositions(const std::vector<int>& values, int maximum) const {
+    if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 values");
+    std::array<uint16_t, MOTOR_COUNT> output{};
+    for (size_t i = 0; i < MOTOR_COUNT; ++i) output[i] = static_cast<uint16_t>(static_cast<uint32_t>(std::clamp(values[i], 0, maximum)) * 65535U / maximum);
+    return output;
+}
+
+void OYMotionAP001::setPositions(const std::vector<int>& values) {
+    auto positions = normalizedPositions(values, kPositionInputMaximum);
+    std::lock_guard<std::mutex> 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");
+}
+void OYMotionAP001::setAngles(const std::vector<int>& values) {
+    auto positions = normalizedPositions(values, kAngleInputMaximum);
+    std::lock_guard<std::mutex> 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)");
+}
+void OYMotionAP001::setVelocities(const std::vector<int>& values) {
+    if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 velocities");
+    std::lock_guard<std::mutex> lock(io_mutex_);
+    for (size_t i = 0; i < MOTOR_COUNT; ++i) speeds_[i] = static_cast<uint8_t>(std::clamp(values[i], 0, kVelocityInputMaximum) * 255 / kVelocityInputMaximum);
+}
+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; }
+}
+
+void OYMotionAP001::setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) { tactile_regions_ = regions; }
+std::vector<AbstractDexHand::TactileRegionData> OYMotionAP001::getSensorData() { return {}; }
+AbstractDexHand::TactileRegionData OYMotionAP001::getSensorData(FingerType, TactileRegion) { return {}; }
+AbstractDexHand::ResultantForce OYMotionAP001::getResultantForce(FingerType, TactileRegion) { return {}; }
+AbstractDexHand::ForceNewtons OYMotionAP001::getResultantForceNewtons(FingerType, TactileRegion) { return {}; }
+
+bool OYMotionAP001::checkSdkResult(uint8_t result, uint8_t remote_error, const char* operation) {
+    if (result == HAND_RESP_SUCCESS) return true;
+    setFault("[OYMotionAP001] " + std::string(operation) + " failed: result=" + std::to_string(result) + ", remote_error=" + std::to_string(remote_error));
+    return false;
+}
+void OYMotionAP001::setFault(const std::string& message) { { std::lock_guard<std::mutex> lock(state_mutex_); lifecycle_state_ = Status::FAULT; last_error_ = message; } CMVR_LOG(ERROR) << message; }
+void OYMotionAP001::setState(Status next) { std::lock_guard<std::mutex> lock(state_mutex_); lifecycle_state_ = next; if (next == Status::INITIALIZED || next == Status::STREAMING) last_error_.clear(); }
+} // namespace cmvr::device

protos/cmvr/config/dexhand_config/dexhand_config.proto

@@ -45,6 +45,14 @@ message ZeroSimTouchDexHand {
     string id = 1;
 }
 
+message OYMotionAP001Config {
+    string id = 1;
+    string can_interface = 2;
+    uint32 hand_id = 3;
+    uint32 master_id = 4;
+    uint32 command_timeout_ms = 5;
+}
+
 
 
 message DexHandDeviceConfig {
@@ -55,6 +63,7 @@ message DexHandDeviceConfig {
         RH56DFTPDexHandConfig rh56dftp = 10;
         PX6AXGen3 px_6ax_gen3 = 11;
         ZeroSimTouchDexHand zero_sim_touch = 12;
+        OYMotionAP001Config oymotion_ap001 = 13;
     }
 }