SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add px 6ax gen3 as touch pressure sensor

Browse Source
This commit is contained in:
lgv 2026-05-08 16:58:52 +08:00
parent 2807fd158f
commit b050942838
37 changed files with 2555 additions and 536 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
.codex Normal file
View File

31
cmvr-es/applications/include/touch_screen_app.h
View File

@ -63,7 +63,12 @@ public:
POSITION_ONLY // 保留锁定时看到的完整 tag 姿态，只按位置对齐。
};
struct Options {
enum class TactileCriterion {
FZ = 0,
MAGNITUDE
};
struct Config {
// 是否在触控流程开始前先回到指定初始关节位姿。
bool move_to_init_position_before_start{false};
// 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。
@ -201,11 +206,11 @@ public:
// 使用该手指的哪个触觉区域。
device::AbstractDexHand::TactileRegion tactile_region{
device::AbstractDexHand::TactileRegion::TIP};
// 触觉压力和阈值；总和超过该值认为已经接触。
// 三维合力标量化方式：直接使用法向 fz，或使用三维力模长。
TactileCriterion tactile_criterion{TactileCriterion::FZ};
// 触觉区域三维合力按 tactile_criterion 标量化后的阈值；超过该值认为已经接触。
double tactile_pressure_sum_threshold{100.0};
// 触觉峰值阈值；为 0 时表示不使用峰值判据。
double tactile_pressure_peak_threshold{0.0};
// 当前帧非 0 触觉点数量阈值；只有达到该数量后，才进一步判断 sum/peak 阈值。
// 保留兼容的旧字段；当前合力判定逻辑不再使用非零点数量阈值。
int tactile_nonzero_count_threshold{1};
};
@ -221,8 +226,8 @@ public:
const std::shared_ptr<device::AbstractCamera>& camera,
const cmvr::config::TouchScreenAppConfig& config);
bool setOptionsFromConfig();
bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config);
bool setConfigFromProto();
bool setConfigFromProto(const cmvr::config::TouchScreenAppConfig& config);
bool setTouchSpeedlForwardL(double forward_l);
bool startFromPixel(int u, int v);
@ -243,7 +248,6 @@ public:
int targetU() const { return target_u_; }
int targetV() const { return target_v_; }
double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; }
int lastActiveTagId() const { return last_active_tag_id_; }
const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
@ -255,10 +259,10 @@ public:
private:
using Clock = std::chrono::steady_clock;
static bool optionsFromConfig(const cmvr::config::TouchScreenAppConfig& config,
Options& options_out);
void setOptions(const Options& options);
bool applyOptions();
static bool configFromProto(const cmvr::config::TouchScreenAppConfig& proto_config,
Config& config_out);
void setConfig(const Config& config);
bool applyConfig();
bool validateControlJointNames() const;
bool stepAligning(double dt);
bool stepTouching();
@ -289,7 +293,7 @@ private:
std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
perception::TagRelativeTarget3D tracker_;
IbvsController ibvs_;
Options options_{};
Config config_{};
Phase phase_{Phase::IDLE};
Phase phase_after_retract_{Phase::DONE};
@ -307,7 +311,6 @@ private:
int last_active_tag_id_{-1};
double last_touch_pressure_sum_{0.0};
double last_touch_pressure_peak_{0.0};
int last_touch_nonzero_count_{0};
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
bool locked_target_rotation_valid_{false};

520
cmvr-es/applications/src/touch_screen_app.cpp
View File

File diff suppressed because it is too large Load Diff

4
cmvr-es/applications/src/touch_screen_app_test.cpp
View File

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

25
cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt
View File

@ -2,6 +2,7 @@ rh56dftp_dexhands {
id: "hand1"
ip: "192.168.1.213"
port: 6000
poll_interval_ms: 10
enable: false
}
@ -9,5 +10,27 @@ rh56dftp_dexhands {
id: "hand2"
ip: "192.168.1.224"
port: 6000
poll_interval_ms: 10
enable: true
}
}
px_6ax_gen3 {
id: "paxini_tip_1"
serial_port: "/dev/ttyACM1"
sensor_model: "S1813_core"
module_id: 2
baud_rate: 921600
distributed_length: 153
resultant_length: 3
poll_interval_ms: 5
response_timeout_ms: 200
response_header_bytes: 14
tactile_rows: 1
tactile_cols: 51
tactile_finger: "INDEX"
tactile_region: "TIP"
sensor_name: "Paxini Gen3末端压力"
polling_read_mode: PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE
auto_calibrate: false
enable: true
}

24
cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt
View File

@ -1,5 +1,5 @@
robot_id: "hc01"
dexhand_id: "hand2"
dexhand_id: "paxini_tip_1"
camera_id: "right_hand_cam"
move_to_init_position_before_start: true
move_to_init_position: true
@ -17,7 +17,7 @@ init_joint_positions {
}
init_joint_positions {
joint_name: "R_ELBOW_R"
rad: 1.69508
rad: 1.61
}
init_joint_positions {
joint_name: "R_WRIST_P"
@ -44,8 +44,8 @@ depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
hover_target_in_camera {
x: 0.005
y: 0.075
x: -0.001
y: 0.08
z: 0.15
}
target_rx: 3.14159265358979323846
@ -111,14 +111,14 @@ pause_after_align_reached: false
touch_twist_base {
vx: 0.0
vy: -0.08
vy: -0.04
vz: 0.0
wx: 0.0
wy: 0.0
wz: 0.0
}
touch_use_speedl: true
touch_speedl_acceleration: 3.0
touch_speedl_acceleration: 6.0
touch_forward_l: 0.064
touch_movel_speed: 0.1
touch_movel_acceleration: 5.0
@ -130,7 +130,7 @@ touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_speedl_forward_l: 0.0255
touch_speedl_forward_l: 0.035
dwell_time_s: -1
retract_twist_base {
@ -141,11 +141,11 @@ retract_twist_base {
wy: 0.0
wz: 0.0
}
retract_acceleration: 3.0
retract_acceleration: 8.0
retract_duration_s: 0.45
tactile_finger: TOUCH_SCREEN_FINGER_TYPE_RING
tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP
tactile_pressure_sum_threshold: 200.0
tactile_pressure_peak_threshold: 0
tactile_nonzero_count_threshold: 4
tactile_criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
tactile_pressure_sum_threshold: 1.0
tactile_nonzero_count_threshold: 1

24
cmvr-es/common/utils/ffmpeg/include/CameraCapture.h
View File

@ -15,16 +15,16 @@ extern "C"{
namespace ffmpeg {
struct CameraConfig {
std::string device_name = "0"; // Windows: "0" (dshow), Linux: "/dev/video0"
int width = 1280;
int height = 720;
int fps = 30;
AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P; // 保持原有的像素格式
};
class CameraCapture {
public:
struct Config {
std::string device_name = "0"; // Windows: "0" (dshow), Linux: "/dev/video0"
int width = 1280;
int height = 720;
int fps = 30;
AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P; // 保持原有的像素格式
};
// 回调函数：同时返回AVFrame和cv::Mat
using FrameCallback = std::function<void(AVFrame* frame, cv::Mat rgb_image, int64_t timestamp)>;
@ -32,7 +32,7 @@ namespace ffmpeg {
~CameraCapture();
// 初始化摄像头
int initialize(const CameraConfig& config);
int initialize(const Config& config);
// 开始捕获
int start_capture(FrameCallback callback);
@ -41,7 +41,7 @@ namespace ffmpeg {
void stop_capture();
// 获取当前配置
CameraConfig get_config() const { return config_; }
Config get_config() const { return config_; }
// 获取输入格式上下文
AVFormatContext* get_format_context() { return fmt_ctx_; }
@ -50,7 +50,7 @@ namespace ffmpeg {
int get_video_stream_index() const { return video_stream_index_; }
private:
CameraConfig config_;
Config config_;
AVFormatContext* fmt_ctx_ = nullptr;
AVCodecContext* decoder_ctx_ = nullptr;
int video_stream_index_ = -1;
@ -77,4 +77,4 @@ namespace ffmpeg {
void cleanup();
};
} // namespace ffmpeg
} // namespace ffmpeg

24
cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h
View File

@ -19,16 +19,16 @@ extern "C" {
namespace ffmpeg {
struct RealSenseConfig {
std::string serial_number = ""; // 设备序列号，为空则使用第一个设备
int width = 1280;
int height = 720;
int fps = 30;
AVPixelFormat pixel_format = AV_PIX_FMT_BGR24; // RealSense输出BGR格式
};
class RealSenseCapture {
public:
struct Config {
std::string serial_number = ""; // 设备序列号，为空则使用第一个设备
int width = 1280;
int height = 720;
int fps = 30;
AVPixelFormat pixel_format = AV_PIX_FMT_BGR24; // RealSense输出BGR格式
};
// 回调函数：同时返回AVFrame和cv::Mat
using FrameCallback = std::function<void(AVFrame* frame, cv::Mat rgb_image, int64_t timestamp)>;
@ -36,7 +36,7 @@ public:
~RealSenseCapture();
// 初始化摄像头
int initialize(const RealSenseConfig& config);
int initialize(const Config& config);
// 开始捕获
int start_capture(FrameCallback callback);
@ -45,7 +45,7 @@ public:
void stop_capture();
// 获取当前配置
RealSenseConfig get_config() const { return config_; }
Config get_config() const { return config_; }
// 获取设备信息
std::string get_device_info() const;
@ -54,7 +54,7 @@ public:
rs2_intrinsics get_intrinsics() const { return intrinsics_; }
private:
RealSenseConfig config_;
Config config_;
// RealSense相关
rs2::context ctx_;
@ -91,4 +91,4 @@ private:
cv::Mat realsense_to_cvmat(rs2::frame frame);
};
} // namespace ffmpeg
} // namespace ffmpeg

34
cmvr-es/common/utils/ffmpeg/include/VideoFrameEncoder.h
View File

@ -19,29 +19,29 @@ extern "C" {
namespace ffmpeg {
struct EncoderConfig {
int width = 1280;
int height = 720;
int fps = 30;
int bitrate = 5000000; // 5Mbps
int gop_size = 15; // GOP大小
int max_b_frames = 2; // 最大B帧数
AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P;
std::string preset = "fast"; // 编码速度预设
std::string tune = "zerolatency"; // 调优参数
std::string profile = "main"; // 编码profile
std::string codec = "libx265";
};
class VideoFrameEncoder {
public:
struct Config {
int width = 1280;
int height = 720;
int fps = 30;
int bitrate = 5000000; // 5Mbps
int gop_size = 15; // GOP大小
int max_b_frames = 2; // 最大B帧数
AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P;
std::string preset = "fast"; // 编码速度预设
std::string tune = "zerolatency"; // 调优参数
std::string profile = "main"; // 编码profile
std::string codec = "libx265";
};
using PacketCallback = std::function<void(AVPacket* packet, int64_t pts, int64_t dts)>;
VideoFrameEncoder();
~VideoFrameEncoder();
// 初始化编码器
int initialize(const EncoderConfig& config);
int initialize(const Config& config);
// 编码一帧
int encode_frame(AVFrame* frame);
@ -76,7 +76,7 @@ public:
int64_t get_frame_index() const { return frame_index_; }
private:
EncoderConfig config_;
Config config_;
AVCodecContext* encoder_ctx_ = nullptr;
SwsContext* sws_ctx_ = nullptr;
AVFrame* converted_frame_ = nullptr;
@ -98,4 +98,4 @@ private:
int64_t get_current_timestamp();
};
} // namespace ffmpeg
} // namespace ffmpeg

24
cmvr-es/common/utils/ffmpeg/include/VideoWriter.h
View File

@ -12,22 +12,22 @@ extern "C"{
namespace ffmpeg {
struct WriterConfig {
std::string output_file = "output.mp4";
int width = 1280;
int height = 720;
int fps = 30;
int bitrate = 5000000;
AVRational time_base = {1, 90000}; // 默认时间基
};
class VideoWriter {
public:
struct Config {
std::string output_file = "output.mp4";
int width = 1280;
int height = 720;
int fps = 30;
int bitrate = 5000000;
AVRational time_base = {1, 90000}; // 默认时间基
};
VideoWriter();
~VideoWriter();
// 初始化写入器
int initialize(const WriterConfig& config, AVCodecContext* codec_ctx);
int initialize(const Config& config, AVCodecContext* codec_ctx);
// 写入数据包
int write_packet(AVPacket* packet, int64_t pts, int64_t dts);
@ -47,7 +47,7 @@ namespace ffmpeg {
}
private:
WriterConfig config_;
Config config_;
AVFormatContext* fmt_ctx_ = nullptr;
AVStream* video_stream_ = nullptr;
AVCodecContext* codec_ctx_ref_ = nullptr;
@ -60,4 +60,4 @@ namespace ffmpeg {
void cleanup();
};
} // namespace ffmpeg
} // namespace ffmpeg

4
cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp
View File

@ -22,7 +22,7 @@ CameraCapture::~CameraCapture() {
if (sws_ctx_) sws_freeContext(sws_ctx_);
}
int CameraCapture::initialize(const CameraConfig& config) {
int CameraCapture::initialize(const Config& config) {
config_ = config;
// 初始化设备
@ -260,4 +260,4 @@ void CameraCapture::cleanup() {
}
}
} // namespace ffmpeg
} // namespace ffmpeg

4
cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp
View File

@ -17,7 +17,7 @@ RealSenseCapture::~RealSenseCapture() {
if (rgb_frame_) av_frame_free(&rgb_frame_);
}
int RealSenseCapture::initialize(const RealSenseConfig& config) {
int RealSenseCapture::initialize(const Config& config) {
std::lock_guard<std::mutex> lock(mutex_);
config_ = config;
@ -298,4 +298,4 @@ std::string RealSenseCapture::get_device_info() const {
}
}
} // namespace ffmpeg
} // namespace ffmpeg

4
cmvr-es/common/utils/ffmpeg/src/VideoFrameEncoder.cpp
View File

@ -23,7 +23,7 @@ int64_t VideoFrameEncoder::get_current_timestamp() {
return av_rescale_q(elapsed, {1, 1000000}, time_base_);
}
int VideoFrameEncoder::initialize(const EncoderConfig& config) {
int VideoFrameEncoder::initialize(const Config& config) {
config_ = config;
// 查找编码器
@ -237,4 +237,4 @@ void VideoFrameEncoder::cleanup() {
}
}
} // namespace ffmpeg
} // namespace ffmpeg

4
cmvr-es/common/utils/ffmpeg/src/VideoWriter.cpp
View File

@ -10,7 +10,7 @@ VideoWriter::~VideoWriter() {
cleanup();
}
int VideoWriter::initialize(const WriterConfig& config, AVCodecContext* codec_ctx) {
int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) {
config_ = config;
codec_ctx_ref_ = codec_ctx;
@ -150,4 +150,4 @@ void VideoWriter::cleanup() {
codec_ctx_ref_ = nullptr;
}
} // namespace ffmpeg
} // namespace ffmpeg

1
cmvr-es/device_manager/CMakeLists.txt
View File

@ -13,6 +13,7 @@ target_link_libraries(device_manager PRIVATE
cmvr_es::device::ffmpeg_microphone
cmvr_es::device::realsense_camera
cmvr_es::device::rh56dftp_dexhand
cmvr_es::device::px_6ax_gen3
cmvr::device::head_esp32
osqp
cmvr_es::device::humanoid_robot

10
cmvr-es/device_manager/src/device_factory.cpp
View File

@ -10,6 +10,7 @@
#include "devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h"
#include "devices/camera/realsense_camera/include/realsense_camera.h"
#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
#include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
#include "devices/robot/humanoid_robot/include/humanoid_robot.h"
//#include "robot/ti5_robot/ti5_robot.h"
#include "cmvr/api/system_command.pb.h"
@ -92,6 +93,9 @@ std::shared_ptr<AbstractDexHand> DeviceFactory::create_dexhand_(const XmlNode& c
if (cfg.getNodeName() == "RH56DFTP") {
return std::make_shared<RH56DFTPDexhand>(cfg);
}
else if (cfg.getNodeName() == "PX6AXGen3") {
return std::make_shared<PX6AXGen3>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
@ -177,6 +181,7 @@ std::shared_ptr<AbstractSpeaker> DeviceFactory::create_speaker_(const XmlNode& c
template std::shared_ptr<AbstractCamera> DeviceFactory::createFromConfig<AbstractCamera>(const config::UVCCameraConfig& cfg);
template std::shared_ptr<AbstractCamera> DeviceFactory::createFromConfig<AbstractCamera>(const config::RealSenseCameraConfig& cfg);
template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::RH56DFTPDexHandConfig& cfg);
template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::PX6AXGen3& cfg);
template std::shared_ptr<AbstractMicrophone> DeviceFactory::createFromConfig<AbstractMicrophone>(const config::FFMpegMicroPhoneConfig& cfg);
template std::shared_ptr<AbstractSpeaker> DeviceFactory::createFromConfig<AbstractSpeaker>(const config::FFMpegSpeakerConfig& cfg);
template <typename DeviceType, typename ConfigType>
@ -205,6 +210,10 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
{
return std::make_shared<RH56DFTPDexhand>(cfg);
}
else if constexpr (std::is_same_v<ConfigType, config::PX6AXGen3>)
{
return std::make_shared<PX6AXGen3>(cfg);
}
else
{
LOG(ERROR) << "[DeviceFactory]: Unsupported dexhand device type ";
@ -244,4 +253,3 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
return nullptr;
}
}

16
cmvr-es/device_manager/src/device_manager.cpp
View File

@ -281,6 +281,21 @@ void DeviceManager::init_devices_() {
std::get<std::shared_ptr<AbstractDexHand>>(devices_[id])->init();
}
for (int i = 0; i < dexhand_cfg.px_6ax_gen3().size(); i++) {
auto dexhand = dexhand_cfg.px_6ax_gen3(i);
if (!dexhand.enable())
continue;
auto id = dexhand.id();
if (devices_.count(id)) {
LOG(ERROR) << "[DeviceManager]: Duplicate DexHand Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate DexHand Device ID" + id);
}
std::cout << "DexHand[" << i << "]: " << id << std::endl;
auto device = dev_factory_->createFromConfig<AbstractDexHand,config::PX6AXGen3>(dexhand);
devices_[id] = device;
std::get<std::shared_ptr<AbstractDexHand>>(devices_[id])->init();
}
auto robot_node = dmgr_node.getChild("Robot");
for (auto &node: robot_node.getChildren()){
string id = node.getAttrString("id");
@ -373,4 +388,3 @@ void DeviceManager::get_os_info_() {
}
}
}

3
cmvr-es/devices/dexhand/CMakeLists.txt
View File

@ -1 +1,2 @@
add_subdirectory(rh56dftp_dexhand)
add_subdirectory(rh56dftp_dexhand)
add_subdirectory(px_6ax_gen3)

24
cmvr-es/devices/dexhand/abstract_dexhand.h
View File

@ -6,6 +6,7 @@
#define CMVR_ES_ABSTRACT_DEXHAND_H
#pragma once
#include <cmath>
#include <cstdint>
#include <memory>
#include <stdexcept>
@ -18,7 +19,27 @@
namespace cmvr::device {
class AbstractDexHand : public AbstractDevice {
public:
using TactilePoint = uint16_t;
struct TactilePoint {
int32_t fx{0};
int32_t fy{0};
int32_t fz{0};
static TactilePoint fromFz(const int32_t value) {
return TactilePoint{0, 0, value};
}
double magnitude() const {
return std::sqrt(static_cast<double>(fx) * static_cast<double>(fx) +
static_cast<double>(fy) * static_cast<double>(fy) +
static_cast<double>(fz) * static_cast<double>(fz));
}
bool isZero() const {
return fx == 0 && fy == 0 && fz == 0;
}
};
using ResultantForce = TactilePoint;
enum class FingerType {
PINKY,
@ -124,6 +145,7 @@ namespace cmvr::device {
virtual void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) = 0;
virtual std::vector<TactileRegionData> getSensorData() = 0;
virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0;
virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0;
virtual void setPositions(const std::vector<int>&) {
throw std::logic_error("setPositions is not supported by this dexhand abstraction.");

28
cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt Normal file
View File

@ -0,0 +1,28 @@
add_library(px_6ax_gen3 SHARED src/px_6ax_gen3.cpp)
target_include_directories(px_6ax_gen3 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::device::px_6ax_gen3 ALIAS px_6ax_gen3)
target_link_libraries(px_6ax_gen3
PUBLIC cmvr_es::hardware
PRIVATE
cmvr_es::proto
glog
)
install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
add_executable(px_6ax_gen3_test
src/px_6ax_gen3_test.cpp
)
target_link_libraries(px_6ax_gen3_test PRIVATE
cmvr_es::device::px_6ax_gen3
cmvr_es::common
cmvr_es::proto
glog
gtest
gtest_main
pthread
)

132
cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h Normal file
View File

@ -0,0 +1,132 @@
//
// Created by Codex on 2026/4/3.
//
#ifndef PX_6AX_GEN3_DEXHAND_H
#define PX_6AX_GEN3_DEXHAND_H
#include <array>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
#include "hardware/include/abstract_serial_transport.h"
#include "../../abstract_dexhand.h"
namespace cmvr::device {
class PX6AXGen3 final : public AbstractDexHand {
public:
using FingerType = AbstractDexHand::FingerType;
using ResultantForce = AbstractDexHand::ResultantForce;
using TactilePoint = AbstractDexHand::TactilePoint;
using TactileRegion = AbstractDexHand::TactileRegion;
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
using TactileRegionData = AbstractDexHand::TactileRegionData;
using Status = AbstractDexHand::Status;
enum class PollingReadMode {
DISTRIBUTED_FORCE,
RESULTANT_FORCE,
DISTRIBUTED_AND_RESULTANT_FORCE
};
explicit PX6AXGen3(const XmlNode& cfg);
explicit PX6AXGen3(const config::PX6AXGen3& cfg);
~PX6AXGen3() override;
void init() override;
void start() override;
void stop() override;
Status state() const override;
std::string lastError() const override;
void getState(DexHandState& state) override;
void setAngles(const std::vector<int>& finger_joint_angles) 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;
private:
struct SensorSnapshot {
std::vector<TactilePoint> tactile_points;
std::array<int, 3> resultant_force_tenths{};
int rows{0};
int cols{0};
bool tactile_valid{false};
bool resultant_valid{false};
};
static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode);
static PollingReadMode parsePollingReadModeName(std::string value);
void parseXmlConfig();
void initializeSnapshot();
void ensureConnected();
void closeConnection();
void calibrateIfRequested();
void refreshSensorData();
void refreshSensorData(bool read_distributed, bool read_resultant);
void refreshSensorDataWithRetry(int max_attempts, std::chrono::milliseconds retry_interval);
void pollingLoop();
void ensureSensorReady(bool allow_background, bool require_tactile, bool require_resultant);
bool isSupportedRegion(FingerType finger, TactileRegion region) const;
bool isSnapshotReady(bool require_tactile, bool require_resultant) const;
TactileRegionData buildSupportedRegionSnapshot() const;
std::pair<bool, bool> resolvePollingReadSelection() const;
void clearOperationalError();
void handleRefreshFailure(const std::string& error, bool had_valid_snapshot);
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_;
mutable std::mutex lifecycle_mutex_;
Status lifecycle_state_{Status::CREATED};
std::string last_error_;
std::string port_name_;
std::string sensor_model_{"S1813_elite"};
std::string sensor_name_{"PX-6AX GEN3末端压力"};
int module_id_{0};
int device_address_{1};
int baud_rate_{921600};
int distributed_length_{0};
int resultant_length_{3};
int response_header_bytes_{14};
int tactile_rows_{1};
int tactile_cols_{0};
int response_timeout_ms_{200};
FingerType tactile_finger_{FingerType::INDEX};
TactileRegion tactile_region_{TactileRegion::TIP};
PollingReadMode polling_read_mode_{PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE};
bool auto_calibrate_{false};
bool calibration_performed_{false};
mutable std::mutex refresh_mutex_;
mutable std::mutex snapshot_mutex_;
SensorSnapshot latest_snapshot_;
mutable std::mutex polling_mutex_;
std::condition_variable polling_cv_;
bool requested_polling_{true};
std::thread polling_thread_;
std::atomic<bool> polling_thread_running_{false};
std::chrono::milliseconds poll_interval_{10};
};
}
#endif //PX_6AX_GEN3_DEXHAND_H

864
cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp Normal file
View File

@ -0,0 +1,864 @@
//
// Created by Codex on 2026/4/3.
//
#include "../include/px_6ax_gen3.h"
#include <algorithm>
#include <sstream>
#include <stdexcept>
#include <tuple>
#include "hardware/include/posix_serial_transport.h"
using namespace cmvr::device;
namespace {
using FingerType = AbstractDexHand::FingerType;
using TactilePoint = AbstractDexHand::TactilePoint;
using TactileRegion = AbstractDexHand::TactileRegion;
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
using TactileRegionData = AbstractDexHand::TactileRegionData;
using Status = AbstractDexHand::Status;
enum class CommandType {
CALIBRATION,
RESULTANT_FORCE,
DISTRIBUTED_FORCE
};
uint8_t calculateLrc(const std::vector<uint8_t>& data) {
uint8_t lrc = 0;
for (const uint8_t byte : data) {
lrc = static_cast<uint8_t>((lrc + byte) & 0xFFU);
}
return static_cast<uint8_t>((~lrc + 1U) & 0xFFU);
}
FingerType parseFingerTypeName(std::string value) {
std::transform(value.begin(), value.end(), value.begin(), [](unsigned char ch) {
return static_cast<char>(std::toupper(ch));
});
if (value == "PINKY") {
return FingerType::PINKY;
}
if (value == "RING") {
return FingerType::RING;
}
if (value == "MIDDLE" || value == "MIDDLE_FINGER") {
return FingerType::MIDDLE;
}
if (value == "INDEX") {
return FingerType::INDEX;
}
if (value == "THUMB") {
return FingerType::THUMB;
}
if (value == "PALM") {
return FingerType::PALM;
}
throw std::invalid_argument("Unsupported tactile finger type: " + value);
}
TactileRegion parseTactileRegionName(std::string value) {
std::transform(value.begin(), value.end(), value.begin(), [](unsigned char ch) {
return static_cast<char>(std::toupper(ch));
});
if (value == "TIP") {
return TactileRegion::TIP;
}
if (value == "FINGER") {
return TactileRegion::FINGER;
}
if (value == "PAD") {
return TactileRegion::PAD;
}
if (value == "THUMB_MIDDLE") {
return TactileRegion::THUMB_MIDDLE;
}
if (value == "PALM_PAD") {
return TactileRegion::PALM_PAD;
}
throw std::invalid_argument("Unsupported tactile region type: " + value);
}
int parseSignedByte(const uint8_t byte) {
return byte <= 127U ? static_cast<int>(byte) : static_cast<int>(byte) - 256;
}
std::vector<uint8_t> buildCommandFrame(const CommandType command,
const int device_address,
const int distributed_length) {
std::vector<uint8_t> frame;
switch (command) {
case CommandType::CALIBRATION:
frame = {
0x55, 0xAA, 0x0A, 0x00,
static_cast<uint8_t>(device_address),
0x00, 0x79, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01
};
break;
case CommandType::RESULTANT_FORCE:
frame = {
0x55, 0xAA, 0x09, 0x00,
static_cast<uint8_t>(device_address),
0x00, 0xFB, 0xF0, 0x03, 0x00, 0x00, 0x03, 0x00
};
break;
case CommandType::DISTRIBUTED_FORCE:
frame = {
0x55, 0xAA, 0x09, 0x00,
static_cast<uint8_t>(device_address),
0x00, 0xFB, 0x0E, 0x04, 0x00, 0x00,
static_cast<uint8_t>(distributed_length & 0xFF),
static_cast<uint8_t>((distributed_length >> 8) & 0xFF)
};
break;
}
frame.push_back(calculateLrc(frame));
return frame;
}
std::vector<uint8_t> extractPayload(const std::vector<uint8_t>& 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) {
throw std::runtime_error("PX-6AX GEN3 response shorter than expected payload window.");
}
return std::vector<uint8_t>(
response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes),
response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes + payload_length));
}
size_t findResponseFrameOffset(const std::vector<uint8_t>& 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;
}
std::string previewBytesHex(const std::vector<uint8_t>& data, const size_t max_bytes = 32U) {
std::ostringstream stream;
const size_t preview_size = std::min(max_bytes, data.size());
for (size_t i = 0; i < preview_size; ++i) {
if (i > 0) {
stream << ' ';
}
stream << std::hex << std::uppercase;
stream.width(2);
stream.fill('0');
stream << static_cast<int>(data[i]);
}
if (data.size() > preview_size) {
stream << " ...";
}
return stream.str();
}
std::vector<uint8_t> 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) {
const auto deadline = std::chrono::steady_clock::now() + timeout;
std::vector<uint8_t> 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) {
const auto now = std::chrono::steady_clock::now();
if (now >= deadline) {
break;
}
std::vector<uint8_t> extra_bytes;
const auto remaining_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now);
if (!serial.read(1U, remaining_timeout, extra_bytes)) {
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) {
throw std::runtime_error(
"Failed to read " + response_name + ": " + serial.lastError() +
", raw=" + previewBytesHex(response));
}
return response;
}
std::array<int, 3> parseResultantPayload(const std::vector<uint8_t>& payload) {
if (payload.size() < 3) {
throw std::runtime_error("PX-6AX GEN3 resultant payload must contain at least 3 bytes.");
}
return {
parseSignedByte(payload[0]),
parseSignedByte(payload[1]),
static_cast<int>(payload[2])
};
}
std::vector<TactilePoint> parseDistributedPayload(const std::vector<uint8_t>& payload) {
const size_t point_count = payload.size() / 3;
std::vector<TactilePoint> points;
points.reserve(point_count);
for (size_t i = 0; i < point_count; ++i) {
points.push_back(TactilePoint{
parseSignedByte(payload[i * 3]),
parseSignedByte(payload[i * 3 + 1]),
static_cast<int>(payload[i * 3 + 2])
});
}
return points;
}
std::pair<int, int> resolveMatrixShape(const int configured_rows,
const int configured_cols,
const size_t point_count) {
if (point_count == 0U) {
return {0, 0};
}
if (configured_rows > 0 && configured_cols > 0 &&
static_cast<size_t>(configured_rows * configured_cols) == point_count) {
return {configured_rows, configured_cols};
}
if (configured_rows > 0 && configured_cols <= 0 &&
point_count % static_cast<size_t>(configured_rows) == 0U) {
return {configured_rows, static_cast<int>(point_count / static_cast<size_t>(configured_rows))};
}
if (configured_cols > 0 && configured_rows <= 0 &&
point_count % static_cast<size_t>(configured_cols) == 0U) {
return {static_cast<int>(point_count / static_cast<size_t>(configured_cols)), configured_cols};
}
return {1, static_cast<int>(point_count)};
}
}
PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadMode(const config::PX6AXGen3PollingReadMode mode) {
switch (mode) {
case config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE:
return PollingReadMode::DISTRIBUTED_FORCE;
case config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE:
return PollingReadMode::RESULTANT_FORCE;
case config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_AND_RESULTANT_FORCE:
return PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE;
default:
throw std::invalid_argument(
"Unsupported PX6AXGen3 polling_read_mode enum value: " +
std::to_string(static_cast<int>(mode)));
}
}
PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value) {
std::transform(value.begin(), value.end(), value.begin(), [](unsigned char ch) {
if (ch == '-' || ch == ' ' || ch == '+') {
return '_';
}
return static_cast<char>(std::toupper(ch));
});
if (value == "DISTRIBUTED_FORCE" ||
value == "PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE") {
return PollingReadMode::DISTRIBUTED_FORCE;
}
if (value == "RESULTANT_FORCE" ||
value == "PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE") {
return PollingReadMode::RESULTANT_FORCE;
}
if (value == "DISTRIBUTED_AND_RESULTANT_FORCE" ||
value == "RESULTANT_AND_DISTRIBUTED_FORCE" ||
value == "BOTH" ||
value == "PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_AND_RESULTANT_FORCE") {
return PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE;
}
throw std::invalid_argument("Unsupported PX6AXGen3 polling_read_mode: " + value);
}
PX6AXGen3::PX6AXGen3(const XmlNode& cfg)
: AbstractDexHand(cfg),
serial_(std::make_unique<::cmvr::PosixSerialTransport>()) {
parseXmlConfig();
initializeSnapshot();
}
PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
: serial_(std::make_unique<::cmvr::PosixSerialTransport>()),
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());
device_address_ = module_id_ + 1;
if (config_.baud_rate() > 0) {
baud_rate_ = config_.baud_rate();
}
distributed_length_ = config_.distributed_length();
if (distributed_length_ <= 0) {
throw std::invalid_argument("PX6AXGen3 requires config.distributed_length to be explicitly configured.");
}
if (config_.resultant_length() > 0) {
resultant_length_ = config_.resultant_length();
}
if (config_.response_header_bytes() > 0) {
response_header_bytes_ = config_.response_header_bytes();
}
if (config_.tactile_rows() > 0) {
tactile_rows_ = config_.tactile_rows();
}
if (config_.tactile_cols() > 0) {
tactile_cols_ = config_.tactile_cols();
}
if (config_.response_timeout_ms() > 0) {
response_timeout_ms_ = config_.response_timeout_ms();
}
if (!config_.tactile_finger().empty()) {
tactile_finger_ = parseFingerTypeName(config_.tactile_finger());
}
if (!config_.tactile_region().empty()) {
tactile_region_ = parseTactileRegionName(config_.tactile_region());
}
if (!config_.sensor_name().empty()) {
sensor_name_ = config_.sensor_name();
}
polling_read_mode_ = parsePollingReadMode(config_.polling_read_mode());
auto_calibrate_ = config_.auto_calibrate();
if (config_.poll_interval_ms() > 0) {
poll_interval_ = std::chrono::milliseconds(config_.poll_interval_ms());
}
initializeSnapshot();
}
PX6AXGen3::~PX6AXGen3() {
stop();
}
void PX6AXGen3::init() {
try {
ensureConnected();
refreshSensorDataWithRetry(
5,
std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
} catch (const std::exception& e) {
enterFault("[PX6AXGen3](init): " + std::string(e.what()));
throw;
}
}
void PX6AXGen3::start() {
if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) {
transitionTo(Status::STREAMING);
return;
}
try {
if (polling_thread_.joinable()) {
polling_thread_.join();
}
ensureConnected();
if (requested_polling_) {
refreshSensorDataWithRetry(
5,
std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
}
polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this);
transitionTo(Status::STREAMING);
polling_cv_.notify_all();
} catch (const std::exception& e) {
polling_thread_running_.store(false, std::memory_order_release);
enterFault("[PX6AXGen3](start): " + std::string(e.what()));
throw;
}
}
void PX6AXGen3::stop() {
polling_thread_running_.store(false, std::memory_order_release);
polling_cv_.notify_all();
if (polling_thread_.joinable() && polling_thread_.get_id() != std::this_thread::get_id()) {
polling_thread_.join();
}
closeConnection();
if (state() != Status::FAULT) {
transitionTo(Status::STOPPED);
}
}
PX6AXGen3::Status PX6AXGen3::state() const {
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
return lifecycle_state_;
}
std::string PX6AXGen3::lastError() const {
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
return last_error_;
}
void PX6AXGen3::getState(DexHandState& state_out) {
DexHandState next_state{};
next_state.is_initialized = isOperationalState(state());
{
std::lock_guard<std::mutex> lock(snapshot_mutex_);
if (latest_snapshot_.resultant_valid) {
next_state.hands[0].force = latest_snapshot_.resultant_force_tenths[2];
}
}
const auto error = lastError();
if (!error.empty()) {
next_state.hands[0].error_message.push_back(error);
}
state_out = std::move(next_state);
}
void PX6AXGen3::setAngles(const std::vector<int>&) {
throw std::logic_error("PX6AXGen3 is a tactile sensor only and does not support setAngles.");
}
void PX6AXGen3::setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) {
for (const auto& region : regions) {
if (!isSupportedRegion(region.first, region.second)) {
throw std::invalid_argument("PX6AXGen3 only exposes INDEX/TIP tactile data.");
}
}
{
std::lock_guard<std::mutex> lock(polling_mutex_);
requested_polling_ = !regions.empty();
}
polling_cv_.notify_all();
if (!regions.empty() && isOperationalState(state())) {
refreshSensorData();
}
}
std::vector<TactileRegionData> PX6AXGen3::getSensorData() {
ensureSensorReady(true, true, false);
std::vector<TactileRegionData> regions;
regions.push_back(buildSupportedRegionSnapshot());
return regions;
}
TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) {
if (!isSupportedRegion(finger, region)) {
throw std::invalid_argument("PX6AXGen3 only supports INDEX/TIP tactile data.");
}
ensureSensorReady(true, true, false);
return buildSupportedRegionSnapshot();
}
PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) {
if (!isSupportedRegion(finger, region)) {
throw std::invalid_argument("PX6AXGen3 only supports INDEX/TIP tactile data.");
}
ensureSensorReady(true, false, true);
std::lock_guard<std::mutex> lock(snapshot_mutex_);
if (!latest_snapshot_.resultant_valid) {
throw std::runtime_error("PX6AXGen3 resultant-force snapshot is not ready.");
}
return ResultantForce{
latest_snapshot_.resultant_force_tenths[0],
latest_snapshot_.resultant_force_tenths[1],
latest_snapshot_.resultant_force_tenths[2]
};
}
void PX6AXGen3::parseXmlConfig() {
id_ = cfg_.getAttrString("id");
port_name_ = cfg_.getAttrString("serial");
sensor_model_ = cfg_.getAttrDefault("sensor_model", sensor_model_);
sensor_name_ = cfg_.getAttrDefault("sensor_name", sensor_name_);
module_id_ = std::max(0, cfg_.getAttrDefault("module_id", module_id_));
device_address_ = module_id_ + 1;
baud_rate_ = cfg_.getAttrDefault("baud_rate", baud_rate_);
distributed_length_ = cfg_.getAttrDefault("distributed_length", 0);
if (distributed_length_ <= 0) {
throw std::invalid_argument("PX6AXGen3 requires XML attribute distributed_length to be explicitly configured.");
}
resultant_length_ = cfg_.getAttrDefault("resultant_length", resultant_length_);
response_header_bytes_ = cfg_.getAttrDefault("response_header_bytes", response_header_bytes_);
tactile_rows_ = cfg_.getAttrDefault("tactile_rows", tactile_rows_);
tactile_cols_ = cfg_.getAttrDefault("tactile_cols", tactile_cols_);
response_timeout_ms_ = cfg_.getAttrDefault("response_timeout_ms", response_timeout_ms_);
tactile_finger_ = parseFingerTypeName(cfg_.getAttrDefault("tactile_finger", "INDEX"));
tactile_region_ = parseTactileRegionName(cfg_.getAttrDefault("tactile_region", "TIP"));
polling_read_mode_ = parsePollingReadModeName(
cfg_.getAttrDefault("polling_read_mode", "DISTRIBUTED_AND_RESULTANT_FORCE"));
auto_calibrate_ = cfg_.getAttrDefault("auto_calibrate", false);
poll_interval_ = std::chrono::milliseconds(std::max(1, cfg_.getAttrDefault("poll_interval_ms", static_cast<int>(poll_interval_.count()))));
}
void PX6AXGen3::initializeSnapshot() {
std::lock_guard<std::mutex> lock(snapshot_mutex_);
latest_snapshot_ = SensorSnapshot{};
}
void PX6AXGen3::ensureConnected() {
if (port_name_.empty()) {
throw std::runtime_error("PX6AXGen3 serial port is not configured.");
}
if (!serial_) {
serial_ = std::make_unique<::cmvr::PosixSerialTransport>();
}
if (!serial_->isOpen()) {
if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) {
throw std::runtime_error("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError());
}
calibration_performed_ = false;
}
calibrateIfRequested();
if (!isOperationalState(state())) {
transitionTo(Status::INITIALIZED);
}
}
void PX6AXGen3::closeConnection() {
if (serial_) {
if (!serial_->close()) {
LOG(WARNING) << "[PX6AXGen3] Failed to close serial transport: " << serial_->lastError();
}
}
calibration_performed_ = false;
}
void PX6AXGen3::calibrateIfRequested() {
if (!auto_calibrate_ || calibration_performed_) {
return;
}
const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_);
if (!serial_->flushInput()) {
throw std::runtime_error("Failed to flush serial input before calibration: " + serial_->lastError());
}
if (!serial_->write(frame)) {
throw std::runtime_error("Failed to send calibration command: " + serial_->lastError());
}
const auto response = readFramedResponse(
*serial_,
2U,
frame.size(),
std::chrono::milliseconds(response_timeout_ms_),
"calibration response");
if (findResponseFrameOffset(response, 2U) == std::string::npos) {
throw std::runtime_error(
"PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" +
previewBytesHex(response));
}
calibration_performed_ = true;
}
void PX6AXGen3::refreshSensorData() {
const auto [read_distributed, read_resultant] = resolvePollingReadSelection();
refreshSensorData(read_distributed, read_resultant);
}
void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) {
if (!read_distributed && !read_resultant) {
throw std::invalid_argument("PX6AXGen3 refreshSensorData requires at least one data type to read.");
}
std::lock_guard<std::mutex> refresh_lock(refresh_mutex_);
const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant);
try {
ensureConnected();
std::vector<TactilePoint> 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()) {
throw std::runtime_error("Failed to flush serial input before distributed-force read: " + serial_->lastError());
}
if (!serial_->write(distributed_frame)) {
throw std::runtime_error("Failed to send distributed-force command: " + serial_->lastError());
}
const size_t distributed_frame_bytes =
static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(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) {
throw std::runtime_error(
"Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response));
}
tactile_points = parseDistributedPayload(extractPayload(
distributed_response,
distributed_frame_offset,
static_cast<size_t>(response_header_bytes_),
static_cast<size_t>(distributed_length_)));
std::tie(rows, cols) = resolveMatrixShape(tactile_rows_, tactile_cols_, tactile_points.size());
tactile_valid = !tactile_points.empty();
}
std::array<int, 3> resultant_force_tenths{};
bool resultant_valid = false;
if (read_resultant) {
try {
const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
if (!serial_->flushInput()) {
throw std::runtime_error("Failed to flush serial input before resultant-force read: " + serial_->lastError());
}
if (!serial_->write(resultant_frame)) {
throw std::runtime_error("Failed to send resultant-force command: " + serial_->lastError());
}
const size_t resultant_frame_bytes =
static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(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<size_t>(response_header_bytes_),
static_cast<size_t>(resultant_length_)));
resultant_valid = true;
} else {
throw std::runtime_error(
"Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response));
}
} catch (const std::exception& e) {
if (!read_distributed) {
throw;
}
LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what();
}
}
std::lock_guard<std::mutex> lock(snapshot_mutex_);
if (read_distributed) {
latest_snapshot_.tactile_points = std::move(tactile_points);
latest_snapshot_.rows = rows;
latest_snapshot_.cols = cols;
latest_snapshot_.tactile_valid = tactile_valid;
}
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);
throw;
}
}
void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts,
const std::chrono::milliseconds retry_interval) {
std::string last_error;
for (int attempt = 0; attempt < max_attempts; ++attempt) {
try {
refreshSensorData();
return;
} catch (const std::exception& e) {
last_error = e.what();
}
if (attempt + 1 < max_attempts) {
std::this_thread::sleep_for(retry_interval);
}
}
throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error);
}
void PX6AXGen3::pollingLoop() {
auto next_poll_deadline = std::chrono::steady_clock::now();
std::unique_lock<std::mutex> lock(polling_mutex_);
while (polling_thread_running_.load(std::memory_order_acquire)) {
if (!requested_polling_) {
polling_cv_.wait(lock, [this]() {
return !polling_thread_running_.load(std::memory_order_acquire) || requested_polling_;
});
next_poll_deadline = std::chrono::steady_clock::now();
continue;
}
lock.unlock();
try {
refreshSensorData();
} catch (const std::exception& e) {
LOG(ERROR) << e.what();
}
lock.lock();
next_poll_deadline += poll_interval_;
const auto now = std::chrono::steady_clock::now();
if (next_poll_deadline <= now) {
next_poll_deadline = now;
continue;
}
polling_cv_.wait_until(lock, next_poll_deadline, [this]() {
return !polling_thread_running_.load(std::memory_order_acquire);
});
}
}
void PX6AXGen3::ensureSensorReady(const bool allow_background,
const bool require_tactile,
const bool require_resultant) {
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);
}
}
bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion region) const {
return finger == tactile_finger_ && region == tactile_region_;
}
bool PX6AXGen3::isSnapshotReady(const bool require_tactile, const bool require_resultant) const {
std::lock_guard<std::mutex> lock(snapshot_mutex_);
return (!require_tactile || latest_snapshot_.tactile_valid) &&
(!require_resultant || latest_snapshot_.resultant_valid);
}
TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
auto snapshot = std::make_shared<std::vector<TactilePoint>>();
int rows = 0;
int cols = 0;
{
std::lock_guard<std::mutex> lock(snapshot_mutex_);
if (!latest_snapshot_.tactile_valid) {
throw std::runtime_error("PX6AXGen3 tactile snapshot is not ready.");
}
*snapshot = latest_snapshot_.tactile_points;
rows = latest_snapshot_.rows;
cols = latest_snapshot_.cols;
}
return TactileRegionData(
tactile_finger_,
tactile_region_,
AbstractDexHand::TactileMatrixView{snapshot->data(), rows, cols},
sensor_name_.c_str(),
snapshot);
}
std::pair<bool, bool> PX6AXGen3::resolvePollingReadSelection() const {
switch (polling_read_mode_) {
case PollingReadMode::DISTRIBUTED_FORCE:
return {true, false};
case PollingReadMode::RESULTANT_FORCE:
return {false, true};
case PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE:
return {true, true};
}
throw std::logic_error("Unsupported PX6AXGen3 polling read mode.");
}
void PX6AXGen3::clearOperationalError() {
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) {
last_error_.clear();
}
}
void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_valid_snapshot) {
closeConnection();
if (had_valid_snapshot) {
{
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) {
last_error_ = error;
}
}
LOG(WARNING) << error;
return;
}
enterFault(error);
}
void PX6AXGen3::transitionTo(const Status next_state) {
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
lifecycle_state_ = next_state;
if (next_state == Status::INITIALIZED || next_state == Status::STREAMING) {
last_error_.clear();
}
}
void PX6AXGen3::enterFault(const std::string& error) {
{
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
lifecycle_state_ = Status::FAULT;
last_error_ = error;
}
polling_thread_running_.store(false, std::memory_order_release);
polling_cv_.notify_all();
closeConnection();
LOG(ERROR) << error;
}
bool PX6AXGen3::isOperationalState(const Status lifecycle) const {
return lifecycle == Status::INITIALIZED || lifecycle == Status::STREAMING;
}

170
cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp Normal file
View File

@ -0,0 +1,170 @@
#include "gtest/gtest.h"
#include "../include/px_6ax_gen3.h"
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
#include "common/utils/config_helper/include/config_helper.h"
#include <chrono>
#include <cstdint>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>
namespace {
using DexHand = cmvr::device::AbstractDexHand;
using PX6AXGen3 = cmvr::device::PX6AXGen3;
DexHand::FingerType parseFingerType(const std::string& value) {
if (value == "PINKY") {
return DexHand::FingerType::PINKY;
}
if (value == "RING") {
return DexHand::FingerType::RING;
}
if (value == "MIDDLE" || value == "MIDDLE_FINGER") {
return DexHand::FingerType::MIDDLE;
}
if (value == "THUMB") {
return DexHand::FingerType::THUMB;
}
if (value == "PALM") {
return DexHand::FingerType::PALM;
}
return DexHand::FingerType::INDEX;
}
DexHand::TactileRegion parseTactileRegion(const std::string& value) {
if (value == "FINGER") {
return DexHand::TactileRegion::FINGER;
}
if (value == "PAD") {
return DexHand::TactileRegion::PAD;
}
if (value == "THUMB_MIDDLE") {
return DexHand::TactileRegion::THUMB_MIDDLE;
}
if (value == "PALM_PAD") {
return DexHand::TactileRegion::PALM_PAD;
}
return DexHand::TactileRegion::TIP;
}
struct StopGuard {
std::shared_ptr<PX6AXGen3> hand;
~StopGuard() {
if (!hand) {
return;
}
try {
hand->stop();
} catch (...) {
}
}
};
const cmvr::config::PX6AXGen3* findTestConfig(const cmvr::config::DexHandConfig& dexhand_config) {
const cmvr::config::PX6AXGen3* candidate = nullptr;
for (const auto& config : dexhand_config.px_6ax_gen3()) {
if (config.enable() && !config.serial_port().empty()) {
return &config;
}
if (candidate == nullptr && !config.serial_port().empty()) {
candidate = &config;
}
}
return candidate;
}
DexHand::ResultantForce readFirstValidResultantForce(PX6AXGen3& hand,
const DexHand::FingerType finger,
const DexHand::TactileRegion region,
const int max_attempts,
const std::chrono::milliseconds retry_interval) {
std::string last_error;
for (int attempt = 0; attempt < max_attempts; ++attempt) {
try {
return hand.getResultantForce(finger, region);
} catch (const std::exception& ex) {
last_error = ex.what();
}
if (attempt + 1 < max_attempts) {
std::this_thread::sleep_for(retry_interval);
}
}
throw std::runtime_error("Failed to read PX6AXGen3 resultant force: " + last_error);
}
DexHand::ResultantForce readStateResultantForce(PX6AXGen3& hand) {
cmvr::device::DexHandState state;
hand.getState(state);
return DexHand::ResultantForce{0, 0, state.hands[0].force};
}
} // namespace
TEST(PX6AXGen3Test, PrintResultantForceOnly) {
cmvr::config::DexHandConfig dexhand_config;
ASSERT_TRUE(cmvr::ConfigHelper::getDexHandsConfig(dexhand_config));
const cmvr::config::PX6AXGen3* hand_config = findTestConfig(dexhand_config);
ASSERT_NE(hand_config, nullptr);
const auto& test_config = *hand_config;
auto hand = std::make_shared<PX6AXGen3>(test_config);
ASSERT_NO_THROW(hand->init());
ASSERT_NO_THROW(hand->start());
StopGuard stop_guard{hand};
EXPECT_EQ(hand->state(), DexHand::Status::STREAMING);
const DexHand::FingerType finger = parseFingerType(test_config.tactile_finger());
const DexHand::TactileRegion region = parseTactileRegion(test_config.tactile_region());
const int warmup_ms = test_config.poll_interval_ms() > 0
? test_config.poll_interval_ms() * 5
: 200;
const int iterations = 1000000;
const int read_interval_ms = test_config.poll_interval_ms() > 0
? test_config.poll_interval_ms()
: 10;
if (warmup_ms > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms));
}
bool read_once = false;
for (int iteration = 0; iteration < iterations; ++iteration) {
DexHand::ResultantForce resultant_force{};
ASSERT_NO_THROW(resultant_force = readFirstValidResultantForce(
*hand,
finger,
region,
10,
std::chrono::milliseconds(100)));
DexHand::ResultantForce state_force{};
ASSERT_NO_THROW(state_force = readStateResultantForce(*hand));
std::cout << "[PX6AXGen3Test] iter=" << (iteration + 1)
<< " resultant_fx=" << resultant_force.fx
<< " resultant_fy=" << resultant_force.fy
<< " resultant_fz=" << resultant_force.fz
<< " state_resultant_fz=" << state_force.fz
<< std::endl;
read_once = true;
if (read_interval_ms > 0 && iteration + 1 < iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms));
}
}
EXPECT_TRUE(read_once);
}

2
cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h
View File

@ -47,6 +47,7 @@ namespace cmvr::device {
class RH56DFTPDexhand final : public AbstractDexHand {
public:
using FingerType = AbstractDexHand::FingerType;
using ResultantForce = AbstractDexHand::ResultantForce;
using TactileRegion = AbstractDexHand::TactileRegion;
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
using TactileRegionData = AbstractDexHand::TactileRegionData;
@ -72,6 +73,7 @@ namespace cmvr::device {
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;
private:
struct TactileBufferLease {

9
cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h
View File

@ -4,6 +4,7 @@
#include <array>
#include <cstddef>
#include <cstdint>
#include <stdexcept>
#include "../../abstract_dexhand.h"
@ -37,17 +38,17 @@ namespace cmvr::device {
static constexpr size_t POINT_COUNT = 1062;
void clear() {
values_.fill(0);
values_.fill(AbstractDexHand::TactilePoint{});
}
void assignRegionData(AbstractDexHand::FingerType finger,
AbstractDexHand::TactileRegion region,
const AbstractDexHand::TactilePoint* input,
const uint16_t* input,
int valueCount) {
const auto& layout = regionLayout(finger, region);
auto* destination = values_.data() + layout.offset;
const auto sanitize_point = [](const AbstractDexHand::TactilePoint value) {
return value > static_cast<AbstractDexHand::TactilePoint>(4096) ? 0 : value;
const auto sanitize_point = [](const uint16_t value) {
return AbstractDexHand::TactilePoint::fromFz(value > 4096U ? 0 : static_cast<int32_t>(value));
};
int input_index = 0;

23
cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp
View File

@ -328,6 +328,9 @@ RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg)
if (dexhandCfg_.port() > 0) {
port_ = dexhandCfg_.port();
}
if (dexhandCfg_.poll_interval_ms() > 0) {
tactile_poll_interval_ = std::chrono::milliseconds(dexhandCfg_.poll_interval_ms());
}
initializeTactileBuffers();
}
@ -465,10 +468,30 @@ TactileRegionData RH56DFTPDexhand::getSensorData(FingerType finger, TactileRegio
return acquireRegionData(finger, region);
}
RH56DFTPDexhand::ResultantForce RH56DFTPDexhand::getResultantForce(FingerType finger, TactileRegion region) {
const auto region_data = getSensorData(finger, region);
if (!region_data.valid()) {
throw std::runtime_error("RH56DFTPDexhand tactile region data is not ready.");
}
ResultantForce resultant{};
for (int row = 0; row < region_data.view.rows; ++row) {
const auto* row_data = region_data.view.rowData(row);
for (int col = 0; col < region_data.view.cols; ++col) {
resultant.fx += row_data[col].fx;
resultant.fy += row_data[col].fy;
resultant.fz += row_data[col].fz;
}
}
return resultant;
}
void RH56DFTPDexhand::parseXmlConfig() {
id_ = cfg_.getAttrString("id");
ip_address_ = cfg_.getAttrString("ip_address");
port_ = cfg_.getAttrDefault("port", kDefaultPort);
tactile_poll_interval_ = std::chrono::milliseconds(
std::max(1, cfg_.getAttrDefault("poll_interval_ms", static_cast<int>(tactile_poll_interval_.count()))));
}
void RH56DFTPDexhand::initializeTactileBuffers() {

4
cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp
View File

@ -99,7 +99,7 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
last_pressure_sum = 0.0;
last_pressure_peak = 0.0;
for (int index = 0; index < region_data.view.pointCount(); ++index) {
const double pressure = static_cast<double>(region_data.view.data[index]);
const double pressure = static_cast<double>(region_data.view.data[index].fz);
last_pressure_sum += pressure;
if (pressure > last_pressure_peak) {
last_pressure_peak = pressure;
@ -134,4 +134,4 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
<< "\n";
EXPECT_GT(point_count, 0);
}
}

2
cmvr-es/hardware/CMakeLists.txt
View File

@ -1,9 +1,11 @@
add_library(hardware SHARED
src/serial_interface.cpp
src/esp32_serial_port.cpp
src/posix_serial_transport.cpp
)
target_include_directories(hardware PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
)

38
cmvr-es/hardware/include/abstract_serial_transport.h Normal file
View File

@ -0,0 +1,38 @@
//
// Created by Codex on 2026/4/3.
//
#ifndef CMVR_ES_ABSTRACT_SERIAL_TRANSPORT_H
#define CMVR_ES_ABSTRACT_SERIAL_TRANSPORT_H
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <string>
#include <vector>
namespace cmvr {
class AbstractSerialTransport {
public:
struct Config {
std::string port_name;
int baud_rate{0};
};
virtual ~AbstractSerialTransport() = default;
virtual bool open(const Config& config) = 0;
virtual bool close() = 0;
virtual bool isOpen() const = 0;
virtual bool flushInput() = 0;
virtual bool write(const std::vector<uint8_t>& data) = 0;
virtual bool read(
size_t min_size,
std::chrono::milliseconds timeout,
std::vector<uint8_t>& out_buffer) = 0;
virtual std::string lastError() const = 0;
};
}
#endif // CMVR_ES_ABSTRACT_SERIAL_TRANSPORT_H

41
cmvr-es/hardware/include/posix_serial_transport.h Normal file
View File

@ -0,0 +1,41 @@
//
// Created by Codex on 2026/4/3.
//
#ifndef CMVR_ES_POSIX_SERIAL_TRANSPORT_H
#define CMVR_ES_POSIX_SERIAL_TRANSPORT_H
#include <mutex>
#include <string>
#include "abstract_serial_transport.h"
namespace cmvr {
class PosixSerialTransport final : public AbstractSerialTransport {
public:
PosixSerialTransport() = default;
~PosixSerialTransport() override;
bool open(const Config& config) override;
bool close() override;
bool isOpen() const override;
bool flushInput() override;
bool write(const std::vector<uint8_t>& data) override;
bool read(
size_t min_size,
std::chrono::milliseconds timeout,
std::vector<uint8_t>& out_buffer) override;
std::string lastError() const override;
private:
bool closeUnlocked();
void setLastErrorUnlocked(std::string error);
int fd_{-1};
std::string last_error_;
mutable std::mutex io_mutex_;
};
}
#endif // CMVR_ES_POSIX_SERIAL_TRANSPORT_H

226
cmvr-es/hardware/src/posix_serial_transport.cpp Normal file
View File

@ -0,0 +1,226 @@
//
// Created by Codex on 2026/4/3.
//
#include "../include/posix_serial_transport.h"
#include <algorithm>
#include <array>
#include <cerrno>
#include <cstring>
#include <fcntl.h>
#include <poll.h>
#include <stdexcept>
#include <sys/ioctl.h>
#include <unistd.h>
#include <asm/termbits.h>
namespace cmvr {
PosixSerialTransport::~PosixSerialTransport() {
close();
}
bool PosixSerialTransport::open(const Config& config) {
std::lock_guard<std::mutex> lock(io_mutex_);
closeUnlocked();
last_error_.clear();
if (config.port_name.empty()) {
setLastErrorUnlocked("Serial port name must not be empty.");
return false;
}
if (config.baud_rate <= 0) {
setLastErrorUnlocked("Serial baud rate must be positive.");
return false;
}
fd_ = ::open(config.port_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd_ < 0) {
setLastErrorUnlocked(
"Failed to open serial port " + config.port_name + ": " + std::string(std::strerror(errno)));
return false;
}
struct termios2 options {};
if (::ioctl(fd_, TCGETS2, &options) != 0) {
setLastErrorUnlocked("Failed to query serial attributes: " + std::string(std::strerror(errno)));
closeUnlocked();
return false;
}
options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXOFF | IXANY);
options.c_oflag &= ~OPOST;
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
options.c_cflag &= ~(CSIZE | PARENB | CBAUD);
options.c_cflag |= (CS8 | CLOCAL | CREAD | BOTHER);
options.c_cflag &= ~CRTSCTS;
options.c_cflag &= ~CSTOPB;
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 0;
options.c_ispeed = static_cast<speed_t>(config.baud_rate);
options.c_ospeed = static_cast<speed_t>(config.baud_rate);
if (::ioctl(fd_, TCFLSH, TCIOFLUSH) != 0) {
setLastErrorUnlocked("Failed to flush serial buffers: " + std::string(std::strerror(errno)));
closeUnlocked();
return false;
}
if (::ioctl(fd_, TCSETS2, &options) != 0) {
setLastErrorUnlocked("Failed to apply serial attributes: " + std::string(std::strerror(errno)));
closeUnlocked();
return false;
}
return true;
}
bool PosixSerialTransport::close() {
std::lock_guard<std::mutex> lock(io_mutex_);
last_error_.clear();
return closeUnlocked();
}
bool PosixSerialTransport::isOpen() const {
std::lock_guard<std::mutex> lock(io_mutex_);
return fd_ >= 0;
}
bool PosixSerialTransport::flushInput() {
std::lock_guard<std::mutex> lock(io_mutex_);
last_error_.clear();
if (fd_ < 0) {
setLastErrorUnlocked("Serial port is not open.");
return false;
}
if (::ioctl(fd_, TCFLSH, TCIFLUSH) != 0) {
setLastErrorUnlocked("Failed to flush serial input: " + std::string(std::strerror(errno)));
return false;
}
return true;
}
bool PosixSerialTransport::write(const std::vector<uint8_t>& data) {
std::lock_guard<std::mutex> lock(io_mutex_);
last_error_.clear();
if (fd_ < 0) {
setLastErrorUnlocked("Serial port is not open.");
return false;
}
size_t offset = 0;
while (offset < data.size()) {
const ssize_t written = ::write(fd_, data.data() + offset, data.size() - offset);
if (written > 0) {
offset += static_cast<size_t>(written);
continue;
}
if (written < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
pollfd descriptor{};
descriptor.fd = fd_;
descriptor.events = POLLOUT;
if (::poll(&descriptor, 1, 50) < 0) {
setLastErrorUnlocked(
"Polling serial write readiness failed: " + std::string(std::strerror(errno)));
return false;
}
continue;
}
setLastErrorUnlocked("Writing to serial port failed: " + std::string(std::strerror(errno)));
return false;
}
return true;
}
bool PosixSerialTransport::read(
const size_t min_size,
const std::chrono::milliseconds timeout,
std::vector<uint8_t>& out_buffer) {
std::lock_guard<std::mutex> lock(io_mutex_);
last_error_.clear();
out_buffer.clear();
if (fd_ < 0) {
setLastErrorUnlocked("Serial port is not open.");
return false;
}
out_buffer.reserve(min_size + 16U);
const auto deadline = std::chrono::steady_clock::now() + timeout;
std::array<uint8_t, 256> temp{};
while (out_buffer.size() < min_size) {
const auto now = std::chrono::steady_clock::now();
if (now >= deadline) {
break;
}
const auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now);
const int wait_ms = std::max(1, static_cast<int>(remaining.count()));
pollfd descriptor{};
descriptor.fd = fd_;
descriptor.events = POLLIN;
const int rc = ::poll(&descriptor, 1, wait_ms);
if (rc < 0) {
if (errno == EINTR) {
continue;
}
setLastErrorUnlocked(
"Polling serial read readiness failed: " + std::string(std::strerror(errno)));
return false;
}
if (rc == 0 || (descriptor.revents & POLLIN) == 0) {
continue;
}
const ssize_t bytes_read = ::read(fd_, temp.data(), temp.size());
if (bytes_read > 0) {
out_buffer.insert(out_buffer.end(), temp.begin(), temp.begin() + bytes_read);
continue;
}
if (bytes_read < 0 && (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR)) {
continue;
}
if (bytes_read < 0) {
setLastErrorUnlocked("Reading serial port failed: " + std::string(std::strerror(errno)));
return false;
}
}
if (out_buffer.size() < min_size) {
setLastErrorUnlocked(
"Serial read timeout, expected at least " + std::to_string(min_size) +
" bytes, got " + std::to_string(out_buffer.size()) + ".");
return false;
}
return true;
}
std::string PosixSerialTransport::lastError() const {
std::lock_guard<std::mutex> lock(io_mutex_);
return last_error_;
}
bool PosixSerialTransport::closeUnlocked() {
if (fd_ < 0) {
return true;
}
const int fd = fd_;
fd_ = -1;
if (::close(fd) != 0) {
setLastErrorUnlocked("Failed to close serial port: " + std::string(std::strerror(errno)));
return false;
}
return true;
}
void PosixSerialTransport::setLastErrorUnlocked(std::string error) {
last_error_ = std::move(error);
}
}

3
cmvr-es/service/grpc/src/grpc_dexhand_service.cpp
View File

@ -72,7 +72,8 @@ void fillSensorData(const AbstractDexHand::TactileRegionData& tactile_data,
auto* row_data = sensor_data->add_data();
const auto* values = tactile_data.view.rowData(row);
for (int col = 0; col < tactile_data.view.cols; ++col) {
row_data->add_values(static_cast<int32_t>(values[col]));
// Keep the existing scalar wire format by exposing the normal-force projection.
row_data->add_values(static_cast<int32_t>(values[col].fz));
}
}
}

5
cmvr-es/service/grpc/src/grpc_hlc_service.cpp
View File

@ -93,7 +93,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
bool align_reached = false;
bool touch_triggered = false;
auto last_logged_status = cmvr::app::TouchScreenApp::Status::IDLE;
constexpr double kControlDt = 0.01;
constexpr double kControlDt = 0.001;
auto next_step_time = std::chrono::steady_clock::now();
while (touch_app_.isBusy()) {
if (context != nullptr && context->IsCancelled()) {
@ -122,8 +122,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
<< cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase())
<< ", active_tag=" << touch_app_.lastActiveTagId()
<< ", nonzero_count=" << touch_app_.lastTouchNonzeroCount()
<< ", pressure_sum=" << touch_app_.lastTouchPressureSum()
<< ", pressure_peak=" << touch_app_.lastTouchPressurePeak();
<< ", pressure_sum=" << touch_app_.lastTouchPressureSum();
last_logged_status = touch_app_.lastStatus();
}

340
compile_commands.json
View File

File diff suppressed because it is too large Load Diff

31
protos/cmvr/config/dexhand_config/dexhand_config.proto
View File

@ -6,10 +6,39 @@ message RH56DFTPDexHandConfig{
string ip = 2;
int32 port = 3;
bool enable = 4;
int32 poll_interval_ms = 5;
}
enum PX6AXGen3PollingReadMode {
PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_AND_RESULTANT_FORCE = 0;
PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE = 1;
PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE = 2;
}
message PX6AXGen3{
string id = 1;
string serial_port = 2;
string sensor_model = 3;
int32 module_id = 4;
int32 baud_rate = 5;
int32 distributed_length = 6;
int32 resultant_length = 7;
int32 poll_interval_ms = 8;
bool auto_calibrate = 9;
bool enable = 10;
int32 response_timeout_ms = 11;
int32 tactile_rows = 12;
int32 tactile_cols = 13;
int32 response_header_bytes = 14;
string tactile_finger = 15;
string tactile_region = 16;
string sensor_name = 17;
PX6AXGen3PollingReadMode polling_read_mode = 18;
}
message DexHandConfig{
repeated RH56DFTPDexHandConfig rh56dftp_dexhands = 1;
}
repeated PX6AXGen3 px_6ax_gen3 = 2;
}

10
protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto
View File

@ -69,6 +69,11 @@ enum TouchScreenTactileRegion {
TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4;
}
enum TouchScreenTactileCriterion {
TOUCH_SCREEN_TACTILE_CRITERION_FZ = 0;
TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE = 1;
}
enum TouchScreenAlignMode {
TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION = 0;
TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION = 1;
@ -76,6 +81,9 @@ enum TouchScreenAlignMode {
}
message TouchScreenAppConfig {
reserved 39;
reserved "tactile_pressure_peak_threshold";
optional string robot_id = 40;
optional string dexhand_id = 41;
optional string camera_id = 42;
@ -141,7 +149,7 @@ message TouchScreenAppConfig {
optional TouchScreenFingerType tactile_finger = 36;
optional TouchScreenTactileRegion tactile_region = 37;
optional TouchScreenTactileCriterion tactile_criterion = 59;
optional double tactile_pressure_sum_threshold = 38;
optional double tactile_pressure_peak_threshold = 39;
optional int32 tactile_nonzero_count_threshold = 56;
}

383
script/USB_UI.py Normal file
View File

@ -0,0 +1,383 @@
import serial
import time
import serial.tools.list_ports
from typing import Optional, Dict, List
import logging
import tkinter as tk
from tkinter import ttk, messagebox, scrolledtext
# 配置日志系统
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(levelname)s - %(message)s'
)
logger = logging.getLogger(__name__)
# 传感器型号与分布力数据长度的映射（单位：字节）
SENSOR_DISTRIBUTED_LENGTH = {
"S1813_elite": 93,
"S2015_elite": 156,
"S1813_core": 153,
"S2716_core": 348,
"S3013_core": 288,
"M2826_omega": 381,
"L3530_omega": 405,
"S1610_elite": 75,
"M2324_core": 204,
"M3025_core": 231,
"L5325_omega": 717,
"M2020_elite": 27
}
class SensorUI:
def __init__(self, root):
self.root = root
self.root.title("传感器数据采集系统")
self.root.geometry("900x700")
self.root.resizable(True, True)
self.ser = None # 串口对象
self.running = False # 数据采集状态
self.init_ui()
def init_ui(self):
# ===== 配置区域 =====
config_frame = ttk.LabelFrame(self.root, text="设备配置")
config_frame.pack(fill=tk.X, padx=10, pady=5)
# COM口选择
ttk.Label(config_frame, text="COM口:").grid(row=0, column=0, padx=5, pady=5, sticky=tk.W)
self.com_var = tk.StringVar()
self.com_combo = ttk.Combobox(config_frame, textvariable=self.com_var, width=10)
self.com_combo.grid(row=0, column=1, padx=5, pady=5)
ttk.Button(config_frame, text="刷新", command=self.refresh_com_ports).grid(row=0, column=2, padx=5, pady=5)
# 传感器型号选择（联动数据长度）
ttk.Label(config_frame, text="传感器型号:").grid(row=0, column=3, padx=5, pady=5, sticky=tk.W)
self.sensor_model = tk.StringVar(value="S1813_elite")
self.sensor_combo = ttk.Combobox(config_frame, textvariable=self.sensor_model, width=15)
self.sensor_combo['values'] = list(SENSOR_DISTRIBUTED_LENGTH.keys())
self.sensor_combo.bind("<<ComboboxSelected>>", self.update_length_by_model) # 选择型号时更新长度
self.sensor_combo.grid(row=0, column=4, padx=5, pady=5)
# 模组号选择（联动设备地址）
ttk.Label(config_frame, text="模组号:").grid(row=0, column=5, padx=5, pady=5, sticky=tk.W)
self.module_id = tk.StringVar(value="00") # 默认模组号00
self.module_combo = ttk.Combobox(config_frame, textvariable=self.module_id, width=5)
self.module_combo['values'] = ["00", "01", "02", "03", "04", "05", "06", "07"]
self.module_combo.bind("<<ComboboxSelected>>", self.update_device_addr) # 选择模组号时更新设备地址
self.module_combo.grid(row=0, column=6, padx=5, pady=5)
# 设备地址显示（只读，由模组号计算得出）
ttk.Label(config_frame, text="设备地址:").grid(row=0, column=7, padx=5, pady=5, sticky=tk.W)
self.device_addr_var = tk.StringVar(value="01") # 模组号00对应设备地址01
ttk.Label(config_frame, textvariable=self.device_addr_var, width=5).grid(row=0, column=8, padx=5, pady=5)
# 数据长度设置（可手动修改）
ttk.Label(config_frame, text="分布力数据长度(字节):").grid(row=1, column=0, padx=5, pady=5, sticky=tk.W)
self.dist_length = tk.StringVar(value=str(SENSOR_DISTRIBUTED_LENGTH["S1813_elite"]))
self.dist_entry = ttk.Entry(config_frame, textvariable=self.dist_length, width=10)
self.dist_entry.grid(row=1, column=1, padx=5, pady=5)
ttk.Label(config_frame, text="合力数据长度(字节):").grid(row=1, column=2, padx=5, pady=5, sticky=tk.W)
self.result_length = tk.StringVar(value="3")
ttk.Entry(config_frame, textvariable=self.result_length, width=10).grid(row=1, column=3, padx=5, pady=5)
# 控制按钮
btn_frame = ttk.Frame(config_frame)
btn_frame.grid(row=1, column=4, columnspan=5, padx=5, pady=5)
ttk.Button(btn_frame, text="连接设备", command=self.connect_device).pack(side=tk.LEFT, padx=5)
ttk.Button(btn_frame, text="传感器标定", command=self.calibrate_sensor).pack(side=tk.LEFT, padx=5)
ttk.Button(btn_frame, text="开始采集", command=self.start_collection).pack(side=tk.LEFT, padx=5)
ttk.Button(btn_frame, text="停止采集", command=self.stop_collection).pack(side=tk.LEFT, padx=5)
# ===== 日志显示区域 =====
log_frame = ttk.LabelFrame(self.root, text="数据日志")
log_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=5)
self.log_text = scrolledtext.ScrolledText(log_frame, wrap=tk.WORD, state=tk.DISABLED)
self.log_text.pack(fill=tk.BOTH, expand=True, padx=5, pady=5)
# 初始化COM口列表和设备地址
self.refresh_com_ports()
self.update_device_addr()
def refresh_com_ports(self):
"""刷新COM口列表"""
ports = [port.device for port in serial.tools.list_ports.comports()]
self.com_combo['values'] = ports
if ports:
self.com_var.set(ports[0])
def update_device_addr(self, event=None):
"""根据模组号计算设备地址（设备地址 = 模组号 + 1，转为十六进制）"""
try:
module_dec = int(self.module_id.get(), 16) # 模组号转为十进制
device_dec = module_dec + 1 # 设备地址 = 模组号 + 1
device_hex = f"{device_dec:02X}" # 转为2位十六进制（大写）
self.device_addr_var.set(device_hex)
self.log(f"模组号{self.module_id.get()} → 设备地址{device_hex}")
except ValueError:
self.log("模组号格式错误，无法计算设备地址")
def update_length_by_model(self, event=None):
"""根据选择的传感器型号自动更新分布力数据长度"""
model = self.sensor_model.get()
if model in SENSOR_DISTRIBUTED_LENGTH:
self.dist_length.set(str(SENSOR_DISTRIBUTED_LENGTH[model]))
self.log(f"自动更新{model}的分布力数据长度为: {SENSOR_DISTRIBUTED_LENGTH[model]}字节")
def log(self, message: str):
"""在UI中显示日志"""
self.log_text.config(state=tk.NORMAL)
self.log_text.insert(tk.END, message + "\n")
self.log_text.see(tk.END)
self.log_text.config(state=tk.DISABLED)
def connect_device(self):
"""连接串口设备"""
if self.ser and self.ser.is_open:
self.ser.close()
com_port = self.com_var.get()
if not com_port:
messagebox.showerror("错误", "请选择COM口")
return
try:
self.ser = serial.Serial(
port=com_port,
baudrate=921600,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0.1, # 读取超时缩短至0.1秒
write_timeout=0.1, # 写入超时缩短
inter_byte_timeout=0.0005, # 字节间隔超时缩短
xonxoff=False,
rtscts=False
)
if self.ser.is_open:
self.log(f"串口连接成功: {com_port}（波特率921600）")
logger.info(f"串口连接成功: {com_port}")
except serial.SerialException as e:
self.log(f"串口连接失败: {str(e)}")
logger.error(f"串口连接失败: {str(e)}")
messagebox.showerror("连接失败", str(e))
def get_commands(self) -> Dict[str, str]:
"""生成命令帧"""
model = self.sensor_model.get()
device_addr = self.device_addr_var.get() # 设备地址（十六进制，如"01"）
dist_len = int(self.dist_length.get()) # 分布力数据长度（十进制）
# 1. 分布力命令帧：
# 命令帧格式：55 AA 09 00 [设备地址] 00 FB 0E 04 00 00 [长度低8位] [长度高8位]
len_low = dist_len & 0xFF # 数据长度低8位
len_high = (dist_len >> 8) & 0xFF # 数据长度高8位
len_hex = f"{len_low:02X}{len_high:02X}" # 小端格式（低位+高位）
distributed_frame = f"55 AA 09 00 {device_addr} 00 FB 0E 04 00 00 {len_hex[0:2]} {len_hex[2:4]}"
# 2. 标定命令帧：
# 格式：55 AA 0A 00 [设备地址] 00 79 03 00 00 00 01 00 01
calibration_frame = f"55 AA 0A 00 {device_addr} 00 79 03 00 00 00 01 00 01"
# 3. 合力命令帧：
# 格式：55 AA 09 00 [设备地址] 00 FB F0 03 00 00 03 00
resultant_frame = f"55 AA 09 00 {device_addr} 00 FB F0 03 00 00 03 00"
return {
"calibration": calibration_frame,
"resultant_force": resultant_frame,
"distributed_force": distributed_frame
}
def calculate_lrc(self, data: bytes) -> int:
"""计算LRC校验值"""
lrc = 0
for byte in data:
lrc = (lrc + byte) & 0xFF
lrc = ((~lrc) + 1) & 0xFF
return lrc
def send_command(self, command_type: str) -> Optional[bytes]:
"""发送命令并返回响应"""
if not self.ser or not self.ser.is_open:
self.log("未连接设备，请先连接")
return None
commands = self.get_commands()
if command_type not in commands:
self.log(f"未知命令类型: {command_type}")
return None
# 生成带LRC的命令帧
frame = commands[command_type].replace(" ", "") # 移除空格
try:
frame_bytes = bytes.fromhex(frame)
lrc = self.calculate_lrc(frame_bytes)
frame_with_lrc = frame + f"{lrc:02X}" # 追加LRC
except ValueError as e:
self.log(f"生成命令帧失败: {str(e)}")
return None
# 发送命令并打印详情（含设备地址和数据长度）
try:
data_bytes = bytes.fromhex(frame_with_lrc)
self.ser.write(data_bytes)
print(f"发送{command_type}命令（设备地址={self.device_addr_var.get()}）: {frame_with_lrc}")
except serial.SerialException as e:
self.log(f"命令发送失败: {str(e)}")
return None
# 读取响应
time.sleep(0.01)
response = b""
start_time = time.time()
while time.time() - start_time < 0.2: # 1.5秒超时
if self.ser.in_waiting > 0:
response += self.ser.read(self.ser.in_waiting) # 一次性读取所有等待的数据，减少循环次数
# 若已读取到预期长度（根据命令类型），提前退出
if command_type == "resultant_force" and len(response) >= 14 + 3:
break
if command_type == "distributed_force" and len(response) >= 14 + int(self.dist_length.get()):
break
# 极短延迟
time.sleep(0.0001)
if response:
print(f"收到响应（{len(response)}字节）: {response.hex()}")
return response
print("未收到响应")
return None
def calibrate_sensor(self):
"""标定"""
response = self.send_command("calibration")
if response:
# 校验帧头
if response[:2].hex() == "aa55":
self.log(f"{self.sensor_model.get()} 标定成功")
else:
self.log("标定响应格式错误")
else:
self.log("标定失败")
def parse_resultant_force(self, data: bytes) -> Optional[Dict]:
"""解析合力数据"""
if len(data) != int(self.result_length.get()):
print(f"合力数据长度错误（预期{self.result_length.get()}字节，实际{len(data)}字节）")
return None
byte1, byte2, byte3 = data[0], data[1], data[2]
val1 = byte1 if byte1 <= 127 else byte1 - 256
val2 = byte2 if byte2 <= 127 else byte2 - 256
val3 = byte3
# 转换为物理单位
force_x = val1 * 0.1
force_y = val2 * 0.1
force_z = val3 * 0.1
return {
"raw": (byte1, byte2, byte3),
"parsed": (val1, val2, val3),
"force_N": (force_x, force_y, force_z)
}
def parse_distributed_force(self, data: bytes) -> List[Dict]:
"""解析分布力数据"""
parsed = []
total_len = int(self.dist_length.get())
if len(data) < total_len:
print(f"分布力数据长度不足（预期{total_len}字节，实际{len(data)}字节）")
total_len = len(data)
group_count = total_len // 3 # 每3字节一组
for i in range(group_count):
byte1 = data[i*3]
byte2 = data[i*3 + 1]
byte3 = data[i*3 + 2]
val1 = byte1 if byte1 <= 127 else byte1 - 256
val2 = byte2 if byte2 <= 127 else byte2 - 256
val3 = byte3
force_x = val1 * 0.1
force_y = val2 * 0.1
force_z = val3 * 0.1
parsed.append({
"index": i,
"raw": (byte1, byte2, byte3),
"parsed": (val1, val2, val3),
"force_N": (force_x, force_y, force_z)
})
return parsed
def collect_data(self):
"""循环采集数据"""
if not self.running or not self.ser or not self.ser.is_open:
return
# 读取合力数据
result_response = self.send_command("resultant_force")
if result_response and len(result_response) > 14:
result_data = result_response[14:14 + int(self.result_length.get())]
result_parsed = self.parse_resultant_force(result_data)
if result_parsed:
print("\n===== 合力数据 =====")
print(f"合力数据原始字节: 0x{result_parsed['raw'][0]:02X}, 0x{result_parsed['raw'][1]:02X}, 0x{result_parsed['raw'][2]:02X}")
print(f"合力数据解析值: X={result_parsed['parsed'][0]}, Y={result_parsed['parsed'][1]}, Z={result_parsed['parsed'][2]}")
self.log(f"合力数据: X={result_parsed['force_N'][0]:.1f}N, Y={result_parsed['force_N'][1]:.1f}N, Z={result_parsed['force_N'][2]:.1f}N")
self.log(f"")
# 读取分布力数据
dist_response = self.send_command("distributed_force")
if dist_response and len(dist_response) > 14:
dist_data = dist_response[14:14 + int(self.dist_length.get())]
dist_parsed = self.parse_distributed_force(dist_data)
if dist_parsed:
print("\n===== 分布力数据 =====")
print(f"总测点: {len(dist_parsed)}")
for point in dist_parsed:
print(f"测点{point['index']:02d} | 物理值: X={point['force_N'][0]:.1f}N, Y={point['force_N'][1]:.1f}N, Z={point['force_N'][2]:.1f}N")
print(f"------------------------------------------")
# 继续循环采集,10ms间隔
self.root.after(10, self.collect_data)
def start_collection(self):
"""开始数据采集"""
if self.running:
self.log("已在采集数据中")
return
if not self.ser or not self.ser.is_open:
messagebox.showwarning("警告", "请先连接设备")
return
self.running = True
self.log("开始数据采集...")
self.collect_data()
def stop_collection(self):
"""停止数据采集"""
self.running = False
self.log("已停止数据采集")
def on_close(self):
"""关闭窗口时释放资源"""
self.running = False
if self.ser and self.ser.is_open:
self.ser.close()
self.root.destroy()
if __name__ == "__main__":
root = tk.Tk()
app = SensorUI(root)
root.protocol("WM_DELETE_WINDOW", app.on_close)
root.mainloop()