From 149d8cdb5dc2b2cd10c2b570140115a5927b2a78 Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 18 Sep 2026 13:51:13 +0800 Subject: [PATCH] fix(touch): validate tactile readings and use newton thresholds --- cmvr-es/config/devices/dexhand/dexhand.pb.txt | 2 + .../touch_screen_task.pb.txt | 2 +- .../touch_screen_task_mujoco.pb.txt | 2 +- cmvr-es/devices/dexhand/abstract_dexhand.h | 17 + .../dexhand/px_6ax_gen3/CMakeLists.txt | 9 + cmvr-es/devices/dexhand/px_6ax_gen3/README.md | 51 +++ .../dexhand/px_6ax_gen3/include/px_6ax_gen3.h | 12 +- .../dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp | 431 ++++++++---------- .../px_6ax_gen3/src/px_6ax_gen3_real_test.cpp | 201 ++++++++ .../px_6ax_gen3/src/px_6ax_gen3_test.cpp | 318 +++++++++++++ .../include/zero_sim_touch_dexhand.h | 1 + .../src/zero_sim_touch_dexhand.cpp | 5 + .../include/touch_screen_task.h | 5 +- .../src/touch_screen_task.cpp | 17 +- .../dexhand_config/dexhand_config.proto | 3 + .../touch_screen_task_config.proto | 2 + script/test_px_6ax_gen3.sh | 12 + 17 files changed, 847 insertions(+), 243 deletions(-) create mode 100644 cmvr-es/devices/dexhand/px_6ax_gen3/README.md create mode 100644 cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp create mode 100644 cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp create mode 100755 script/test_px_6ax_gen3.sh diff --git a/cmvr-es/config/devices/dexhand/dexhand.pb.txt b/cmvr-es/config/devices/dexhand/dexhand.pb.txt index aa6a7497..c1730107 100644 --- a/cmvr-es/config/devices/dexhand/dexhand.pb.txt +++ b/cmvr-es/config/devices/dexhand/dexhand.pb.txt @@ -28,6 +28,8 @@ dexhand { resultant_length: 3 poll_interval_ms: 5 response_timeout_ms: 200 + # 触觉数据最大有效期；超过此时间报不可用，不继续返回旧力值。 + max_sample_age_ms: 50 response_header_bytes: 14 tactile_rows: 1 tactile_cols: 51 diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt index 2fdafe8b..2d472a2d 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt @@ -102,7 +102,7 @@ touch_screen_task { finger: TOUCH_SCREEN_FINGER_TYPE_INDEX region: TOUCH_SCREEN_TACTILE_REGION_TIP criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ - force_threshold: 1.0 + force_threshold: 0.1 # N } dwell_time_s: 0.0 } diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt index 2c60e3a8..c4878e59 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -111,7 +111,7 @@ touch_screen_task { finger: TOUCH_SCREEN_FINGER_TYPE_INDEX region: TOUCH_SCREEN_TACTILE_REGION_TIP criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ - force_threshold: 1.0 + force_threshold: 0.1 # N } dwell_time_s: 0.0 } diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 1cf8b9c7..3a21be43 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,17 @@ namespace cmvr::device { using ResultantForce = TactilePoint; + // Physical force, separate from device-specific integer tactile data. + struct ForceNewtons { + double fx{0.0}; + double fy{0.0}; + double fz{0.0}; + + double magnitude() const { + return std::hypot(fx, fy, fz); + } + }; + enum class FingerType { PINKY, RING, @@ -147,6 +159,11 @@ namespace cmvr::device { virtual std::vector getSensorData() = 0; virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0; virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0; + // Backends must provide a documented conversion; raw pressure counts + // cannot be assumed to represent newtons. + virtual ForceNewtons getResultantForceNewtons(FingerType, TactileRegion) { + throw std::runtime_error(typeName() + " does not provide force in newtons."); + } virtual void setPositions(const std::vector&) { CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction."; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt b/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt index b6a30201..0b592756 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt @@ -12,3 +12,12 @@ target_link_libraries(px_6ax_gen3 ) install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib) + +# Sensor-only executable: no DeviceManager, arm initialization or calibration. +add_executable(px_6ax_gen3_real_test src/px_6ax_gen3_real_test.cpp) +target_link_libraries(px_6ax_gen3_real_test PRIVATE + px_6ax_gen3 cmvr_es::proto cmvr_es::logging pthread) + +add_executable(px_6ax_gen3_test src/px_6ax_gen3_test.cpp) +target_link_libraries(px_6ax_gen3_test PRIVATE + px_6ax_gen3 cmvr_es::proto cmvr_es::logging gtest gtest_main pthread) diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/README.md b/cmvr-es/devices/dexhand/px_6ax_gen3/README.md new file mode 100644 index 00000000..6b861cdf --- /dev/null +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/README.md @@ -0,0 +1,51 @@ +# PX-6AX GEN3 传感器读取与真实 USB 测试 + +真实测试只创建 PX6AXGen3 和 POSIX 串口，不初始化机械臂或 DeviceManager；禁用自动标定，传输层只允许 `0xFB` 读取命令。 + +在仓库根目录运行： + +```bash +# 每次读取都发起一个真实请求：检查串口应答耗时和按压力值。 +./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \ + --mode sync --duration-s 30 --csv /tmp/paxini-sync.csv --raw-log /tmp/paxini-frames.log + +# 与触屏任务相同：后台轮询，前台读取最新有效缓存。 +./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \ + --mode stream --duration-s 30 --csv /tmp/paxini-stream.csv +``` + +也可把 `--port` 指定为 `/dev/serial/by-id/` 下的稳定设备链接。设备端口取决于连接顺序；测试参数不会更改机器人部署配置里的串口。`module-id=2` 对应协议设备地址 3。 + +脚本会构建真实测试并优先加载本次构建的驱动和 protobuf。默认构建目录为 `cmake-build-debug`，可用 `CMVR_BUILD_DIR` 覆盖；构建目录须已完成 CMake 配置。串口应可读写，运行测试前退出占用同一串口的程序。 + +测试期间可用手轻按、松开传感器，终端每 100 ms 显示一次力值，CSV 记录每次 getter 调用。按 Ctrl-C 可结束。CSV 的无效读数留空，不能当作零力。进程在初始化失败、没有有效数据或存在读取失败时返回非零退出码。 + +- `sync` 的 `read_ms` 是一次驱动请求/应答调用的耗时；默认每 5 ms 请求一次。 +- `stream` 的 `read_ms` 是读取缓存耗时，成功次数包含重复快照，不能据此推算传感器实际更新频率。 +- `force_N`、`fz_N` 和 CSV 中的三个力分量均由驱动直接返回，单位为 N；例如原始值 1 对应 0.1 N、108 对应 10.8 N。传感器标称输出频率 83.3 Hz；请求频率可以高于内部测量更新频率。 +- 串口往返时间不包含“物理接触到传感器产生非零输出”的全部时间。需要按压试验或外部同步信号才能测量接触检测延迟。 +- `--raw-log` 会记录原始 TX/RX 和单调时钟时间，用于对照手册定位帧问题；写日志会给时间测量带来少量开销。 + +## 驱动行为 + +`getResultantForceNewtons()` 将合力寄存器的三个原始分量各乘以 0.1，返回以 N 为单位的浮点力值；原有 `getResultantForce()` 保留原始整数。触屏任务和 USB 测试使用牛顿接口，不再额外换算。触屏任务的 `force_threshold`、力值日志和 `lastTouchPressureSum()` 均使用 N；阈值 `0.1` 与旧版原始值阈值 `1.0` 对应相同力度。FZ 判据比较法向力，MAGNITUDE 判据比较三轴合力大小，多区域按原逻辑累加。 + +MuJoCo 零值触觉后端也实现牛顿接口。尚无确定换算系数的其他后端（如 RH56DFTP）调用该接口会明确报错，不会把原始压力计数当成 N。 + +合力应答按 `14 字节头部 + 3 字节数据 + 1 字节 LRC` 完整读取。验证帧长度、设备地址、预留位、功能码、寄存器地址、字节数及 LRC，并支持分片、请求回显和噪声后的重新定位。 + +读状态字节属于内部调试信息（手册 5.3.5）；实测正常读应答为 `0x01`，不能套用写应答 `0x00=成功` 的规则。自动标定的写应答要求完整 15 字节并且状态为 0。 + +`max_sample_age_ms` 默认 50 ms，可在设备配置中调整。每类数据分别记录请求开始时间：较晚返回的旧请求不能被重新标记为新数据。此值独立于 `response_timeout_ms`（默认 200 ms）。后台读取接口遇到过期或失效快照会抛出异常，不等待串口补读、不返回旧力值或伪造零值；触屏任务现有的异常捕获会将其识别为触觉不可用。 + +通信失败会使快照失效，后台继续尝试恢复，只有通过验证的新应答才能恢复有效数据。`stop()` 清除缓存；停止或故障状态下的 getter 报错，显式 `init()` / `start()` 后才能恢复。初始化重试失败会返回 false。 + +## 自动回归测试 + +真实设备无法稳定制造的坏帧和超时，用可注入的串口实现验证： + +```bash +cmake --build cmake-build-debug --target px_6ax_gen3_test -j 4 +LD_LIBRARY_PATH="$PWD/cmake-build-debug:$PWD/cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3:$PWD/cmake-build-debug/cmvr-es/hardware:$PWD/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \ + ./cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3/px_6ax_gen3_test +``` diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h index 163f12c8..7b6b51d9 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h @@ -39,6 +39,8 @@ namespace cmvr::device { }; explicit PX6AXGen3(const config::PX6AXGen3& cfg); + PX6AXGen3(const config::PX6AXGen3& cfg, + std::unique_ptr<::cmvr::AbstractSerialTransport> serial); ~PX6AXGen3() override; std::string typeName() const override { return "PX6AXGen3"; } @@ -54,7 +56,10 @@ namespace cmvr::device { void setTactilePollingRegions(const std::vector& regions) override; std::vector getSensorData() override; TactileRegionData getSensorData(FingerType finger, TactileRegion region) override; + // Throws when no fresh, validated sample is available. A failed read is + // never represented as a zero force (which would mean no contact). ResultantForce getResultantForce(FingerType finger, TactileRegion region) override; + ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override; private: struct SensorSnapshot { @@ -64,6 +69,8 @@ namespace cmvr::device { int cols{0}; bool tactile_valid{false}; bool resultant_valid{false}; + std::chrono::steady_clock::time_point tactile_request_time{}; + std::chrono::steady_clock::time_point resultant_request_time{}; }; static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode); @@ -81,17 +88,19 @@ namespace cmvr::device { bool isSupportedRegion(FingerType finger, TactileRegion region) const; bool isSnapshotReady(bool require_tactile, bool require_resultant) const; + bool isSampleFresh(std::chrono::steady_clock::time_point request_time) const; TactileRegionData buildSupportedRegionSnapshot() const; std::pair resolvePollingReadSelection() const; void clearOperationalError(); - void handleRefreshFailure(const std::string& error, bool had_valid_snapshot); + void handleRefreshFailure(const std::string& error); void transitionTo(Status next_state); void enterFault(const std::string& error); bool isOperationalState(Status lifecycle) const; std::unique_ptr<::cmvr::AbstractSerialTransport> serial_; config::PX6AXGen3 config_; + bool config_valid_{false}; mutable std::mutex lifecycle_mutex_; Status lifecycle_state_{Status::CREATED}; @@ -109,6 +118,7 @@ namespace cmvr::device { int tactile_rows_{1}; int tactile_cols_{0}; int response_timeout_ms_{200}; + std::chrono::milliseconds max_sample_age_{50}; FingerType tactile_finger_{FingerType::INDEX}; TactileRegion tactile_region_{TactileRegion::TIP}; PollingReadMode polling_read_mode_{PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE}; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp index d15d4cf6..588b38b5 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp @@ -125,32 +125,13 @@ namespace { return frame; } - std::vector extractPayload(const std::vector& response, - const size_t frame_offset, - const size_t response_header_bytes, - const size_t payload_length) { - if (response.size() < frame_offset + response_header_bytes + payload_length) { - CMVR_LOG(ERROR) << "PX-6AX GEN3 response shorter than expected payload window."; - return {}; - } - return std::vector( - response.begin() + static_cast(frame_offset + response_header_bytes), - response.begin() + static_cast(frame_offset + response_header_bytes + payload_length)); - } + // UART read reply: 14-byte header, N payload bytes, one LRC byte. + constexpr size_t kResponseHeaderBytes = 14U; + constexpr size_t kResponseOverheadBytes = kResponseHeaderBytes + 1U; - size_t findResponseFrameOffset(const std::vector& response, - const size_t expected_frame_bytes) { - if (response.size() < expected_frame_bytes) { - return std::string::npos; - } - - for (size_t offset = 0; offset + expected_frame_bytes <= response.size(); ++offset) { - if (response[offset] == 0xAA && response[offset + 1] == 0x55) { - return offset; - } - } - - return std::string::npos; + uint16_t readLe16(const std::vector& bytes, const size_t offset) { + return static_cast(bytes[offset]) | + static_cast(static_cast(bytes[offset + 1U]) << 8U); } std::string previewBytesHex(const std::vector& data, const size_t max_bytes = 32U) { @@ -171,39 +152,83 @@ namespace { return stream.str(); } - std::vector readFramedResponse(cmvr::AbstractSerialTransport& serial, - const size_t expected_frame_bytes, - const size_t max_prefix_bytes, - const std::chrono::milliseconds timeout, - const std::string& response_name) { + std::vector transact(cmvr::AbstractSerialTransport& serial, + const std::vector& request, + const size_t payload_length, + const std::chrono::milliseconds timeout) { + if (!serial.flushInput()) { + throw std::runtime_error("Failed to flush PX6AXGen3 input: " + serial.lastError()); + } + if (!serial.write(request)) { + throw std::runtime_error("Failed to send PX6AXGen3 request: " + serial.lastError()); + } + + const size_t expected_size = kResponseOverheadBytes + payload_length; + // Permit a request echo/noisy prefix, but bound resynchronization work. + const size_t max_response_bytes = expected_size + 4096U; const auto deadline = std::chrono::steady_clock::now() + timeout; std::vector response; - const bool read_ok = serial.read(expected_frame_bytes, timeout, response); - auto frame_offset = findResponseFrameOffset(response, expected_frame_bytes); - - while (frame_offset == std::string::npos && - response.size() < expected_frame_bytes + max_prefix_bytes) { + size_t offset = 0; + std::string error = "Incomplete PX6AXGen3 response"; + while (response.size() < max_response_bytes) { const auto now = std::chrono::steady_clock::now(); if (now >= deadline) { break; } - - std::vector extra_bytes; - const auto remaining_timeout = std::chrono::duration_cast(deadline - now); - if (!serial.read(1U, remaining_timeout, extra_bytes)) { + std::vector chunk; + const auto remaining = std::max(std::chrono::milliseconds(1), + std::chrono::duration_cast(deadline - now)); + // Read available bytes; never wait for a guessed length before + // identifying the header. This also handles split headers/echoes. + const bool read_ok = serial.read(1U, remaining, chunk); + if (chunk.size() > max_response_bytes - response.size()) { + throw std::runtime_error("PX6AXGen3 response exceeds resynchronization limit"); + } + response.insert(response.end(), chunk.begin(), chunk.end()); + while (offset + kResponseHeaderBytes <= response.size()) { + if (response[offset] != 0xAA || response[offset + 1U] != 0x55) { + ++offset; + continue; + } + // Frame length counts data[4] through data[N+13], excluding LRC. + if (readLe16(response, offset + 2U) != payload_length + 10U || + response[offset + 4U] != request[4] || + response[offset + 5U] != 0x00 || + !std::equal(request.begin() + 6, request.begin() + 13, + response.begin() + static_cast(offset + 6U))) { + error = "PX6AXGen3 response does not match request (length/address/function)"; + ++offset; + continue; + } + if (response.size() - offset < expected_size) { + break; + } + uint8_t sum = 0; + for (size_t i = offset; i < offset + expected_size; ++i) { + sum = static_cast(sum + response[i]); + } + if (sum != 0U) { + error = "Invalid PX6AXGen3 response LRC"; + ++offset; + continue; + } + // Manual 5.3.5: read status is internal/debug information; + // hardware returns 0x01 on normal force reads. Only write ACKs + // define 0x00 as success (5.4.2), so do not apply it to reads. + if (request[6] == 0x79 && response[offset + 13U] != 0x00) { + throw std::runtime_error("PX6AXGen3 returned status " + + std::to_string(response[offset + 13U])); + } + return std::vector( + response.begin() + static_cast(offset + kResponseHeaderBytes), + response.begin() + static_cast(offset + kResponseHeaderBytes + payload_length)); + } + if (!read_ok || chunk.empty()) { break; } - - response.insert(response.end(), extra_bytes.begin(), extra_bytes.end()); - frame_offset = findResponseFrameOffset(response, expected_frame_bytes); } - - if (!read_ok && frame_offset == std::string::npos) { - CMVR_LOG(ERROR) << "Failed to read " << response_name << ": " << serial.lastError() - << ", raw=" << previewBytesHex(response); - } - - return response; + throw std::runtime_error(error + "; " + serial.lastError() + + ", raw=" + previewBytesHex(response)); } std::array parseResultantPayload(const std::vector& payload) { @@ -303,21 +328,28 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value } PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) - : serial_(std::make_unique<::cmvr::PosixSerialTransport>()), - config_(cfg) { + : PX6AXGen3(cfg, std::make_unique<::cmvr::PosixSerialTransport>()) {} + +PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg, + std::unique_ptr<::cmvr::AbstractSerialTransport> serial) + : serial_(std::move(serial)), config_(cfg) { id_ = config_.id(); port_name_ = config_.serial_port(); if (!config_.sensor_model().empty()) { sensor_model_ = config_.sensor_model(); } - module_id_ = std::max(0, config_.module_id()); + module_id_ = config_.module_id(); + if (!serial_ || module_id_ < 0 || module_id_ > 254) { + enterFault("PX6AXGen3 requires a serial transport and module_id in [0, 254]."); + return; + } device_address_ = module_id_ + 1; if (config_.baud_rate() > 0) { baud_rate_ = config_.baud_rate(); } distributed_length_ = config_.distributed_length(); - if (distributed_length_ <= 0) { - enterFault("PX6AXGen3 requires config.distributed_length to be explicitly configured."); + if (distributed_length_ <= 0 || distributed_length_ > 65525 || distributed_length_ % 3 != 0) { + enterFault("PX6AXGen3 distributed_length must be a positive multiple of 3 fitting the UART frame."); return; } if (config_.resultant_length() > 0) { @@ -326,6 +358,15 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) if (config_.response_header_bytes() > 0) { response_header_bytes_ = config_.response_header_bytes(); } + if (resultant_length_ != 3 || response_header_bytes_ != kResponseHeaderBytes || + config_.resultant_length() < 0 || config_.response_header_bytes() < 0 || + config_.max_sample_age_ms() < 0) { + enterFault("PX6AXGen3 requires resultant_length=3, response_header_bytes=14 and a nonnegative max_sample_age_ms."); + return; + } + if (config_.max_sample_age_ms() > 0) { + max_sample_age_ = std::chrono::milliseconds(config_.max_sample_age_ms()); + } if (config_.tactile_rows() > 0) { tactile_rows_ = config_.tactile_rows(); } @@ -350,6 +391,7 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) poll_interval_ = std::chrono::milliseconds(config_.poll_interval_ms()); } initializeSnapshot(); + config_valid_ = true; } PX6AXGen3::~PX6AXGen3() { @@ -357,15 +399,15 @@ PX6AXGen3::~PX6AXGen3() { } bool PX6AXGen3::init() { + if (!config_valid_) { + return false; + } try { - ensureConnected(); - if (!isOperationalState(state())) { - return false; - } refreshSensorDataWithRetry( 5, std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2))); - return isOperationalState(state()); + const auto [tactile, resultant] = resolvePollingReadSelection(); + return isOperationalState(state()) && isSnapshotReady(tactile, resultant); } catch (const std::exception& e) { enterFault("[PX6AXGen3](init): " + std::string(e.what())); return false; @@ -373,8 +415,10 @@ bool PX6AXGen3::init() { } bool PX6AXGen3::start() { + if (!config_valid_) { + return false; + } if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) { - transitionTo(Status::STREAMING); return true; } @@ -383,19 +427,22 @@ bool PX6AXGen3::start() { polling_thread_.join(); } - ensureConnected(); - if (!isOperationalState(state())) { - polling_thread_running_.store(false, std::memory_order_release); - return false; + bool requested_polling; + { + std::lock_guard lock(polling_mutex_); + requested_polling = requested_polling_; } - if (requested_polling_) { + if (requested_polling) { refreshSensorDataWithRetry( 5, std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2))); + } else { + std::lock_guard lock(refresh_mutex_); + ensureConnected(); } - polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this); transitionTo(Status::STREAMING); + polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this); polling_cv_.notify_all(); return true; } catch (const std::exception& e) { @@ -413,6 +460,8 @@ bool PX6AXGen3::stop() { polling_thread_.join(); } + std::lock_guard lock(refresh_mutex_); + initializeSnapshot(); closeConnection(); if (state() != Status::FAULT) { @@ -434,10 +483,13 @@ std::string PX6AXGen3::lastError() const { void PX6AXGen3::getState(DexHandState& state_out) { DexHandState next_state{}; next_state.is_initialized = isOperationalState(state()); + const auto [tactile, resultant] = resolvePollingReadSelection(); + const bool ready = isSnapshotReady(tactile, resultant); + next_state.is_initialized = next_state.is_initialized && ready; { std::lock_guard lock(snapshot_mutex_); - if (latest_snapshot_.resultant_valid) { + if (latest_snapshot_.resultant_valid && isSampleFresh(latest_snapshot_.resultant_request_time)) { next_state.hands[0].force = latest_snapshot_.resultant_force_tenths[2]; } } @@ -445,6 +497,8 @@ void PX6AXGen3::getState(DexHandState& state_out) { const auto error = lastError(); if (!error.empty()) { next_state.hands[0].error_message.push_back(error); + } else if (!ready) { + next_state.hands[0].error_message.push_back("PX6AXGen3 sample is unavailable or stale."); } state_out = std::move(next_state); @@ -468,7 +522,8 @@ void PX6AXGen3::setTactilePollingRegions(const std::vector& re } polling_cv_.notify_all(); - if (!regions.empty() && isOperationalState(state())) { + if (!regions.empty() && isOperationalState(state()) && + !polling_thread_running_.load(std::memory_order_acquire)) { refreshSensorData(); } } @@ -482,8 +537,7 @@ std::vector PX6AXGen3::getSensorData() { TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) { if (!isSupportedRegion(finger, region)) { - CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data."; - return {}; + throw std::invalid_argument("PX6AXGen3 requested tactile region is not configured."); } ensureSensorReady(true, true, false); @@ -492,16 +546,14 @@ TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion regi PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) { if (!isSupportedRegion(finger, region)) { - CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data."; - return {}; + throw std::invalid_argument("PX6AXGen3 requested resultant-force region is not configured."); } ensureSensorReady(true, false, true); std::lock_guard lock(snapshot_mutex_); - if (!latest_snapshot_.resultant_valid) { - CMVR_LOG(ERROR) << "PX6AXGen3 resultant-force snapshot is not ready."; - return {}; + if (!latest_snapshot_.resultant_valid || !isSampleFresh(latest_snapshot_.resultant_request_time)) { + throw std::runtime_error("PX6AXGen3 resultant-force sample is unavailable or stale."); } return ResultantForce{ @@ -511,29 +563,28 @@ PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, Tactil }; } +PX6AXGen3::ForceNewtons PX6AXGen3::getResultantForceNewtons(FingerType finger, TactileRegion region) { + const auto raw = getResultantForce(finger, region); + // PX-6AX GEN3 manual 5.6.2: one resultant-force LSB is 0.1 N. + constexpr double kNewtonsPerCount = 0.1; + return {raw.fx * kNewtonsPerCount, raw.fy * kNewtonsPerCount, raw.fz * kNewtonsPerCount}; +} + void PX6AXGen3::initializeSnapshot() { std::lock_guard lock(snapshot_mutex_); latest_snapshot_ = SensorSnapshot{}; } void PX6AXGen3::ensureConnected() { - if (port_name_.empty()) { - enterFault("PX6AXGen3 serial port is not configured."); - return; + if (!config_valid_ || port_name_.empty()) { + throw std::runtime_error("PX6AXGen3 serial/configuration is invalid."); } - - if (!serial_) { - serial_ = std::make_unique<::cmvr::PosixSerialTransport>(); - } - if (!serial_->isOpen()) { if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) { - enterFault("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError()); - return; + throw std::runtime_error("Failed to open PX6AXGen3 serial transport: " + serial_->lastError()); } calibration_performed_ = false; } - calibrateIfRequested(); if (!isOperationalState(state())) { transitionTo(Status::INITIALIZED); @@ -553,27 +604,9 @@ void PX6AXGen3::calibrateIfRequested() { if (!auto_calibrate_ || calibration_performed_) { return; } - const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_); - if (!serial_->flushInput()) { - enterFault("Failed to flush serial input before calibration: " + serial_->lastError()); - return; - } - if (!serial_->write(frame)) { - enterFault("Failed to send calibration command: " + serial_->lastError()); - return; - } - const auto response = readFramedResponse( - *serial_, - 2U, - frame.size(), - std::chrono::milliseconds(response_timeout_ms_), - "calibration response"); - if (findResponseFrameOffset(response, 2U) == std::string::npos) { - enterFault("PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" + - previewBytesHex(response)); - return; - } + // Write acknowledgment has a complete 14-byte header and LRC, no payload. + transact(*serial_, frame, 0U, std::chrono::milliseconds(response_timeout_ms_)); calibration_performed_ = true; } @@ -584,117 +617,51 @@ void PX6AXGen3::refreshSensorData() { void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) { if (!read_distributed && !read_resultant) { - CMVR_LOG(ERROR) << "PX6AXGen3 refreshSensorData requires at least one data type to read."; - return; + throw std::invalid_argument("PX6AXGen3 refresh requires at least one data type."); } std::lock_guard refresh_lock(refresh_mutex_); - const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant); - try { ensureConnected(); - if (!isOperationalState(state())) { - return; - } - - std::vector tactile_points; - int rows = 0; - int cols = 0; - bool tactile_valid = false; - - if (read_distributed) { - const auto distributed_frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_); - if (!serial_->flushInput()) { - handleRefreshFailure("Failed to flush serial input before distributed-force read: " + serial_->lastError(), had_valid_snapshot); - return; - } - if (!serial_->write(distributed_frame)) { - handleRefreshFailure("Failed to send distributed-force command: " + serial_->lastError(), had_valid_snapshot); - return; - } - const size_t distributed_frame_bytes = - static_cast(response_header_bytes_) + static_cast(distributed_length_); - const auto distributed_response = readFramedResponse( - *serial_, - distributed_frame_bytes, - distributed_frame.size(), - std::chrono::milliseconds(response_timeout_ms_), - "distributed tactile response"); - const auto distributed_frame_offset = - findResponseFrameOffset(distributed_response, distributed_frame_bytes); - if (distributed_frame_offset == std::string::npos) { - handleRefreshFailure("Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response), had_valid_snapshot); - return; - } - - tactile_points = parseDistributedPayload(extractPayload( - distributed_response, - distributed_frame_offset, - static_cast(response_header_bytes_), - static_cast(distributed_length_))); - std::tie(rows, cols) = resolveMatrixShape(tactile_rows_, tactile_cols_, tactile_points.size()); - tactile_valid = !tactile_points.empty(); - } - - std::array resultant_force_tenths{}; - bool resultant_valid = false; + // Publish force first: a slower distributed read must not postpone a + // contact measurement. Each channel has its own request timestamp. if (read_resultant) { - try { - const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_); - if (!serial_->flushInput()) { - handleRefreshFailure("Failed to flush serial input before resultant-force read: " + serial_->lastError(), had_valid_snapshot); - return; - } - if (!serial_->write(resultant_frame)) { - handleRefreshFailure("Failed to send resultant-force command: " + serial_->lastError(), had_valid_snapshot); - return; - } - const size_t resultant_frame_bytes = - static_cast(response_header_bytes_) + static_cast(resultant_length_); - const auto resultant_response = readFramedResponse( - *serial_, - resultant_frame_bytes, - resultant_frame.size(), - std::chrono::milliseconds(response_timeout_ms_), - "resultant-force response"); - const auto resultant_frame_offset = - findResponseFrameOffset(resultant_response, resultant_frame_bytes); - if (resultant_frame_offset != std::string::npos) { - resultant_force_tenths = parseResultantPayload(extractPayload( - resultant_response, - resultant_frame_offset, - static_cast(response_header_bytes_), - static_cast(resultant_length_))); - resultant_valid = true; - } else { - handleRefreshFailure("Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response), had_valid_snapshot); - return; - } - } catch (const std::exception& e) { - if (!read_distributed) { - handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot); - return; - } - CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what(); + const auto frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_); + const auto request_time = std::chrono::steady_clock::now(); + const auto payload = transact(*serial_, frame, resultant_length_, + std::chrono::milliseconds(response_timeout_ms_)); + if (!isSampleFresh(request_time)) { + throw std::runtime_error("PX6AXGen3 resultant-force response arrived too late."); } + const auto force = parseResultantPayload(payload); + std::lock_guard lock(snapshot_mutex_); + latest_snapshot_.resultant_force_tenths = force; + latest_snapshot_.resultant_request_time = request_time; + latest_snapshot_.resultant_valid = true; } - - std::lock_guard lock(snapshot_mutex_); if (read_distributed) { - latest_snapshot_.tactile_points = std::move(tactile_points); + const auto frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_); + const auto request_time = std::chrono::steady_clock::now(); + const auto payload = transact(*serial_, frame, distributed_length_, + std::chrono::milliseconds(response_timeout_ms_)); + if (!isSampleFresh(request_time)) { + throw std::runtime_error("PX6AXGen3 distributed-force response arrived too late."); + } + auto points = parseDistributedPayload(payload); + const auto [rows, cols] = resolveMatrixShape(tactile_rows_, tactile_cols_, points.size()); + std::lock_guard lock(snapshot_mutex_); + latest_snapshot_.tactile_points = std::move(points); latest_snapshot_.rows = rows; latest_snapshot_.cols = cols; - latest_snapshot_.tactile_valid = tactile_valid; + latest_snapshot_.tactile_request_time = request_time; + latest_snapshot_.tactile_valid = true; } - if (read_resultant) { - latest_snapshot_.resultant_force_tenths = resultant_force_tenths; - latest_snapshot_.resultant_valid = resultant_valid; - } - clearOperationalError(); } catch (const std::exception& e) { - handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot); - return; + handleRefreshFailure(e.what()); + // Let initialization retries and synchronous callers see the failure. + // The polling thread catches it and continues reconnecting in background. + throw; } } @@ -715,7 +682,7 @@ void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts, } } - handleRefreshFailure(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error, isSnapshotReady(true, true)); + throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error); } void PX6AXGen3::pollingLoop() { @@ -754,18 +721,21 @@ void PX6AXGen3::pollingLoop() { void PX6AXGen3::ensureSensorReady(const bool allow_background, const bool require_tactile, const bool require_resultant) { + const auto lifecycle = state(); + if (!config_valid_ || lifecycle == Status::STOPPED || lifecycle == Status::FAULT) { + throw std::runtime_error("PX6AXGen3 is not operational: " + lastError()); + } const auto [polls_tactile, polls_resultant] = resolvePollingReadSelection(); const bool background_covers_request = (!require_tactile || polls_tactile) && (!require_resultant || polls_resultant); - const bool background_ready = allow_background && - background_covers_request && - polling_thread_running_.load(std::memory_order_acquire) && - isSnapshotReady(require_tactile, require_resultant); - - if (!background_ready) { - refreshSensorData(require_tactile, require_resultant); + if (allow_background && background_covers_request && + polling_thread_running_.load(std::memory_order_acquire)) { + // The getter checks freshness while copying under snapshot_mutex_. + // Never block the control loop on serial I/O to replace a stale sample. + return; } + refreshSensorData(require_tactile, require_resultant); } bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion region) const { @@ -774,8 +744,15 @@ bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion r bool PX6AXGen3::isSnapshotReady(const bool require_tactile, const bool require_resultant) const { std::lock_guard lock(snapshot_mutex_); - return (!require_tactile || latest_snapshot_.tactile_valid) && - (!require_resultant || latest_snapshot_.resultant_valid); + return (!require_tactile || (latest_snapshot_.tactile_valid && + isSampleFresh(latest_snapshot_.tactile_request_time))) && + (!require_resultant || (latest_snapshot_.resultant_valid && + isSampleFresh(latest_snapshot_.resultant_request_time))); +} + +bool PX6AXGen3::isSampleFresh(const std::chrono::steady_clock::time_point request_time) const { + return request_time != std::chrono::steady_clock::time_point{} && + std::chrono::steady_clock::now() - request_time <= max_sample_age_; } TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const { @@ -785,9 +762,8 @@ TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const { { std::lock_guard lock(snapshot_mutex_); - if (!latest_snapshot_.tactile_valid) { - CMVR_LOG(ERROR) << "PX6AXGen3 tactile snapshot is not ready."; - return {}; + if (!latest_snapshot_.tactile_valid || !isSampleFresh(latest_snapshot_.tactile_request_time)) { + throw std::runtime_error("PX6AXGen3 tactile sample is unavailable or stale."); } *snapshot = latest_snapshot_.tactile_points; rows = latest_snapshot_.rows; @@ -823,21 +799,15 @@ void PX6AXGen3::clearOperationalError() { } } -void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_valid_snapshot) { - closeConnection(); - - if (had_valid_snapshot) { - { - std::lock_guard lock(lifecycle_mutex_); - if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) { - last_error_ = error; - } - } - CMVR_LOG(WARNING) << error; - return; +void PX6AXGen3::handleRefreshFailure(const std::string& error) { + // Invalidate before any close/reconnect work. Keep the worker alive so a + // subsequent valid transaction can restore service, but never expose old data. + initializeSnapshot(); + { + std::lock_guard lock(lifecycle_mutex_); + last_error_ = error; } - - enterFault(error); + closeConnection(); } void PX6AXGen3::transitionTo(const Status next_state) { @@ -857,6 +827,7 @@ void PX6AXGen3::enterFault(const std::string& error) { polling_thread_running_.store(false, std::memory_order_release); polling_cv_.notify_all(); + initializeSnapshot(); closeConnection(); CMVR_LOG(ERROR) << error; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp new file mode 100644 index 00000000..4459a289 --- /dev/null +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp @@ -0,0 +1,201 @@ +#include "../include/px_6ax_gen3.h" +#include "hardware/include/posix_serial_transport.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +using Clock = std::chrono::steady_clock; +volatile std::sig_atomic_t interrupted = 0; +void interrupt(int) { interrupted = 1; } + +// Wrap the actual POSIX serial transport; optionally capture every transmitted +// and received byte to diagnose framing/USB latency without a second reader. +class TraceTransport final : public cmvr::AbstractSerialTransport { +public: + explicit TraceTransport(const std::string& path) { + if (!path.empty()) { + trace_.open(path); + if (!trace_) throw std::runtime_error("Cannot open raw log: " + path); + } + } + bool open(const Config& cfg) override { return serial_.open(cfg); } + bool close() override { return serial_.close(); } + bool isOpen() const override { return serial_.isOpen(); } + bool flushInput() override { return serial_.flushInput(); } + bool write(const std::vector& data) override { + // Only permit sensor read requests, even if driver defaults change. + if (data.size() != 14 || data[6] != 0xFB) { + throw std::runtime_error("Real test only permits force read commands."); + } + const bool ok = serial_.write(data); + record(ok ? "TX" : "TX_FAILED", data); + return ok; + } + bool read(size_t n, std::chrono::milliseconds timeout, std::vector& data) override { + const bool ok = serial_.read(n, timeout, data); + record(ok ? "RX" : "RX_FAILED", data); + return ok; + } + std::string lastError() const override { return serial_.lastError(); } +private: + void record(const char* direction, const std::vector& data) { + if (!trace_.is_open()) return; + trace_ << std::fixed << std::setprecision(3) + << std::chrono::duration(Clock::now() - begin_).count() + << " ms " << direction; + for (const auto byte : data) { + trace_ << ' ' << std::hex << std::setw(2) << std::setfill('0') << static_cast(byte); + } + trace_ << std::dec << std::setfill(' ') << '\n'; + trace_.flush(); + } + cmvr::PosixSerialTransport serial_; + std::ofstream trace_; + Clock::time_point begin_{Clock::now()}; +}; + +struct Options { + std::string port{"/dev/ttyACM0"}; + std::string mode{"sync"}; + std::string csv; + std::string raw_log; + int module_id{2}; + int poll_ms{5}; + int max_age_ms{50}; + int timeout_ms{200}; + double duration_s{10.0}; +}; + +void usage() { + std::cout << "Usage: px_6ax_gen3_real_test [--port /dev/ttyACM0] [--module-id 2]\n" + " [--duration-s 10] [--mode sync|stream] [--poll-ms 5]\n" + " [--max-age-ms 50] [--timeout-ms 200] [--csv samples.csv]\n" + " [--raw-log frames.log]\n" + "sync: each read sends a new force request; latency is request/response time.\n" + "stream: exercise background polling and cached getters used by the task;\n" + " getter counts include repeated samples, not sensor update frequency.\n" + "Only reads the sensor. No calibration or robot commands. Ctrl-C stops.\n"; +} + +Options parseOptions(int argc, char** argv) { + Options o; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg == "--help") { usage(); std::exit(0); } + if (++i >= argc) throw std::invalid_argument("Missing value for " + arg); + const std::string value = argv[i]; + if (arg == "--port") o.port = value; + else if (arg == "--mode") o.mode = value; + else if (arg == "--module-id") o.module_id = std::stoi(value); + else if (arg == "--duration-s") o.duration_s = std::stod(value); + else if (arg == "--poll-ms") o.poll_ms = std::stoi(value); + else if (arg == "--max-age-ms") o.max_age_ms = std::stoi(value); + else if (arg == "--timeout-ms") o.timeout_ms = std::stoi(value); + else if (arg == "--csv") o.csv = value; + else if (arg == "--raw-log") o.raw_log = value; + else throw std::invalid_argument("Unknown option: " + arg); + } + if ((o.mode != "sync" && o.mode != "stream") || !std::isfinite(o.duration_s) || + o.duration_s <= 0 || o.poll_ms <= 0 || o.max_age_ms <= 0 || o.timeout_ms <= 0) { + throw std::invalid_argument("Invalid mode, duration or timing option"); + } + return o; +} + +double percentile(const std::vector& sorted, double fraction) { + return sorted.empty() ? 0.0 : sorted[static_cast((sorted.size() - 1) * fraction)]; +} +} + +int main(int argc, char** argv) { + try { + const auto options = parseOptions(argc, argv); + std::signal(SIGINT, interrupt); + std::signal(SIGTERM, interrupt); + cmvr::config::PX6AXGen3 cfg; + cfg.set_serial_port(options.port); + cfg.set_module_id(options.module_id); + cfg.set_baud_rate(921600); + cfg.set_distributed_length(153); + cfg.set_resultant_length(3); + cfg.set_poll_interval_ms(options.poll_ms); + cfg.set_response_timeout_ms(options.timeout_ms); + cfg.set_max_sample_age_ms(options.max_age_ms); + cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE); + cfg.set_auto_calibrate(false); + cmvr::device::PX6AXGen3 sensor(cfg, std::make_unique(options.raw_log)); + std::ofstream csv; + if (!options.csv.empty()) { + csv.open(options.csv); + if (!csv) throw std::runtime_error("Cannot open CSV: " + options.csv); + csv << "elapsed_ms,read_ms,valid,fx_N,fy_N,fz_N\n"; + } + std::cout << "port=" << options.port << " module_id=" << options.module_id + << " device_address=" << options.module_id + 1 << " mode=" << options.mode + << " max_sample_age_ms=" << options.max_age_ms << '\n'; + if (!sensor.init()) throw std::runtime_error("Sensor init failed: " + sensor.lastError()); + if (options.mode == "stream" && !sensor.start()) { + throw std::runtime_error("Sensor start failed: " + sensor.lastError()); + } + const auto begin = Clock::now(); + auto next = begin; + auto next_print = begin; + std::vector durations; + size_t failures = 0; + double max_fz = 0.0; + while (!interrupted && std::chrono::duration(Clock::now() - begin).count() < options.duration_s) { + const auto read_begin = Clock::now(); + cmvr::device::AbstractDexHand::ForceNewtons force; + bool valid = true; + std::string error; + try { + force = sensor.getResultantForceNewtons(cmvr::device::AbstractDexHand::FingerType::INDEX, + cmvr::device::AbstractDexHand::TactileRegion::TIP); + } catch (const std::exception& e) { + valid = false; + error = e.what(); + ++failures; + } + const auto now = Clock::now(); + const double read_ms = std::chrono::duration(now - read_begin).count(); + const double elapsed_ms = std::chrono::duration(now - begin).count(); + if (valid) { durations.push_back(read_ms); max_fz = std::max(max_fz, force.fz); } + if (csv.is_open()) { + csv << std::fixed << std::setprecision(3) << elapsed_ms << ',' << read_ms << ',' << valid; + if (valid) csv << ',' << force.fx << ',' << force.fy << ',' << force.fz; + else csv << ",,,"; + csv << '\n'; + } + if (now >= next_print) { + std::cout << std::fixed << std::setprecision(3) << "t_ms=" << elapsed_ms + << " read_ms=" << read_ms; + if (valid) std::cout << " force_N=[" << force.fx << ',' << force.fy << ',' << force.fz + << "] fz_N=" << force.fz; + else std::cout << " UNAVAILABLE: " << error; + std::cout << std::endl; + next_print = now + std::chrono::milliseconds(100); + } + next += std::chrono::milliseconds(options.poll_ms); + if (next < now) next = now; + std::this_thread::sleep_until(next); + } + const double elapsed_s = std::chrono::duration(Clock::now() - begin).count(); + sensor.stop(); + std::sort(durations.begin(), durations.end()); + std::cout << "SUMMARY mode=" << options.mode << " valid_reads=" << durations.size() + << " unavailable_reads=" << failures << " valid_reads_per_s=" << durations.size() / elapsed_s + << " p50_ms=" << percentile(durations, .5) << " p95_ms=" << percentile(durations, .95) + << " max_ms=" << percentile(durations, 1) << " max_fz_N=" << max_fz << '\n'; + return durations.empty() || failures != 0 ? 1 : 0; + } catch (const std::exception& e) { + std::cerr << e.what() << '\n'; + return 1; + } +} diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp new file mode 100644 index 00000000..fee9564a --- /dev/null +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp @@ -0,0 +1,318 @@ +#include "../include/px_6ax_gen3.h" + +#include +#include +#include +#include +#include + +namespace { +using Sensor = cmvr::device::PX6AXGen3; +using Bytes = std::vector; +using namespace std::chrono_literals; + +void checksum(Bytes& frame) { + unsigned sum = 0; + for (size_t i = 0; i + 1 < frame.size(); ++i) sum += frame[i]; + frame.back() = static_cast(-sum); +} + +Bytes reply(const Bytes& request) { + const bool is_write = request[6] == 0x79; + const size_t n = is_write ? 0U : request[11] | (size_t(request[12]) << 8U); + Bytes frame{0xAA, 0x55, static_cast((n + 10U) & 0xffU), + static_cast((n + 10U) >> 8U)}; + frame.insert(frame.end(), request.begin() + 4, request.begin() + 13); + // Real device returns status=1 on reads; zero is only the write ACK status. + frame.push_back(is_write ? 0 : 1); + for (size_t i = 0; i < n; ++i) { + frame.push_back(i % 3U == 0 ? 0x80 : (i % 3U == 1 ? 0x7f : 10)); + } + frame.push_back(0); + checksum(frame); + return frame; +} + +class ScriptedTransport final : public cmvr::AbstractSerialTransport { +public: + bool open(const Config&) override { opened = true; return true; } + bool close() override { opened = false; return true; } + bool isOpen() const override { return opened; } + bool flushInput() override { pending.clear(); return true; } + bool write(const Bytes& request) override { + ++writes; + unsigned sum = 0; + for (auto byte : request) sum += byte; + EXPECT_EQ(sum % 256, 0U); + pending = dropping.load() ? Bytes{} : make_reply(request); + return true; + } + bool read(size_t, std::chrono::milliseconds timeout, Bytes& out) override { + out.clear(); + if (pending.empty()) { + waiting.store(true); + std::this_thread::sleep_for(timeout); + waiting.store(false); + return false; + } + const size_t n = std::min(chunk_size, pending.size()); + out.assign(pending.begin(), pending.begin() + n); + pending.erase(pending.begin(), pending.begin() + n); + return true; + } + std::string lastError() const override { return "scripted timeout"; } + std::function make_reply{reply}; + size_t chunk_size{256}; + std::atomic writes{0}; + std::atomic dropping{false}; + std::atomic waiting{false}; +private: + bool opened{false}; + Bytes pending; +}; + +cmvr::config::PX6AXGen3 config() { + cmvr::config::PX6AXGen3 cfg; + cfg.set_serial_port("scripted"); + cfg.set_module_id(2); + cfg.set_distributed_length(153); + cfg.set_poll_interval_ms(5); + cfg.set_response_timeout_ms(5); + cfg.set_max_sample_age_ms(50); + cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE); + return cfg; +} + +Sensor::ResultantForce force(Sensor& sensor) { + return sensor.getResultantForce(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP); +} + +Sensor::ForceNewtons forceNewtons(cmvr::device::AbstractDexHand& sensor) { + return sensor.getResultantForceNewtons(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP); +} + +bool waitFor(const std::function& predicate) { + const auto deadline = std::chrono::steady_clock::now() + 1s; + do { + if (predicate()) return true; + std::this_thread::sleep_for(1ms); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +TEST(PX6AXGen3, ParsesRealReadStatusAndSignedForcesAcrossByteFragments) { + auto serial = std::make_unique(); + serial->chunk_size = 1; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + const auto f = force(sensor); + EXPECT_EQ(f.fx, -128); + EXPECT_EQ(f.fy, 127); + EXPECT_EQ(f.fz, 10); +} + +TEST(PX6AXGen3, NewtonInterfacePreservesSignsAndComputesPhysicalMagnitude) { + auto serial = std::make_unique(); + auto* transport = serial.get(); + serial->make_reply = [](const Bytes& request) { + auto frame = reply(request); + frame[14] = 0xfd; // -3 raw = -0.3 N + frame[15] = 4; + frame[16] = 12; + checksum(frame); + return frame; + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + const auto writes_before = transport->writes.load(); + const auto f = forceNewtons(sensor); + EXPECT_EQ(transport->writes.load(), writes_before + 1); + EXPECT_DOUBLE_EQ(f.fx, -0.3); + EXPECT_DOUBLE_EQ(f.fy, 0.4); + EXPECT_DOUBLE_EQ(f.fz, 1.2); + EXPECT_DOUBLE_EQ(f.magnitude(), 1.3); + EXPECT_EQ(force(sensor).fz, 12); // Raw interface remains unchanged. +} + +class NewtonForce : public testing::TestWithParam {}; +TEST_P(NewtonForce, ConvertsRawFzOnceAndPreservesPointOneNewtonTrigger) { + auto serial = std::make_unique(); + const int raw_fz = GetParam(); + serial->make_reply = [raw_fz](const Bytes& request) { + auto frame = reply(request); + frame[14] = 0; + frame[15] = 0; + frame[16] = static_cast(raw_fz); + checksum(frame); + return frame; + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + const auto f = forceNewtons(sensor); + EXPECT_DOUBLE_EQ(f.fz, static_cast(raw_fz) / 10.0); + EXPECT_DOUBLE_EQ(f.magnitude(), f.fz); + EXPECT_EQ(f.fz >= 0.1, raw_fz >= 1); + EXPECT_EQ(force(sensor).fz, raw_fz); +} +INSTANTIATE_TEST_SUITE_P(PhysicalUnits, NewtonForce, testing::Values(0, 1, 10, 108, 255)); + +TEST(PX6AXGen3, ResynchronizesAfterEchoNoiseAndInvalidFrame) { + auto serial = std::make_unique(); + serial->chunk_size = 7; + serial->make_reply = [](const Bytes& request) { + auto bad = reply(request); + bad.back() ^= 1; + Bytes frames{0xAA, 0x00}; + frames.insert(frames.end(), request.begin(), request.end()); + frames.insert(frames.end(), bad.begin(), bad.end()); + const auto valid = reply(request); + frames.insert(frames.end(), valid.begin(), valid.end()); + return frames; + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + EXPECT_EQ(force(sensor).fz, 10); +} + +class InvalidFrame : public testing::TestWithParam {}; +TEST_P(InvalidFrame, RejectsResponseAndFailsInitializationAfterRetries) { + auto serial = std::make_unique(); + auto* transport = serial.get(); + const int kind = GetParam(); + serial->make_reply = [kind](const Bytes& request) { + auto frame = reply(request); + if (kind == 0) { frame.pop_back(); return frame; } // missing LRC + if (kind == 1) { frame.back() ^= 1; return frame; } // bad LRC + // Frame size, device, reserved, function, register, returned byte count. + const int offsets[]{2, 4, 5, 6, 7, 11}; + frame[offsets[kind - 2]] ^= 1; + checksum(frame); // Valid LRC must not bypass request matching. + return frame; + }; + Sensor sensor(config(), std::move(serial)); + EXPECT_FALSE(sensor.init()); + EXPECT_EQ(transport->writes.load(), 5); + EXPECT_EQ(sensor.state(), Sensor::Status::FAULT); + EXPECT_FALSE(sensor.lastError().empty()); + EXPECT_THROW(force(sensor), std::runtime_error); +} +INSTANTIATE_TEST_SUITE_P(ProtocolValidation, InvalidFrame, testing::Range(0, 8)); + +TEST(PX6AXGen3, RetriesTransientStartupFailure) { + auto serial = std::make_unique(); + int count = 0; + serial->make_reply = [&count](const Bytes& request) { + ++count; + return count < 3 ? Bytes{} : reply(request); + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + EXPECT_EQ(count, 3); + EXPECT_TRUE(sensor.lastError().empty()); +} + +TEST(PX6AXGen3, StaleGetterFailsWithoutWaitingForSerialAndRecovers) { + auto cfg = config(); + cfg.set_response_timeout_ms(200); + cfg.set_max_sample_age_ms(30); + auto serial = std::make_unique(); + auto* transport = serial.get(); + Sensor sensor(cfg, std::move(serial)); + ASSERT_TRUE(sensor.init()); + ASSERT_TRUE(sensor.start()); + ASSERT_EQ(force(sensor).fz, 10); + transport->dropping.store(true); + ASSERT_TRUE(waitFor([&] { return transport->waiting.load(); })); + std::this_thread::sleep_for(40ms); + const auto begin = std::chrono::steady_clock::now(); + EXPECT_THROW(force(sensor), std::runtime_error); + EXPECT_THROW(forceNewtons(sensor), std::runtime_error); + EXPECT_LT(std::chrono::steady_clock::now() - begin, 50ms); + cmvr::device::DexHandState state; + sensor.getState(state); + EXPECT_FALSE(state.is_initialized); + EXPECT_FALSE(state.hands[0].error_message.empty()); + transport->dropping.store(false); + ASSERT_TRUE(waitFor([&] { + try { return force(sensor).fz == 10; } + catch (const std::exception&) { return false; } + })); + sensor.stop(); + const auto writes = transport->writes.load(); + EXPECT_THROW(force(sensor), std::runtime_error); + EXPECT_THROW(forceNewtons(sensor), std::runtime_error); + EXPECT_EQ(transport->writes.load(), writes); +} + +TEST(PX6AXGen3, ReadFailureInvalidatesPreviouslyValidSample) { + auto serial = std::make_unique(); + auto* transport = serial.get(); + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + transport->dropping.store(true); + EXPECT_THROW(force(sensor), std::runtime_error); + cmvr::device::DexHandState state; + sensor.getState(state); + EXPECT_FALSE(state.is_initialized); + transport->dropping.store(false); + EXPECT_EQ(force(sensor).fz, 10); +} + +TEST(PX6AXGen3, RejectsLateResponseEvenWithValidChecksum) { + auto cfg = config(); + cfg.set_max_sample_age_ms(2); + auto serial = std::make_unique(); + serial->make_reply = [](const Bytes& request) { + std::this_thread::sleep_for(5ms); + return reply(request); + }; + Sensor sensor(cfg, std::move(serial)); + EXPECT_FALSE(sensor.init()); + EXPECT_NE(sensor.lastError().find("too late"), std::string::npos); +} + +TEST(PX6AXGen3, DistributedDataIsValidatedAndParsed) { + auto cfg = config(); + cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE); + auto serial = std::make_unique(); + serial->chunk_size = 5; + Sensor sensor(cfg, std::move(serial)); + ASSERT_TRUE(sensor.init()); + auto data = sensor.getSensorData(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP); + ASSERT_TRUE(data.valid()); + ASSERT_EQ(data.view.pointCount(), 51); + EXPECT_EQ(data.view.at(0, 50).fx, -128); + EXPECT_EQ(data.view.at(0, 50).fz, 10); +} + +TEST(PX6AXGen3, CalibrationRequiresFullSuccessfulWriteAcknowledgment) { + auto cfg = config(); + cfg.set_auto_calibrate(true); + auto serial = std::make_unique(); + serial->make_reply = [](const Bytes& request) { + if (request[6] == 0x79) return Bytes{0xAA, 0x55}; + return reply(request); + }; + Sensor sensor(cfg, std::move(serial)); + EXPECT_FALSE(sensor.init()); +} + +TEST(PX6AXGen3, CalibrationAcceptsFullWriteAcknowledgment) { + auto cfg = config(); + cfg.set_auto_calibrate(true); + Sensor sensor(cfg, std::make_unique()); + ASSERT_TRUE(sensor.init()); + EXPECT_EQ(force(sensor).fz, 10); +} + +TEST(PX6AXGen3, InvalidConfigurationCannotBeResurrectedByInitOrStart) { + auto cfg = config(); + cfg.set_response_header_bytes(15); + auto serial = std::make_unique(); + auto* transport = serial.get(); + Sensor sensor(cfg, std::move(serial)); + EXPECT_FALSE(sensor.init()); + EXPECT_FALSE(sensor.start()); + EXPECT_EQ(transport->writes.load(), 0); +} +} diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h index a44b5ddb..05f96794 100644 --- a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h +++ b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h @@ -38,6 +38,7 @@ public: std::vector getSensorData() override; TactileRegionData getSensorData(FingerType finger, TactileRegion region) override; ResultantForce getResultantForce(FingerType finger, TactileRegion region) override; + ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override; private: TactileRegionData makeRegionData(FingerType finger, TactileRegion region); diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp index c04387e9..46e95a76 100644 --- a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp +++ b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp @@ -83,6 +83,11 @@ ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce( return TactilePoint::fromFz(0); } +ZeroSimTouchDexHand::ForceNewtons ZeroSimTouchDexHand::getResultantForceNewtons( + const FingerType, const TactileRegion) { + return {}; +} + ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData( const FingerType finger, const TactileRegion region) { tactile_points_[0] = TactilePoint::fromFz(0); diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index ae3fffd1..665989d9 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -95,6 +95,7 @@ public: int targetU() const; int targetV() const; + // Selected force criterion summed over requested tactile regions, in N. double lastTouchPressureSum() const; int lastTouchNonzeroCount() const; int lastActiveTagId() const; @@ -183,8 +184,8 @@ private: int pbvs_debug_count_{0}; int last_active_tag_id_{-1}; - double last_touch_pressure_sum_{0.0}; - double last_touch_resultant_fz_{0.0}; + double last_touch_pressure_sum_{0.0}; // N + double last_touch_resultant_fz_{0.0}; // N int last_touch_nonzero_count_{0}; Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()}; Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()}; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 433e236f..7ad183f4 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -116,15 +116,15 @@ bool isTouchTriggered(const TouchScreenTaskConfig& config, return resultant_force_value >= config.touch().tactile().force_threshold(); } -double tactileForceValue(const device::AbstractDexHand::TactilePoint& point, +double tactileForceValue(const device::AbstractDexHand::ForceNewtons& point, const cmvr::config::TouchScreenTactileCriterion criterion) { switch (criterion) { case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ: - return static_cast(point.fz); + return point.fz; case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE: return point.magnitude(); } - return static_cast(point.fz); + return point.fz; } Eigen::Matrix3d rotationFromTargetEuler(const double rx, @@ -2022,9 +2022,10 @@ bool TouchScreenTask::updateTouchPressure() { double resultant_fz = 0.0; try { for (const auto& tactile_region : tactile_regions) { - const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second); + const auto resultant_force = dexhand_->getResultantForceNewtons( + tactile_region.first, tactile_region.second); resultant_value += tactileForceValue(resultant_force, tactile.criterion()); - resultant_fz += static_cast(resultant_force.fz); + resultant_fz += resultant_force.fz; } } catch (...) { return false; @@ -2043,9 +2044,9 @@ void TouchScreenTask::logTouchPressure(const bool force) { return; } last_touch_pressure_log_time_ = now; - CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << last_touch_resultant_fz_ - << ", criterion_value=" << last_touch_pressure_sum_ - << ", threshold=" << config_.touch().tactile().force_threshold() + CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz_N=" << last_touch_resultant_fz_ + << ", criterion_value_N=" << last_touch_pressure_sum_ + << ", threshold_N=" << config_.touch().tactile().force_threshold() << ", triggered=" << isTouchTriggered(config_, last_touch_pressure_sum_); } diff --git a/protos/cmvr/config/dexhand_config/dexhand_config.proto b/protos/cmvr/config/dexhand_config/dexhand_config.proto index 6ef3b307..1568aa3e 100644 --- a/protos/cmvr/config/dexhand_config/dexhand_config.proto +++ b/protos/cmvr/config/dexhand_config/dexhand_config.proto @@ -36,6 +36,9 @@ message PX6AXGen3{ string tactile_region = 16; string sensor_name = 17; PX6AXGen3PollingReadMode polling_read_mode = 18; + // Maximum age since the sample's request was sent. 0 uses 50 ms. + // Independent of response_timeout_ms: stale cached data must fail promptly. + int32 max_sample_age_ms = 19; } message ZeroSimTouchDexHand { diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto index 588d603e..e4b4c6ac 100644 --- a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto +++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto @@ -112,6 +112,8 @@ message TouchScreenTactileTriggerConfig { optional TouchScreenFingerType finger = 1; optional TouchScreenTactileRegion region = 2; optional TouchScreenTactileCriterion criterion = 3; + // Threshold in newtons (N), applied to the selected force criterion summed + // over the requested tactile regions. Equality also triggers contact. optional double force_threshold = 4; } diff --git a/script/test_px_6ax_gen3.sh b/script/test_px_6ax_gen3.sh new file mode 100755 index 00000000..a3e35af7 --- /dev/null +++ b/script/test_px_6ax_gen3.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Real USB sensor test. The executable only sends force-read requests. +# Example: ./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --duration-s 20 --mode sync +repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)" +build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}" +cmake --build "$build_dir" --target px_6ax_gen3_real_test -j 4 +sensor_build_dir="$build_dir/cmvr-es/devices/dexhand/px_6ax_gen3" +# Put build libraries ahead of installed ones so this tests the current driver/proto. +exec env LD_LIBRARY_PATH="$build_dir:$sensor_build_dir:$build_dir/cmvr-es/hardware:$repo_root/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \ + "$sensor_build_dir/px_6ax_gen3_real_test" "$@"