From baf883bc27f28feb7fa3a1d6836eb6684645527e Mon Sep 17 00:00:00 2001 From: lgv Date: Thu, 26 Mar 2026 15:35:00 +0800 Subject: [PATCH] fix: add touch by distance --- .../applications/include/touch_screen_app.h | 37 +- cmvr-es/applications/src/touch_screen_app.cpp | 510 ++++++--- .../touch_screen_app_config.pb.txt | 54 +- .../config_helper/src/config_setting.cpp | 72 +- .../biohead/biohead_esp32/CMakeLists.txt | 4 +- .../realsense_camera/src/realsense_camera.cpp | 2 +- cmvr-es/devices/dexhand/abstract_dexhand.h | 352 +++---- .../dexhand/rh56dftp_dexhand/CMakeLists.txt | 16 +- .../include/rh56dftp_dexhand.h | 126 ++- .../include/rh56dftp_tactile_buffer.h | 136 +++ .../rh56dftp_dexhand/src/rh56dftp_dexhand.cpp | 989 +++++++++++------- .../src/rh56dftp_dexhand_test.cpp | 137 +++ cmvr-es/devices/robot/abstract_robot.h | 20 +- .../humanoid_robot/include/humanoid_robot.h | 28 +- .../humanoid_robot/src/humanoid_robot.cpp | 796 ++++---------- .../src/humanoid_robot_test.cpp | 254 +++++ .../include/pinocchio_dls_ik_solver.h | 10 +- .../ik_solver/src/pinocchio_dls_ik_solver.cpp | 116 +- .../service/grpc/src/grpc_camera_service.cpp | 2 +- .../service/grpc/src/grpc_dexhand_service.cpp | 356 ++++--- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 13 +- .../grpc/src/grpc_humanoid_robot_service.cpp | 2 +- compile_commands.json | 6 + .../touch_screen_app_config.proto | 10 +- 24 files changed, 2395 insertions(+), 1653 deletions(-) create mode 100644 cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h create mode 100644 cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index b0fc5f9d..8ab1cade 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -150,18 +150,32 @@ public: // 为 true 时对准完成后暂停，不自动进入触控阶段。 bool pause_after_align_reached{false}; - // 触控阶段：speedL 目标 twist。当前按末端 Tool 坐标系解释，字段名保留兼容。 + // 触控阶段前进方向。当前按末端 Tool 坐标系解释，字段名保留兼容。 + // 为 true 时，TOUCHING 阶段使用 speedL；为 false 时使用 moveL。 + // 当前 moveL 路径为同步前进，执行完成后直接结束流程，并按配置决定是否回初始位姿。 + bool touch_use_speedl{true}; // 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。 // 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。 + // 当 TOUCHING 使用 moveL 时，会取其线速度方向并按 touch_forward_l 构造位移目标。 Eigen::Matrix touch_twist_base{ (Eigen::Matrix() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()}; // 触控阶段 speedL 的加速度参数。 - double touch_acceleration{3.0}; - // 前进触控阶段的最大持续时间，单位秒。 - // 该时间由 TouchScreenApp 状态机自行计时，不直接传给 robot->speedL(time)。 - // 大于 0 时，达到该时间后无论压力是否达阈值，都会立即进入回退阶段。 + double touch_speedl_acceleration{3.0}; + // 触控阶段使用 moveL 时，沿 touch_twist_base 线速度方向前进的距离，单位米。 + double touch_forward_l{0.08}; + // 触控阶段使用 moveL 时的末端速度，单位 m/s。 + double touch_movel_speed{0.25}; + // 触控阶段使用 moveL 时的末端加速度，单位 m/s^2。 + double touch_movel_acceleration{1.2}; + // 触控阶段使用 moveL 时的末端 jerk，单位 m/s^3。 + double touch_movel_jerk{5.0}; + // 触控阶段使用 moveL 时的关节速度上限；为空时退回 robot->moveL 默认值。 + std::vector touch_movel_qd_max{2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5}; + // 前进触控阶段的最大累计位移，单位米。 + // 该距离仅在 TOUCHING 使用 speedL 时生效，由 TouchScreenApp 根据末端相对触控起点的累计位移判断。 + // 大于 0 时，达到该距离后无论压力是否达阈值，都会立即进入回退阶段。 // 小于等于 0 时，表示不启用这条限制。 - double touch_forward_duration_s{1.0}; + double touch_speedl_forward_l{0.08}; // 接触后停留与回退。当前按末端 Tool 坐标系解释。 // 检测到接触后在当前位置停留的时间，单位秒。 @@ -169,16 +183,17 @@ public: double dwell_time_s{0.05}; // 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。 // 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。 + // 回退阶段统一使用 speedL。 Eigen::Matrix retract_twist_base{ (Eigen::Matrix() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()}; // 回退阶段 speedL 的加速度参数。 double retract_acceleration{3.0}; - // 回退阶段持续时间，单位秒。 + // 回退阶段 speedL 持续时间，单位秒。 double retract_duration_s{0.8}; // 指尖触觉判据。 // 使用哪根手指的触觉阵列判断是否接触。 - device::FingerType tactile_finger{device::FingerType::INDEX}; + device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX}; // 使用该手指的哪个触觉区域。 TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER}; // 触觉压力和阈值；总和超过该值认为已经接触。 @@ -201,6 +216,7 @@ public: bool setOptionsFromConfig(); bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config); + bool setTouchSpeedlForwardL(double forward_l); bool startFromPixel(int u, int v); bool step(); @@ -246,8 +262,11 @@ private: bool sendZeroJointVelocity() const; bool holdCurrentControlledPosition() const; bool moveToInitPositionIfEnabled() const; + bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; + void logTouchingSpeedLState() const; bool startTouchPhase(); + bool handleTouchTriggered(bool stop_forward_motion); bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract); void enterFailed(Status status); @@ -281,6 +300,8 @@ private: double last_touch_pressure_sum_{0.0}; double last_touch_pressure_peak_{0.0}; Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; + bool touch_start_position_valid_{false}; + Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; Clock::time_point phase_start_time_{}; Status final_status_after_retract_{Status::DONE}; diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index 66e53dbf..f20db377 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -22,14 +23,48 @@ std::vector toStdVector6(const Eigen::Matrix& twist) { return out; } -void accumulateMatrixStats(const std::vector>& matrix, +bool computeLinearMoveDeltaTool(const Eigen::Matrix& twist_base, + const double move_length, + Eigen::Vector3d& delta_out) { + if (!std::isfinite(move_length) || move_length <= 0.0) { + return false; + } + + const Eigen::Vector3d linear = twist_base.head<3>(); + if (!linear.allFinite()) { + return false; + } + + const double linear_norm = linear.norm(); + if (!std::isfinite(linear_norm) || linear_norm <= 1e-9) { + return false; + } + + delta_out = linear / linear_norm * move_length; + return delta_out.allFinite(); +} + +bool isTouchTriggered(const TouchScreenApp::Options& options, + const double pressure_sum, + const double pressure_peak) { + return pressure_sum >= options.tactile_pressure_sum_threshold || + (options.tactile_pressure_peak_threshold > 0.0 && + pressure_peak >= options.tactile_pressure_peak_threshold); +} + +void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& matrix, double& sum_out, double& peak_out) { - for (const auto& row : matrix) { - for (const auto value : row) { - const double v = static_cast(value); - sum_out += v; - peak_out = std::max(peak_out, v); + if (!matrix.valid()) { + return; + } + + for (int row = 0; row < matrix.rows; ++row) { + const auto* row_data = matrix.rowData(row); + for (int col = 0; col < matrix.cols; ++col) { + const double value = static_cast(row_data[col]); + sum_out += value; + peak_out = std::max(peak_out, value); } } } @@ -84,17 +119,34 @@ double rotationErrorRad(const Eigen::Matrix3d& R_current, return std::acos(cos_angle); } -device::TactileRegion toDeviceTactileRegion(const TouchScreenApp::TactileRegion region) { +bool appendRequestedTactileRegions( + const device::AbstractDexHand::FingerType finger, + const TouchScreenApp::TactileRegion region, + std::vector& regions_out) { + using DeviceTactileRegion = device::AbstractDexHand::TactileRegion; + switch (region) { - case TouchScreenApp::TactileRegion::TIP: return device::TactileRegion::TIP; - case TouchScreenApp::TactileRegion::FINGER: return device::TactileRegion::FINGER; - case TouchScreenApp::TactileRegion::PAD: return device::TactileRegion::PAD; + case TouchScreenApp::TactileRegion::TIP: + regions_out.emplace_back(finger, DeviceTactileRegion::TIP); + return true; + case TouchScreenApp::TactileRegion::FINGER: + regions_out.emplace_back(finger, DeviceTactileRegion::FINGER); + return true; + case TouchScreenApp::TactileRegion::PAD: + regions_out.emplace_back(finger, DeviceTactileRegion::PAD); + return true; case TouchScreenApp::TactileRegion::TIP_AND_FINGER: - return device::TactileRegion::TIP_AND_FINGER; + regions_out.emplace_back(finger, DeviceTactileRegion::TIP); + regions_out.emplace_back(finger, DeviceTactileRegion::FINGER); + return true; case TouchScreenApp::TactileRegion::THUMB_MIDDLE: - return device::TactileRegion::THUMB_MIDDLE; + if (finger != device::AbstractDexHand::FingerType::THUMB) { + return false; + } + regions_out.emplace_back(finger, DeviceTactileRegion::THUMB_MIDDLE); + return true; } - return device::TactileRegion::TIP; + return false; } perception::AprilTagPerception::DepthPolicy toDepthPolicy( @@ -121,20 +173,20 @@ perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod( return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; } -device::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) { +device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) { switch (finger) { case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY: - return device::FingerType::PINKY; + return device::AbstractDexHand::FingerType::PINKY; case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING: - return device::FingerType::RING; + return device::AbstractDexHand::FingerType::RING; case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE: - return device::FingerType::MIDDLE; + return device::AbstractDexHand::FingerType::MIDDLE; case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX: - return device::FingerType::INDEX; + return device::AbstractDexHand::FingerType::INDEX; case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB: - return device::FingerType::THUMB; + return device::AbstractDexHand::FingerType::THUMB; } - return device::FingerType::INDEX; + return device::AbstractDexHand::FingerType::INDEX; } TouchScreenApp::TactileRegion toTactileRegion( @@ -409,6 +461,18 @@ bool TouchScreenApp::setOptionsFromConfig(const cmvr::config::TouchScreenAppConf return last_status_ != Status::INVALID_CONFIG; } +bool TouchScreenApp::setTouchSpeedlForwardL(double forward_l) { + if (!std::isfinite(forward_l) || forward_l < 0.0) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + Options options = options_; + options.touch_speedl_forward_l = forward_l; + setOptions(options); + return last_status_ != Status::INVALID_CONFIG; +} + bool TouchScreenApp::startFromPixel(int u, int v) { if (!initialized_) { last_status_ = Status::NOT_INITIALIZED; @@ -435,6 +499,8 @@ bool TouchScreenApp::startFromPixel(int u, int v) { last_touch_pressure_peak_ = 0.0; last_active_tag_id_ = -1; last_align_error_camera_.setZero(); + touch_start_position_valid_ = false; + touch_start_position_base_.setZero(); phase_ = Phase::ALIGNING; phase_after_retract_ = Phase::DONE; final_status_after_retract_ = Status::DONE; @@ -465,7 +531,11 @@ bool TouchScreenApp::step() { return true; } if (!startTouchPhase()) { - enterFailed(Status::ROBOT_COMMAND_FAILED); + enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE || + last_status_ == Status::INVALID_CONFIG || + last_status_ == Status::ROBOT_STATE_FAILED + ? last_status_ + : Status::ROBOT_COMMAND_FAILED); return false; } return true; @@ -508,6 +578,8 @@ void TouchScreenApp::stop() { last_touch_pressure_sum_ = 0.0; last_touch_pressure_peak_ = 0.0; last_align_error_camera_.setZero(); + touch_start_position_valid_ = false; + touch_start_position_base_.setZero(); last_status_ = Status::STOPPED; } @@ -678,11 +750,30 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& if (config.has_touch_twist_base()) { applyTwist6FromConfig(config.touch_twist_base(), options.touch_twist_base); } - if (config.has_touch_acceleration()) { - options.touch_acceleration = config.touch_acceleration(); + if (config.has_touch_use_speedl()) { + options.touch_use_speedl = config.touch_use_speedl(); } - if (config.has_touch_forward_duration_s()) { - options.touch_forward_duration_s = config.touch_forward_duration_s(); + if (config.has_touch_speedl_acceleration()) { + options.touch_speedl_acceleration = config.touch_speedl_acceleration(); + } + if (config.has_touch_forward_l()) { + options.touch_forward_l = config.touch_forward_l(); + } + if (config.has_touch_movel_speed()) { + options.touch_movel_speed = config.touch_movel_speed(); + } + if (config.has_touch_movel_acceleration()) { + options.touch_movel_acceleration = config.touch_movel_acceleration(); + } + if (config.has_touch_movel_jerk()) { + options.touch_movel_jerk = config.touch_movel_jerk(); + } + if (config.touch_movel_qd_max_size() > 0) { + options.touch_movel_qd_max.assign(config.touch_movel_qd_max().begin(), + config.touch_movel_qd_max().end()); + } + if (config.has_touch_speedl_forward_l()) { + options.touch_speedl_forward_l = config.touch_speedl_forward_l(); } if (config.has_dwell_time_s()) { @@ -697,7 +788,6 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& if (config.has_retract_duration_s()) { options.retract_duration_s = config.retract_duration_s(); } - if (config.has_tactile_finger()) { options.tactile_finger = toFingerType(config.tactile_finger()); } @@ -722,10 +812,49 @@ bool TouchScreenApp::applyOptions() { if (!options_.R_camera_to_visp.allFinite() || !options_.R_camera_to_urdf.allFinite()) { return false; } - if (!std::isfinite(options_.touch_forward_duration_s) || - options_.touch_forward_duration_s < 0.0) { + if (!std::isfinite(options_.touch_speedl_forward_l) || + options_.touch_speedl_forward_l < 0.0) { return false; } + if (!options_.touch_use_speedl) { + Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); + if (!computeLinearMoveDeltaTool(options_.touch_twist_base, + options_.touch_forward_l, + touch_forward_delta)) { + return false; + } + if (!std::isfinite(options_.touch_movel_speed) || options_.touch_movel_speed <= 0.0 || + !std::isfinite(options_.touch_movel_acceleration) || + options_.touch_movel_acceleration <= 0.0 || + !std::isfinite(options_.touch_movel_jerk) || options_.touch_movel_jerk <= 0.0) { + return false; + } + if (!options_.touch_movel_qd_max.empty()) { + if (options_.touch_movel_qd_max.size() != options_.control_joint_names.size()) { + return false; + } + for (const double qd_max_i : options_.touch_movel_qd_max) { + if (!std::isfinite(qd_max_i) || qd_max_i <= 0.0) { + return false; + } + } + } + + } + + if (dexhand_) { + std::vector tactile_regions; + if (!appendRequestedTactileRegions(options_.tactile_finger, + options_.tactile_region, + tactile_regions)) { + return false; + } + try { + dexhand_->setTactilePollingRegions(tactile_regions); + } catch (...) { + return false; + } + } perception_->setTagSize(options_.tag_size_m); tracker_.setTargetPointMethod(options_.target_point_method); @@ -902,7 +1031,11 @@ bool TouchScreenApp::stepAligning() { bool TouchScreenApp::stepTouching() { if (!touch_command_started_) { if (!startTouchPhase()) { - enterFailed(Status::ROBOT_COMMAND_FAILED); + enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE || + last_status_ == Status::INVALID_CONFIG || + last_status_ == Status::ROBOT_STATE_FAILED + ? last_status_ + : Status::ROBOT_COMMAND_FAILED); return false; } } @@ -911,65 +1044,56 @@ bool TouchScreenApp::stepTouching() { enterFailed(Status::TACTILE_UNAVAILABLE); return false; } - const auto touchTriggered = [&]() { - return last_touch_pressure_sum_ >= options_.tactile_pressure_sum_threshold || - (options_.tactile_pressure_peak_threshold > 0.0 && - last_touch_pressure_peak_ >= options_.tactile_pressure_peak_threshold); - }; - const auto handleTouchTriggered = [&]() { - if (options_.dwell_time_s < 0.0) { - try { - if (!robot_->speedL(toStdVector6(options_.retract_twist_base), - options_.retract_acceleration, - 0.0, - cmvr::CartesianFrame::Tool)) { + + if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) { + if (options_.touch_use_speedl) { + logTouchingSpeedLState(); + } + if (!handleTouchTriggered(true)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + } + + if (!options_.touch_use_speedl) { + last_status_ = Status::TOUCHING; + return true; + } + + if (options_.touch_speedl_forward_l > 0.0) { + if (!touch_start_position_valid_) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); + if (!readCurrentTouchPointPositionBase(current_position_base)) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + const double traveled_distance = + (current_position_base - touch_start_position_base_).norm(); + if (traveled_distance >= options_.touch_speedl_forward_l) { + logTouchingSpeedLState(); + if (!updateTouchPressure()) { + enterFailed(Status::TACTILE_UNAVAILABLE); + return false; + } + if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) { + if (!handleTouchTriggered(true)) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } - } catch (...) { + return true; + } + if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } - phase_ = Phase::RETRACTING; - phase_after_retract_ = Phase::DONE; - final_status_after_retract_ = Status::DONE; - phase_start_time_ = Clock::now(); - retract_command_started_ = true; - last_status_ = Status::TOUCH_TRIGGERED; return true; } - - try { - robot_->stopSpeedL(); - } catch (...) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - phase_ = Phase::DWELLING; - phase_start_time_ = Clock::now(); - last_status_ = Status::TOUCH_TRIGGERED; - return true; - }; - - if (touchTriggered()) { - return handleTouchTriggered(); - } - - const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); - if (options_.touch_forward_duration_s > 0.0 && - elapsed >= options_.touch_forward_duration_s) { - if (!updateTouchPressure()) { - enterFailed(Status::TACTILE_UNAVAILABLE); - return false; - } - if (touchTriggered()) { - return handleTouchTriggered(); - } - if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - return true; } last_status_ = Status::TOUCHING; @@ -1070,6 +1194,53 @@ bool TouchScreenApp::sendZeroJointVelocity() const { return sendJointVelocity(zero); } +bool TouchScreenApp::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const { + if (!robot_) { + return false; + } + + try { + const auto pose = robot_->fk(true); + p_out << pose.position().x(), pose.position().y(), pose.position().z(); + return p_out.allFinite(); + } catch (...) { + return false; + } +} + +void TouchScreenApp::logTouchingSpeedLState() const { + if (!robot_ || !options_.touch_use_speedl || phase_ != Phase::TOUCHING) { + return; + } + + const Eigen::Matrix cmd_twist_base = robot_->getSpeedLCommandTwistBase(); + Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); + const bool have_current_position = readCurrentTouchPointPositionBase(current_position_base); + const bool have_delta = touch_start_position_valid_ && have_current_position; + Eigen::Vector3d cumulative_delta_base = Eigen::Vector3d::Zero(); + if (have_delta) { + cumulative_delta_base = current_position_base - touch_start_position_base_; + } + + std::cout << "[TouchScreenApp][TOUCHING] speedl_cmd_base=[" + << cmd_twist_base[0] << ", " + << cmd_twist_base[1] << ", " + << cmd_twist_base[2] << ", " + << cmd_twist_base[3] << ", " + << cmd_twist_base[4] << ", " + << cmd_twist_base[5] << "]"; + if (have_delta) { + std::cout << ", cum_tcp_delta_base=[" + << cumulative_delta_base.x() << ", " + << cumulative_delta_base.y() << ", " + << cumulative_delta_base.z() << "]" + << ", cum_tcp_dist=" << cumulative_delta_base.norm(); + } else { + std::cout << ", cum_tcp_delta_base=[unavailable]"; + } + std::cout << std::endl; +} + bool TouchScreenApp::holdCurrentControlledPosition() const { if (!robot_) { return false; @@ -1118,25 +1289,107 @@ bool TouchScreenApp::moveToInitPositionIfEnabled() const { return true; } -bool TouchScreenApp::startTouchPhase() { - if (!robot_) { - return false; - } - try { - if (!robot_->speedL(toStdVector6(options_.touch_twist_base), - options_.touch_acceleration, - 0.0, - cmvr::CartesianFrame::Tool)) { +bool TouchScreenApp::handleTouchTriggered(const bool stop_forward_motion) { + if (options_.dwell_time_s < 0.0) { + if (!startRetractPhase(Phase::DONE, Status::DONE)) { return false; } - } catch (...) { + if (phase_ == Phase::RETRACTING) { + last_status_ = Status::TOUCH_TRIGGERED; + } + return true; + } + + if (stop_forward_motion) { + try { + robot_->stopSpeedL(); + } catch (...) { + return false; + } + } + + phase_ = Phase::DWELLING; + phase_start_time_ = Clock::now(); + last_status_ = Status::TOUCH_TRIGGERED; + return true; +} + +bool TouchScreenApp::startTouchPhase() { + if (!robot_) { + last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } + phase_ = Phase::TOUCHING; phase_start_time_ = Clock::now(); touch_command_started_ = true; retract_command_started_ = false; last_status_ = Status::TOUCHING; + + if (options_.touch_use_speedl) { + touch_start_position_valid_ = readCurrentTouchPointPositionBase(touch_start_position_base_); + if (options_.touch_speedl_forward_l > 0.0 && !touch_start_position_valid_) { + std::cerr << "[TouchScreenApp] startTouchPhase failed: cannot read touch start pose " + << "for speedL distance-based touching" << std::endl; + last_status_ = Status::ROBOT_STATE_FAILED; + return false; + } + try { + if (!robot_->speedL(toStdVector6(options_.touch_twist_base), + options_.touch_speedl_acceleration, + 0.0, + cmvr::CartesianFrame::Tool)) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + } catch (...) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + return true; + } + + Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); + if (!computeLinearMoveDeltaTool(options_.touch_twist_base, + options_.touch_forward_l, + touch_forward_delta)) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + const std::vector pose_cmd = { + touch_forward_delta.x(), + touch_forward_delta.y(), + touch_forward_delta.z(), + 0.0, + 0.0, + 0.0 + }; + + try { + if (!robot_->moveL(pose_cmd, + options_.touch_movel_speed, + options_.touch_movel_acceleration, + options_.touch_movel_jerk, + options_.touch_movel_qd_max, + false)) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + } catch (...) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + + if (!moveToInitPositionIfEnabled()) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + + phase_ = Phase::DONE; + touch_command_started_ = false; + retract_command_started_ = false; + last_status_ = Status::DONE; return true; } @@ -1145,6 +1398,7 @@ bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract, if (!robot_) { return false; } + try { if (!robot_->speedL(toStdVector6(options_.retract_twist_base), options_.retract_acceleration, @@ -1155,6 +1409,7 @@ bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract, } catch (...) { return false; } + phase_ = Phase::RETRACTING; phase_after_retract_ = next_phase_after_retract; final_status_after_retract_ = final_status_after_retract; @@ -1180,70 +1435,31 @@ void TouchScreenApp::enterFailed(const Status status) { } bool TouchScreenApp::updateTouchPressure() { + last_touch_pressure_sum_ = 0.0; + last_touch_pressure_peak_ = 0.0; if (!dexhand_) { - last_touch_pressure_sum_ = 0.0; - last_touch_pressure_peak_ = 0.0; return false; } - const auto& sensors = dexhand_->getSensorData(options_.tactile_finger, - toDeviceTactileRegion(options_.tactile_region)); + std::vector tactile_regions; + if (!appendRequestedTactileRegions(options_.tactile_finger, + options_.tactile_region, + tactile_regions)) { + return false; + } + double sum = 0.0; double peak = 0.0; - - auto accumulate_finger = [&](const auto& finger_sensor) { - switch (options_.tactile_region) { - case TactileRegion::TIP: - accumulateMatrixStats(finger_sensor.tip.data, sum, peak); - break; - case TactileRegion::FINGER: - accumulateMatrixStats(finger_sensor.finger.data, sum, peak); - break; - case TactileRegion::PAD: - accumulateMatrixStats(finger_sensor.pad.data, sum, peak); - break; - case TactileRegion::TIP_AND_FINGER: - accumulateMatrixStats(finger_sensor.tip.data, sum, peak); - accumulateMatrixStats(finger_sensor.finger.data, sum, peak); - break; - case TactileRegion::THUMB_MIDDLE: - break; - } - }; - - switch (options_.tactile_finger) { - case device::FingerType::PINKY: - accumulate_finger(sensors.pinky); - break; - case device::FingerType::RING: - accumulate_finger(sensors.ring); - break; - case device::FingerType::MIDDLE: - accumulate_finger(sensors.middle); - break; - case device::FingerType::INDEX: - accumulate_finger(sensors.index); - break; - case device::FingerType::THUMB: - switch (options_.tactile_region) { - case TactileRegion::TIP: - accumulateMatrixStats(sensors.thumb.tip.data, sum, peak); - break; - case TactileRegion::FINGER: - accumulateMatrixStats(sensors.thumb.finger.data, sum, peak); - break; - case TactileRegion::PAD: - accumulateMatrixStats(sensors.thumb.pad.data, sum, peak); - break; - case TactileRegion::TIP_AND_FINGER: - accumulateMatrixStats(sensors.thumb.tip.data, sum, peak); - accumulateMatrixStats(sensors.thumb.finger.data, sum, peak); - break; - case TactileRegion::THUMB_MIDDLE: - accumulateMatrixStats(sensors.thumb.middle.data, sum, peak); - break; + try { + for (const auto& tactile_region : tactile_regions) { + const auto sensor = dexhand_->getSensorData(tactile_region.first, tactile_region.second); + if (!sensor.valid()) { + return false; } - break; + accumulateMatrixStats(sensor.view, sum, peak); + } + } catch (...) { + return false; } last_touch_pressure_sum_ = sum; diff --git a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt index f7e040f7..695bf873 100644 --- a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt +++ b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt @@ -5,31 +5,31 @@ move_to_init_position_before_start: true move_to_init_position: true init_joint_positions { joint_name: "R_SHOULDER_P" - rad: -0.2423 + rad: -0.3678 } init_joint_positions { joint_name: "R_SHOULDER_R" - rad: 1.2929 + rad: 1.1127 } init_joint_positions { joint_name: "R_SHOULDER_Y" - rad: 1.61 + rad: 1.6084 } init_joint_positions { joint_name: "R_ELBOW_R" - rad: 1.58 + rad: 1.69508 } init_joint_positions { joint_name: "R_WRIST_P" - rad: -2.8792 + rad: -2.5718 } init_joint_positions { joint_name: "R_WRIST_Y" - rad: 0.1150 + rad: 0.1276 } init_joint_positions { joint_name: "R_WRIST_R" - rad: -0.08 + rad: 0.1297 } init_movej_vel: 1.0 init_movej_acc: 2.0 @@ -39,20 +39,20 @@ base_link: "PELVIS_S" flange_link: "R_WRIST_R_S" camera_link: "R_CAM" -tag_size_m: 0.02 +tag_size_m: 0.012 depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE hover_target_in_camera { - x: 0.0 - y: 0.0 - z: 0.20 + x: 0.005 + y: 0.075 + z: 0.15 } target_rx: 3.14159265358979323846 target_ry: 0.0 target_rz: 0.0 -ibvs_lambda: 0.6 +ibvs_lambda: 0.3 ibvs_mu: 0.1 ibvs_qdot_max: 0.15 ibvs_vmax6 { @@ -88,10 +88,10 @@ control_joint_names: "R_WRIST_Y" control_joint_names: "R_WRIST_R" align_xy_threshold_m: 0.003 -align_z_threshold_m: 0.010 -align_rot_threshold_rad: 0.08726646259971647 -align_stable_frames: 5 -align_timeout_s: 100.0 +align_z_threshold_m: 0.003 +align_rot_threshold_rad: 0.1026646259971647 +align_stable_frames: 3 +align_timeout_s: 30.0 pause_after_align_reached: false touch_twist_base { @@ -102,10 +102,22 @@ touch_twist_base { wy: 0.0 wz: 0.0 } -touch_acceleration: 3.0 -touch_forward_duration_s: 1.2 +touch_use_speedl: true +touch_speedl_acceleration: 3.0 +touch_forward_l: 0.064 +touch_movel_speed: 0.1 +touch_movel_acceleration: 5.0 +touch_movel_jerk: 5.0 +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_movel_qd_max: 2.5 +touch_movel_qd_max: 2.5 +touch_movel_qd_max: 2.5 +touch_speedl_forward_l: 0.0255 -dwell_time_s: -1.0 +dwell_time_s: -1 retract_twist_base { vx: 0.0 vy: 0.08 @@ -115,9 +127,9 @@ retract_twist_base { wz: 0.0 } retract_acceleration: 3.0 -retract_duration_s: 1.0 +retract_duration_s: 0.45 tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP -tactile_pressure_sum_threshold: 300.0 +tactile_pressure_sum_threshold: 300000.0 tactile_pressure_peak_threshold: 0.0 diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 2eaf4c98..9e44092d 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -1,19 +1,69 @@ #include "common/utils/config_helper/include/config_setting.h" + +#include +#include +#include #include +namespace { -static std::string basePath() -{ - char* cwd = ::getcwd(nullptr, 0); - if (!cwd) return {}; - std::string s(cwd); - std::free(cwd); - return s + "/config/"; +std::string normalizeDirectory(std::filesystem::path path) { + auto text = path.lexically_normal().string(); + if (!text.empty() && text.back() != '/') { + text.push_back('/'); + } + return text; +} + +std::string probeConfigDirectory(std::filesystem::path start) { + static const std::array candidates = { + std::filesystem::path("output/bin/config"), + std::filesystem::path("cmvr-es/common/config"), + std::filesystem::path("config"), + std::filesystem::path("common/config") + }; + + start = start.lexically_normal(); + while (!start.empty()) { + for (const auto& candidate : candidates) { + const auto path = start / candidate; + if (std::filesystem::exists(path) && std::filesystem::is_directory(path)) { + return normalizeDirectory(path); + } + } + + const auto parent = start.parent_path(); + if (parent == start) { + break; + } + start = parent; + } + return {}; +} + +std::filesystem::path executableDirectory() { + char exe_path[PATH_MAX] = {0}; + const auto count = ::readlink("/proc/self/exe", exe_path, sizeof(exe_path) - 1); + if (count <= 0) { + return {}; + } + exe_path[count] = '\0'; + return std::filesystem::path(exe_path).parent_path(); +} + +} // namespace + +static std::string basePath() { + if (const auto resolved = probeConfigDirectory(executableDirectory()); !resolved.empty()) { + return resolved; + } + + if (const auto resolved = probeConfigDirectory(std::filesystem::current_path()); !resolved.empty()) { + return resolved; + } + + return "config/"; } -// static std::string basePath() -// { -// return "/home/lgv/cmvr/cmvr-es/output/bin/config/"; -// } DEFINE_string(pinocchio_qp_ik_solver_config_file, basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt", diff --git a/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt b/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt index 61740c7a..c21364f2 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt +++ b/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt @@ -3,7 +3,9 @@ add_library(head_esp32 SHARED src/biohead_esp32.cpp) # 设置头文件包含路径 target_include_directories(head_esp32 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - +target_link_libraries(head_esp32 PRIVATE + cmvr_es::hardware +) # 创建命名空间化的 ALIAS（建议使用项目命名空间） add_library(cmvr::device::head_esp32 ALIAS head_esp32) diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index ae8b5eaa..1d88c87f 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -249,7 +249,7 @@ void RealsenseCamera::start() { return; } - constexpr int kStartMaxRetry = 3; + constexpr int kStartMaxRetry = 5; constexpr int kRetrySleepMs = 300; constexpr unsigned int kProbeTimeoutMs = 2000; std::string last_error; diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 9f5d502c..0ead75ff 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -6,236 +6,144 @@ #define CMVR_ES_ABSTRACT_DEXHAND_H #pragma once +#include +#include +#include +#include +#include +#include + #include "devices/abstract_device.h" -#include "cmvr/config/dexhand_config/dexhand_config.pb.h" -namespace cmvr::device{ - // 单个触觉点数据（16位无符号整数） - using TactilePoint = uint16_t; - // 手指触觉数据结构体（支持从数组任意位置解析） - struct FingerTactileData { - std::vector> data; // 触觉数据二维数组 - int rows; // 行数 - int cols; // 列数 - int byteSize; // 总字节数（rows * cols * 2字节/点） - std::string name; // 部位名称 - - // 初始化数据数组 - void init() { - data.resize(rows, std::vector(cols, 0)); - } - - // 从 std::vector 中赋值数据 - void assignFromVector(const std::vector& values, int startIndex = 0) { - int index = startIndex; - for (int i = 0; i < rows; ++i) { - for (int j = 0; j < cols; ++j) { - if (index < values.size()) { - data[i][j] = values[index]; - index++; - } - } - } - } - }; - - // 手掌触觉数据结构体（支持从数组任意位置解析） - struct PalmTactileData { - std::vector> data; // 触觉数据二维数组 - int rows = 8; // 行数（掌心固定为8行） - int cols = 14; // 列数（掌心固定为14列） - int byteSize = 224; // 总字节数（8*14*2=224） - std::string name = "掌心"; // 部位名称 - - // 初始化数据数组 - void init() { - data.resize(rows, std::vector(cols, 0)); - } - - // 从 std::vector 中赋值数据 - void assignFromVector(const std::vector& values, int startIndex = 0) { - int index = startIndex; - for (int j = 0; j < cols; ++j) { - for (int i = rows - 1; i >= 0; --i) { - if (index < values.size()) { - data[i][j] = values[index]; - index++; - } - } - } - } - }; - - // 手指枚举（方便按手指类型解析） - enum FingerType { - PINKY, // 小拇指 - RING, // 无名指 - MIDDLE, // 中指 - INDEX, // 食指 - THUMB // 大拇指 - }; - - // 触觉区域枚举（设备层）。 - enum class TactileRegion { - TIP = 0, - FINGER, - PAD, - TIP_AND_FINGER, - THUMB_MIDDLE - }; - - // 整只手的触觉传感器数据 - struct HandTactileSensors { - // 五指的触觉数据（每根手指包含指端、指尖、指腹） - struct { - FingerTactileData tip; // 指端 - FingerTactileData finger; // 指尖 - FingerTactileData pad; // 指腹 - } pinky, ring, middle, index; // 小拇指、无名指、中指、食指 - - // 大拇指（特殊：多一个指中） - struct { - FingerTactileData tip; // 指端 - FingerTactileData finger; // 指尖 - FingerTactileData middle; // 指中 - FingerTactileData pad; // 指腹 - } thumb; - - // 掌心触觉数据 - PalmTactileData palm; - - // 初始化所有传感器数据（设置相对偏移量） - void init() { - // 小拇指（总370byte） - pinky.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "小拇指指端"}; - pinky.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "小拇指指尖"}; - pinky.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "小拇指指腹"}; - - // 无名指（总370byte） - ring.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "无名指指端"}; - ring.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "无名指指尖"}; - ring.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "无名指指腹"}; - - // 中指（总370byte） - middle.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "中指指端"}; - middle.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "中指指尖"}; - middle.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "中指指腹"}; - - // 食指（总370byte） - index.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "食指指端"}; - index.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "食指指尖"}; - index.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "食指指腹"}; - - // 大拇指（总420byte） - thumb.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指端"}; - thumb.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指尖"}; - thumb.middle = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指中"}; - thumb.pad = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指指腹"}; - - // 掌心（总224byte） - palm = { .name = "掌心"}; - - // 初始化所有数据数组 - pinky.tip.init(); - pinky.finger.init(); - pinky.pad.init(); - - ring.tip.init(); - ring.finger.init(); - ring.pad.init(); - - middle.tip.init(); - middle.finger.init(); - middle.pad.init(); - - index.tip.init(); - index.finger.init(); - index.pad.init(); - - thumb.tip.init(); - thumb.finger.init(); - thumb.middle.init(); - thumb.pad.init(); - - palm.init(); - } - // 按手指类型解析整个手指的数据 - void parseFinger(FingerType fingerType, const std::vector& values) { - switch (fingerType) { - case PINKY: - pinky.tip.assignFromVector(values); - pinky.finger.assignFromVector(values, 9); - pinky.pad.assignFromVector(values, 9 + 96); - break; - - case RING: - ring.tip.assignFromVector(values); - ring.finger.assignFromVector(values, 9); - ring.pad.assignFromVector(values, 9 + 96); - break; - - case MIDDLE: - middle.tip.assignFromVector(values); - middle.finger.assignFromVector(values, 9); - middle.pad.assignFromVector(values, 9 + 96); - break; - - case INDEX: - index.tip.assignFromVector(values); - index.finger.assignFromVector(values, 9); - index.pad.assignFromVector(values, 9 + 96); - break; - - case THUMB: - thumb.tip.assignFromVector(values); - thumb.finger.assignFromVector(values, 9); - thumb.middle.assignFromVector(values, 9 + 96); - thumb.pad.assignFromVector(values, 9 + 96 + 9); - break; - } - } - - // 解析手心（掌心）数据 - void parsePalm(const std::vector& values) { - palm.assignFromVector(values); - } - - // 获取手指名称（用于调试） - std::string getFingerName(FingerType fingerType) { - switch (fingerType) { - case PINKY: return "小拇指"; - case RING: return "无名指"; - case MIDDLE: return "中指"; - case INDEX: return "食指"; - case THUMB: return "大拇指"; - default: return "未知"; - } - } - }; - class AbstractDexHand: public AbstractDevice{ +namespace cmvr::device { + class AbstractDexHand : public AbstractDevice { public: + using TactilePoint = uint16_t; + + enum class FingerType { + PINKY, + RING, + MIDDLE, + INDEX, + THUMB, + PALM + }; + + enum class TactileRegion { + TIP, + FINGER, + PAD, + THUMB_MIDDLE, + PALM_PAD + }; + + using TactileRegionKey = std::pair; + + struct TactileMatrixView { + const TactilePoint* data{nullptr}; + int rows{0}; + int cols{0}; + + bool valid() const { + return data != nullptr && rows > 0 && cols > 0; + } + + int pointCount() const { + return rows * cols; + } + + const TactilePoint* rowData(int row) const { + return data + row * cols; + } + + TactilePoint at(int row, int col) const { + return data[row * cols + col]; + } + }; + + class TactileRegionData { + public: + TactileRegionData() = default; + + TactileRegionData(FingerType finger_in, + TactileRegion region_in, + TactileMatrixView view_in, + const char* name_in, + std::shared_ptr lifetime = {}) + : finger(finger_in), + region(region_in), + view(view_in), + name(name_in), + lifetime_(std::move(lifetime)) {} + + bool valid() const { + return view.valid(); + } + + int byteSize() const { + return view.pointCount() * static_cast(sizeof(TactilePoint)); + } + + FingerType finger{FingerType::PINKY}; + TactileRegion region{TactileRegion::TIP}; + TactileMatrixView view{}; + const char* name{nullptr}; + + private: + std::shared_ptr lifetime_; + }; + + enum class Status { + CREATED, + INITIALIZED, + STREAMING, + STOPPED, + FAULT + }; + AbstractDexHand() = default; - explicit AbstractDexHand(const XmlNode& cfg): AbstractDevice(cfg) {} + explicit AbstractDexHand(const XmlNode& cfg) : AbstractDevice(cfg) {} ~AbstractDexHand() override = default; - virtual void getState(DexHandState &state) {} + virtual Status state() const = 0; + virtual std::string lastError() const = 0; - // control - virtual void setPositions(const std::vector& finger_joint_targets) {} - virtual void setAngles(const std::vector& finger_joint_angles) {} - virtual void setVelocities(const std::vector& finger_joint_velocities) {} - virtual void setPresetAct(int action_id) {} - virtual void execPresetAct(int action_id) {} - virtual void setForce(const std::vector& finger_joint_force) {} - virtual HandTactileSensors& getSensorData() { return hand_tactile_sensors_;} - virtual HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) { - (void)finger_type; - (void)tactile_region; - return getSensorData(); + virtual void getState(DexHandState& state) { + state = DexHandState{}; + const auto lifecycle = this->state(); + state.is_initialized = + lifecycle == Status::INITIALIZED || + lifecycle == Status::STREAMING; + } + + virtual void setAngles(const std::vector& finger_joint_angles) = 0; + virtual void setTactilePollingRegion(FingerType finger, TactileRegion region) { + setTactilePollingRegions({TactileRegionKey{finger, region}}); + } + + virtual void setTactilePollingRegions(const std::vector& regions) = 0; + virtual std::vector getSensorData() = 0; + virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0; + + virtual void setPositions(const std::vector&) { + throw std::logic_error("setPositions is not supported by this dexhand abstraction."); + } + + virtual void setVelocities(const std::vector&) { + throw std::logic_error("setVelocities is not supported by this dexhand abstraction."); + } + + virtual void setPresetAct(int) { + throw std::logic_error("setPresetAct is not supported by this dexhand abstraction."); + } + + virtual void execPresetAct(int) { + throw std::logic_error("execPresetAct is not supported by this dexhand abstraction."); + } + + virtual void setForce(const std::vector&) { + throw std::logic_error("setForce is not supported by this dexhand abstraction."); } - protected: - DexHandState status_; - HandTactileSensors hand_tactile_sensors_; }; } diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt index adc2ce00..1a235638 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt @@ -6,4 +6,18 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand) target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware -lmodbus) -install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib) + +add_executable(rh56dftp_dexhand_test + src/rh56dftp_dexhand_test.cpp +) + +target_link_libraries(rh56dftp_dexhand_test PRIVATE + cmvr_es::device::rh56dftp_dexhand + cmvr_es::common + cmvr_es::proto + glog + gtest + gtest_main + pthread +) diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h index 9455d103..b0fc27d1 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -5,78 +5,124 @@ #ifndef RH56DFTP_DEXHAND_H #define RH56DFTP_DEXHAND_H -#include -#include -#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include + #include -#include -#include "../../../../utils/base/include/os.h" + +#include "cmvr/config/dexhand_config/dexhand_config.pb.h" #include "../../abstract_dexhand.h" -#include "../../../../hardware/include/serial_interface.h" +#include "rh56dftp_tactile_buffer.h" namespace cmvr::device { class ModbusController { - private: - modbus_t *ctx; - std::map regdict; - static constexpr int MAX_REGISTERS_PER_READ = 64; // 根据Modbus协议限制设置 - public: - ModbusController(); + ModbusController() = default; ~ModbusController(); - bool open(const std::string& ip, int port); + void open(const std::string& ip, int port); void close(); + bool isOpen() const; - bool writeRegisters(int address, const std::vector& values); - std::vector readRegisters(int address, int count); - std::vector readRegisterRange(int start_addr, int end_addr); + void writeRegisters(int address, const uint16_t* values, int count); + void readRegisterBlock(int start_addr, int count, std::vector& values); - bool write6(const std::string& reg_name, const std::vector& val); - bool read6(const std::string& reg_name, std::vector& result); + private: + void closeUnlocked(); - static void sleep(float seconds); + modbus_t* ctx_{nullptr}; + mutable std::mutex io_mutex_; }; - class RH56DFTPDexhand final : public AbstractDexHand{ + class RH56DFTPDexhand final : public AbstractDexHand { public: + using FingerType = AbstractDexHand::FingerType; + using TactileRegion = AbstractDexHand::TactileRegion; + using TactileRegionKey = AbstractDexHand::TactileRegionKey; + using TactileRegionData = AbstractDexHand::TactileRegionData; + using Status = AbstractDexHand::Status; + + static constexpr size_t ANGLE_COMMAND_COUNT = 6; + static constexpr size_t TACTILE_REGION_SLOT_COUNT = RH56TactileBuffer::REGION_COUNT; + using RegionMask = std::bitset; + explicit RH56DFTPDexhand(const XmlNode& cfg); - RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg); + explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg); ~RH56DFTPDexhand() 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 getState(DexHandState &state) override; - - // control - void setPositions(const std::vector& finger_joint_targets) override; void setAngles(const std::vector& finger_joint_angles) override; - void setVelocities(const std::vector& finger_joint_velocities) override; - void setPresetAct(int action_id) override;//这个接口暂时不可用，未开放寄存器设置预设 - void execPresetAct(int action_id) override;//这个无用 - void setForce(const std::vector& finger_joint_force) override; - HandTactileSensors& getSensorData() override; - HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) override; - private: - void updateState(); - void updateSensorData(); + void setTactilePollingRegions(const std::vector& regions) override; + std::vector getSensorData() override; + TactileRegionData getSensorData(FingerType finger, TactileRegion region) override; - int default_force_;//上电的力控阈值 - int default_speed_;//上电的自由角转动速度 + private: + struct TactileBufferLease { + std::shared_lock lock; + const RH56TactileBuffer* buffer{nullptr}; + }; + + void parseXmlConfig(); + void initializeTactileBuffers(); + void ensureConnected(); + void refreshTactileData(const RegionMask& mask); + void tactilePollingLoop(); + + std::shared_ptr acquireActiveBufferLease() const; + std::vector acquireAllRegionData() const; + TactileRegionData acquireRegionData(FingerType finger, TactileRegion region) const; + + bool isRequestedMaskCovered(const RegionMask& mask) const; + bool isActiveBufferReady(const RegionMask& mask) const; + void ensureTactileMaskReady(const RegionMask& mask, bool allow_background); + + void transitionTo(Status next_state); + void enterFault(const std::string& error); + bool isOperationalState(Status lifecycle) const; std::string ip_address_; - int port_; - std::shared_ptr controller_; + int port_{6000}; + std::unique_ptr controller_; config::RH56DFTPDexHandConfig dexhandCfg_; + + mutable std::mutex lifecycle_mutex_; + Status lifecycle_state_{Status::CREATED}; + std::string last_error_; + + mutable std::mutex command_mutex_; + std::array last_commanded_angles_{}; + + std::array tactile_buffers_; + std::array tactile_buffer_masks_{}; + mutable std::shared_mutex tactile_buffer_mutex_; + mutable std::mutex tactile_refresh_mutex_; + + mutable std::mutex polling_mutex_; + std::condition_variable polling_cv_; + RegionMask requested_polling_mask_{}; + std::thread tactile_thread_; + std::atomic tactile_thread_running_{false}; + std::atomic active_buffer_index_{0}; + std::chrono::milliseconds tactile_poll_interval_{20}; }; } - - #endif //RH56DFTP_DEXHAND_H diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h new file mode 100644 index 00000000..bc070b06 --- /dev/null +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h @@ -0,0 +1,136 @@ +#ifndef RH56DFTP_TACTILE_BUFFER_H +#define RH56DFTP_TACTILE_BUFFER_H +#pragma once + +#include +#include +#include + +#include "../../abstract_dexhand.h" + +namespace cmvr::device { + + enum class RH56TactileFillOrder { + ROW_MAJOR, + COLUMN_MAJOR_BOTTOM_TO_TOP + }; + + struct RH56TactileRegionLayout { + AbstractDexHand::FingerType finger{AbstractDexHand::FingerType::PINKY}; + AbstractDexHand::TactileRegion region{AbstractDexHand::TactileRegion::TIP}; + const char* name{nullptr}; + int rows{0}; + int cols{0}; + size_t offset{0}; + size_t point_count{0}; + RH56TactileFillOrder fill_order{RH56TactileFillOrder::ROW_MAJOR}; + + AbstractDexHand::TactileMatrixView view(const AbstractDexHand::TactilePoint* buffer) const { + return AbstractDexHand::TactileMatrixView{buffer + offset, rows, cols}; + } + }; + + class RH56TactileBuffer { + public: + static constexpr size_t REGION_COUNT = 17; + static constexpr size_t POINT_COUNT = 1062; + + void clear() { + values_.fill(0); + } + + void assignRegionData(AbstractDexHand::FingerType finger, + AbstractDexHand::TactileRegion region, + const AbstractDexHand::TactilePoint* 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(4096) ? 0 : value; + }; + int input_index = 0; + + if (layout.fill_order == RH56TactileFillOrder::ROW_MAJOR) { + for (size_t point = 0; point < layout.point_count && input_index < valueCount; ++point, ++input_index) { + destination[point] = sanitize_point(input[input_index]); + } + return; + } + + for (int col = 0; col < layout.cols; ++col) { + for (int row = layout.rows - 1; row >= 0 && input_index < valueCount; --row, ++input_index) { + destination[static_cast(row * layout.cols + col)] = sanitize_point(input[input_index]); + } + } + } + + const RH56TactileRegionLayout& regionLayout(AbstractDexHand::FingerType finger, + AbstractDexHand::TactileRegion region) const { + return layouts()[findRegionIndex(finger, region)]; + } + + AbstractDexHand::TactileMatrixView regionView(AbstractDexHand::FingerType finger, + AbstractDexHand::TactileRegion region) const { + const auto& layout = regionLayout(finger, region); + return layout.view(values_.data()); + } + + const std::array& layouts() const { + return staticLayouts(); + } + + const AbstractDexHand::TactilePoint* data() const { + return values_.data(); + } + + private: + static const std::array& staticLayouts() { + static const std::array layouts = {{ + {AbstractDexHand::FingerType::PINKY, AbstractDexHand::TactileRegion::TIP, "小拇指指端", 3, 3, 0, 9, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::PINKY, AbstractDexHand::TactileRegion::FINGER, "小拇指指尖", 12, 8, 9, 96, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::PINKY, AbstractDexHand::TactileRegion::PAD, "小拇指指腹", 10, 8, 105, 80, RH56TactileFillOrder::ROW_MAJOR}, + + {AbstractDexHand::FingerType::RING, AbstractDexHand::TactileRegion::TIP, "无名指指端", 3, 3, 185, 9, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::RING, AbstractDexHand::TactileRegion::FINGER, "无名指指尖", 12, 8, 194, 96, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::RING, AbstractDexHand::TactileRegion::PAD, "无名指指腹", 10, 8, 290, 80, RH56TactileFillOrder::ROW_MAJOR}, + + {AbstractDexHand::FingerType::MIDDLE, AbstractDexHand::TactileRegion::TIP, "中指指端", 3, 3, 370, 9, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::MIDDLE, AbstractDexHand::TactileRegion::FINGER, "中指指尖", 12, 8, 379, 96, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::MIDDLE, AbstractDexHand::TactileRegion::PAD, "中指指腹", 10, 8, 475, 80, RH56TactileFillOrder::ROW_MAJOR}, + + {AbstractDexHand::FingerType::INDEX, AbstractDexHand::TactileRegion::TIP, "食指指端", 3, 3, 555, 9, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::INDEX, AbstractDexHand::TactileRegion::FINGER, "食指指尖", 12, 8, 564, 96, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::INDEX, AbstractDexHand::TactileRegion::PAD, "食指指腹", 10, 8, 660, 80, RH56TactileFillOrder::ROW_MAJOR}, + + {AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::TIP, "大拇指指端", 3, 3, 740, 9, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::FINGER, "大拇指尖", 12, 8, 749, 96, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::THUMB_MIDDLE, "大拇指指中", 3, 3, 845, 9, RH56TactileFillOrder::ROW_MAJOR}, + {AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::PAD, "大拇指指腹", 12, 8, 854, 96, RH56TactileFillOrder::ROW_MAJOR}, + + {AbstractDexHand::FingerType::PALM, + AbstractDexHand::TactileRegion::PALM_PAD, + "掌心", + 8, + 14, + 950, + 112, + RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP} + }}; + return layouts; + } + + static size_t findRegionIndex(AbstractDexHand::FingerType finger, AbstractDexHand::TactileRegion region) { + const auto& region_layouts = staticLayouts(); + for (size_t index = 0; index < region_layouts.size(); ++index) { + if (region_layouts[index].finger == finger && region_layouts[index].region == region) { + return index; + } + } + throw std::invalid_argument("Invalid tactile region for selected finger."); + } + + std::array values_{}; + }; +} + +#endif //RH56DFTP_TACTILE_BUFFER_H diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp index da2dfb83..8ce5f0ee 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -3,468 +3,657 @@ // #include "../include/rh56dftp_dexhand.h" -#include -#include -#include -#include + #include -#include -#include -using namespace std; +#include +#include +#include + using namespace cmvr::device; -// 定义各部分数据地址范围 -const int TOUCH_SENSOR_BASE_ADDR_PINKY = 3000; // 小拇指 -const int TOUCH_SENSOR_END_ADDR_PINKY = 3369; - -const int TOUCH_SENSOR_BASE_ADDR_RING = 3370; // 无名指 -const int TOUCH_SENSOR_END_ADDR_RING = 3739; - -const int TOUCH_SENSOR_BASE_ADDR_MIDDLE = 3740; // 中指 -const int TOUCH_SENSOR_END_ADDR_MIDDLE = 4109; - -const int TOUCH_SENSOR_BASE_ADDR_INDEX = 4110; // 食指 -const int TOUCH_SENSOR_END_ADDR_INDEX = 4479; - -const int TOUCH_SENSOR_BASE_ADDR_THUMB = 4480; // 大拇指 -const int TOUCH_SENSOR_END_ADDR_THUMB = 4899; - -const int TOUCH_SENSOR_BASE_ADDR_PALM = 4900; // 掌心 -const int TOUCH_SENSOR_END_ADDR_PALM = 5123; - -// Modbus 每次最多读取寄存器的数量 -const int MAX_REGISTERS_PER_READ = 125; - namespace { + using DexHand = AbstractDexHand; + using FingerType = DexHand::FingerType; + using TactileRegion = DexHand::TactileRegion; + using TactileRegionKey = DexHand::TactileRegionKey; + using TactileRegionData = DexHand::TactileRegionData; + using Status = DexHand::Status; -std::pair fingerSensorRegisterRange(const FingerType finger_type) { - switch (finger_type) { - case PINKY: return {TOUCH_SENSOR_BASE_ADDR_PINKY, TOUCH_SENSOR_END_ADDR_PINKY}; - case RING: return {TOUCH_SENSOR_BASE_ADDR_RING, TOUCH_SENSOR_END_ADDR_RING}; - case MIDDLE: return {TOUCH_SENSOR_BASE_ADDR_MIDDLE, TOUCH_SENSOR_END_ADDR_MIDDLE}; - case INDEX: return {TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX}; - case THUMB: return {TOUCH_SENSOR_BASE_ADDR_THUMB, TOUCH_SENSOR_END_ADDR_THUMB}; + constexpr int kAngleSetByteAddress = 1486; + constexpr int kDefaultPort = 6000; + constexpr int kMaxRegistersPerRead = 125; + + // The RH56 protocol documents addresses and lengths in bytes, while libmodbus + // expects holding-register indexes (16-bit words). + constexpr int kRegisterByteWidth = 2; + constexpr int kFingerTipByteLength = 18; // 3 x 3 + constexpr int kFingerSurfaceByteLength = 192; // 12 x 8 + constexpr int kFingerPadByteLength = 160; // 10 x 8 + constexpr int kThumbMiddleByteLength = 18; // 3 x 3 + constexpr int kThumbPadByteLength = 192; // 12 x 8 + constexpr int kPalmPadByteLength = 224; // 8 x 14 + + constexpr int toRegisterCount(const int byte_length) { + return byte_length / kRegisterByteWidth; } - return {TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX}; -} -} // namespace + constexpr int toRegisterAddress(const int byte_address) { + return byte_address / kRegisterByteWidth; + } -ModbusController::ModbusController() { - // 初始化寄存器字典 - regdict = { - {"defaultSpeedSet",1032}, - {"defaultForceSet",1044}, - {"posSet",1474}, - {"angleSet", 1486}, - {"forceSet", 1498}, - {"speedSet", 1522}, - {"angleAct", 1546}, - {"posAct",1534}, - {"forceAct", 1582}, - {"current",1594}, - {"error",1606}, - {"temperature",1618}, - {"finger_one_touch",3000}, - {"finger_two_touch",3370}, - {"finger_the_touch",3740}, - {"finger_or_touch",4110}, - {"finger_fiv_touch",4480}, - {"finger_palm_touch",4900} + constexpr int nextByteAddress(const int start_byte_addr, const int byte_length) { + return start_byte_addr + byte_length; + } + + constexpr int kTouchSensorBaseByteAddrPinky = 3000; + constexpr int kTouchSensorBaseByteAddrRing = 3370; + constexpr int kTouchSensorBaseByteAddrMiddle = 3740; + constexpr int kTouchSensorBaseByteAddrIndex = 4110; + constexpr int kTouchSensorBaseByteAddrThumb = 4480; + constexpr int kTouchSensorBaseByteAddrPalm = 4900; + + constexpr int kTouchSensorRegCountPalm = toRegisterCount(kPalmPadByteLength); + + struct TactileRegionConfig { + FingerType finger; + TactileRegion region; + int start_register; + int register_count; }; - ctx = nullptr; + struct TactileReadSegment { + int start_register; + int register_count; + std::vector region_indexes; + }; + + using RegionMask = RH56DFTPDexhand::RegionMask; + + const std::array kTactileRegionConfigs = {{ + {FingerType::PINKY, + TactileRegion::TIP, + toRegisterAddress(kTouchSensorBaseByteAddrPinky), + toRegisterCount(kFingerTipByteLength)}, + {FingerType::PINKY, + TactileRegion::FINGER, + toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength)), + toRegisterCount(kFingerSurfaceByteLength)}, + {FingerType::PINKY, + TactileRegion::PAD, + toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength), + kFingerSurfaceByteLength)), + toRegisterCount(kFingerPadByteLength)}, + + {FingerType::RING, + TactileRegion::TIP, + toRegisterAddress(kTouchSensorBaseByteAddrRing), + toRegisterCount(kFingerTipByteLength)}, + {FingerType::RING, + TactileRegion::FINGER, + toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength)), + toRegisterCount(kFingerSurfaceByteLength)}, + {FingerType::RING, + TactileRegion::PAD, + toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength), + kFingerSurfaceByteLength)), + toRegisterCount(kFingerPadByteLength)}, + + {FingerType::MIDDLE, + TactileRegion::TIP, + toRegisterAddress(kTouchSensorBaseByteAddrMiddle), + toRegisterCount(kFingerTipByteLength)}, + {FingerType::MIDDLE, + TactileRegion::FINGER, + toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength)), + toRegisterCount(kFingerSurfaceByteLength)}, + {FingerType::MIDDLE, + TactileRegion::PAD, + toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength), + kFingerSurfaceByteLength)), + toRegisterCount(kFingerPadByteLength)}, + + {FingerType::INDEX, + TactileRegion::TIP, + toRegisterAddress(kTouchSensorBaseByteAddrIndex), + toRegisterCount(kFingerTipByteLength)}, + {FingerType::INDEX, + TactileRegion::FINGER, + toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength)), + toRegisterCount(kFingerSurfaceByteLength)}, + {FingerType::INDEX, + TactileRegion::PAD, + toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength), + kFingerSurfaceByteLength)), + toRegisterCount(kFingerPadByteLength)}, + + {FingerType::THUMB, + TactileRegion::TIP, + toRegisterAddress(kTouchSensorBaseByteAddrThumb), + toRegisterCount(kFingerTipByteLength)}, + {FingerType::THUMB, + TactileRegion::FINGER, + toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength)), + toRegisterCount(kFingerSurfaceByteLength)}, + {FingerType::THUMB, + TactileRegion::THUMB_MIDDLE, + toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength), + kFingerSurfaceByteLength)), + toRegisterCount(kThumbMiddleByteLength)}, + {FingerType::THUMB, + TactileRegion::PAD, + toRegisterAddress(nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, + kFingerTipByteLength), + kFingerSurfaceByteLength), + kThumbMiddleByteLength)), + toRegisterCount(kThumbPadByteLength)}, + + {FingerType::PALM, + TactileRegion::PALM_PAD, + toRegisterAddress(kTouchSensorBaseByteAddrPalm), + kTouchSensorRegCountPalm} + }}; + + RegionMask buildFullRegionMask() { + RegionMask mask; + mask.set(); + return mask; + } + + int findRegionConfigIndex(const FingerType finger, const TactileRegion region) { + for (size_t i = 0; i < kTactileRegionConfigs.size(); ++i) { + const auto& config = kTactileRegionConfigs[i]; + if (config.finger == finger && config.region == region) { + return static_cast(i); + } + } + return -1; + } + + RegionMask buildRegionMask(const std::vector& selections) { + RegionMask mask; + for (const auto& selection : selections) { + const int index = findRegionConfigIndex(selection.first, selection.second); + if (index < 0) { + throw std::invalid_argument("Invalid tactile region selection."); + } + mask.set(static_cast(index)); + } + return mask; + } + + RegionMask buildSingleRegionMask(const FingerType finger, const TactileRegion region) { + RegionMask mask; + const int index = findRegionConfigIndex(finger, region); + if (index < 0) { + throw std::invalid_argument("Invalid tactile region for selected finger."); + } + mask.set(static_cast(index)); + return mask; + } + + std::vector buildReadPlan(const RegionMask& mask) { + std::vector segments; + for (size_t i = 0; i < kTactileRegionConfigs.size(); ++i) { + if (!mask.test(i)) { + continue; + } + + const auto& config = kTactileRegionConfigs[i]; + if (!segments.empty()) { + auto& last_segment = segments.back(); + const int last_end_register = last_segment.start_register + last_segment.register_count; + if (last_end_register == config.start_register) { + last_segment.register_count += config.register_count; + last_segment.region_indexes.push_back(i); + continue; + } + } + + TactileReadSegment segment; + segment.start_register = config.start_register; + segment.register_count = config.register_count; + segment.region_indexes.push_back(i); + segments.push_back(std::move(segment)); + } + return segments; + } + + std::array encodeAngleCommand( + const std::vector& finger_joint_angles) { + if (finger_joint_angles.size() != RH56DFTPDexhand::ANGLE_COMMAND_COUNT) { + throw std::invalid_argument("RH56DFTPDexhand expects exactly 6 joint angles."); + } + + std::array registers{}; + for (size_t i = 0; i < finger_joint_angles.size(); ++i) { + registers[i] = static_cast(finger_joint_angles[i] & 0xFFFF); + } + return registers; + } + } ModbusController::~ModbusController() { close(); } -bool ModbusController::open(const std::string& ip, int port) { - ctx = modbus_new_tcp(ip.c_str(), port); - if (modbus_connect(ctx) == -1) { - std::cerr << "Modbus连接失败: " << modbus_strerror(errno) << std::endl; - modbus_free(ctx); - ctx = nullptr; - return false; +void ModbusController::open(const std::string& ip, const int port) { + std::lock_guard lock(io_mutex_); + closeUnlocked(); + + ctx_ = modbus_new_tcp(ip.c_str(), port); + if (ctx_ == nullptr) { + throw std::runtime_error("Failed to create Modbus TCP context."); + } + + if (modbus_connect(ctx_) == -1) { + const std::string error = modbus_strerror(errno); + closeUnlocked(); + throw std::runtime_error("Failed to connect Modbus TCP: " + error); } - return true; } void ModbusController::close() { - if (ctx != nullptr) { - modbus_close(ctx); - modbus_free(ctx); - ctx = nullptr; + std::lock_guard lock(io_mutex_); + closeUnlocked(); +} + +bool ModbusController::isOpen() const { + std::lock_guard lock(io_mutex_); + return ctx_ != nullptr; +} + +void ModbusController::writeRegisters(const int address, const uint16_t* values, const int count) { + std::lock_guard lock(io_mutex_); + if (ctx_ == nullptr) { + throw std::runtime_error("Modbus connection is not open."); + } + + const int rc = modbus_write_registers(ctx_, address, count, values); + if (rc == -1) { + throw std::runtime_error( + "Failed to write register block at " + std::to_string(address) + ": " + modbus_strerror(errno)); + } + if (rc != count) { + throw std::runtime_error( + "Incomplete register write at " + std::to_string(address) + ", expected " + std::to_string(count) + + ", got " + std::to_string(rc)); } } -bool ModbusController::writeRegisters(int address, const std::vector& values) { - if (ctx == nullptr) { - std::cerr << "Modbus连接未打开" << std::endl; - return false; +void ModbusController::readRegisterBlock(const int start_addr, const int count, std::vector& values) { + values.assign(static_cast(count), 0); + + std::lock_guard lock(io_mutex_); + if (ctx_ == nullptr) { + throw std::runtime_error("Modbus connection is not open."); } - int ret = modbus_write_registers(ctx, address, values.size(), values.data()); - if (ret == -1) { - std::cerr << "写入寄存器失败: " << modbus_strerror(errno) << std::endl; - return false; - } - return true; -} - -std::vector ModbusController::readRegisters(int address, int count) { - std::vector result(count); - - if (ctx == nullptr) { - std::cerr << "Modbus连接未打开" << std::endl; - return {}; - } - - int ret = modbus_read_registers(ctx, address, count, result.data()); - if (ret == -1) { - std::cerr << "读取寄存器失败: " << modbus_strerror(errno) << std::endl; - return {}; - } - - return result; -} - -std::vector ModbusController::readRegisterRange(int start_addr, int end_addr) { - std::vector register_values; - for (int addr = start_addr; addr <= end_addr; addr += MAX_REGISTERS_PER_READ) { - int current_count = std::min(MAX_REGISTERS_PER_READ, end_addr - addr + 1); - uint16_t tab_reg[MAX_REGISTERS_PER_READ]; - int rc = modbus_read_registers(ctx, addr, current_count, tab_reg); + for (int offset = 0; offset < count; offset += kMaxRegistersPerRead) { + const int current_count = std::min(kMaxRegistersPerRead, count - offset); + const int rc = modbus_read_registers(ctx_, start_addr + offset, current_count, values.data() + offset); if (rc == -1) { - std::cerr << "读取寄存器 " << addr << " 失败: " << modbus_strerror(errno) << std::endl; - register_values.insert(register_values.end(), current_count, 0); - } else { - register_values.insert(register_values.end(), tab_reg, tab_reg + current_count); + throw std::runtime_error( + "Failed to read register block at " + std::to_string(start_addr + offset) + ": " + + modbus_strerror(errno)); } - } - return register_values; -} - -bool ModbusController::write6(const std::string& reg_name, const std::vector& val) { - if (reg_name != "angleSet" && reg_name != "forceSet" && reg_name != "speedSet") { - std::cerr << "函数调用错误，正确方式：str的值为\'angleSet\'/\'forceSet\'/\'speedSet\'，val为长度为6的list，值为0~1000，允许使用-1作为占位符" << std::endl; - return false; - } - - if (val.size() != 6) { - std::cerr << "错误：val必须是长度为6的向量" << std::endl; - return false; - } - - std::vector val_reg(6); - for (int i = 0; i < 6; i++) { - val_reg[i] = val[i] & 0xFFFF; // 取低16位 - } - - return writeRegisters(regdict[reg_name], val_reg); -} - -bool ModbusController::read6(const std::string& reg_name, std::vector& result) { - // 检查寄存器名称是否在允许的范围内 - auto it = regdict.find(reg_name); - if (reg_name == "angleSet" || reg_name == "forceSet" || reg_name == "speedSet" || - reg_name == "angleAct" || reg_name == "forceAct" || reg_name == "posAct") { - // 直接读取与reg_name对应的寄存器，读取的数量为6 - auto values = readRegisters(it->second, 6); - if (values.size() < 6) { - LOG(ERROR) << "[ModbusController](read6): empty register data"; - return false; + if (rc != current_count) { + throw std::runtime_error( + "Incomplete register read at " + std::to_string(start_addr + offset) + ", expected " + + std::to_string(current_count) + ", got " + std::to_string(rc)); } - for (int i = 0; i < 6; i++) - { - result.emplace_back(values[i]); - } - return true; - } - else if (reg_name == "error" || reg_name == "current" || reg_name == "temperature") { - // 读取错误代码、状态代码或温度，每次读取3个寄存器 - std::vector val_act = readRegisters(regdict[reg_name], 3); - if (val_act.size() < 3) { - LOG(ERROR) << "[ModbusController](read6): empty register data"; - return false; - } - - // 初始化存储高低位的数组 - std::vector results; - - // 将每个寄存器的高位和低位分开存储 - for (size_t i = 0; i < val_act.size(); i++) { - // 读取当前寄存器和下一个寄存器 - uint8_t low_byte = val_act[i] & 0xFF; // 低八位 - uint8_t high_byte = (val_act[i] >> 8) & 0xFF; // 高八位 - - results.push_back(low_byte); // 存储低八位 - results.push_back(high_byte); // 存储高八位 - } - - for (int i = 0; i < 6; i++) - { - result.emplace_back(results[i]); - } - return true; - } - else { - LOG(ERROR) << "[ModbusController](read6): invalid parameter"; - return false; } } -// 延时函数，单位为秒 -void ModbusController::sleep(float seconds) { - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(seconds * 1000))); +void ModbusController::closeUnlocked() { + if (ctx_ != nullptr) { + modbus_close(ctx_); + modbus_free(ctx_); + ctx_ = nullptr; + } } -RH56DFTPDexhand::RH56DFTPDexhand(const XmlNode& cfg):AbstractDexHand(cfg) { - try { - id_ = cfg_.getAttrString("id"); - default_force_ = clamp(cfg_.getAttrDefault("default_force", 500), 0, 3000); - default_speed_= clamp(cfg_.getAttrDefault("default_speed", 500), 0, 1000); - ip_address_ = cfg_.getAttrString("ip_address"); - port_ = cfg_.getAttrDefault("port", 6000); - hand_tactile_sensors_.init(); - } - catch (const exception& e) { - LOG(ERROR) << "[RH56DFTPDexhand] ([RH56DFTPDexhand]): Failed to parse XML: " << e.what(); - } - +RH56DFTPDexhand::RH56DFTPDexhand(const XmlNode& cfg) + : AbstractDexHand(cfg), + controller_(std::make_unique()) { + parseXmlConfig(); + initializeTactileBuffers(); } -RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg):dexhandCfg_(cfg) -{ - try { - id_ = dexhandCfg_.id(); - ip_address_ = dexhandCfg_.ip(); +RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg) + : controller_(std::make_unique()), + dexhandCfg_(cfg) { + id_ = dexhandCfg_.id(); + ip_address_ = dexhandCfg_.ip(); + if (dexhandCfg_.port() > 0) { port_ = dexhandCfg_.port(); - hand_tactile_sensors_.init(); } - catch (const exception& e) { - LOG(ERROR) << "[RH56DFTPDexhand] ([RH56DFTPDexhand]): Failed to parse XML: " << e.what(); - } - + initializeTactileBuffers(); } - RH56DFTPDexhand::~RH56DFTPDexhand() { + stop(); } - void RH56DFTPDexhand::init() { - //按照xml预设参数设置 - - controller_ = std::make_shared(); - if (!controller_->open(ip_address_, port_)) { - throw runtime_error("[RH56DFTPDexhand] ([init]): modbus_connect Failed ,id= " + id_ + ",ipaddress=" + ip_address_); + try { + ensureConnected(); + } catch (const std::exception& e) { + enterFault("[RH56DFTPDexhand](init): " + std::string(e.what())); + throw; } - status_.is_initialized = true; - - LOG(INFO)<< "[RH56DFTPDexhand](init) sucess, id="<< id_; -} - -void RH56DFTPDexhand::updateState() { - //读取数据，初始化 - std::vector parsed_values; - controller_->read6("angleAct",parsed_values); - //解析数据 - //自度度实际角度，6个数据，两个字节表示一个角度值 - //保存到status_结构体中 - if (!parsed_values.empty()) { - for (int i = 0; i < 6; i++) { - status_.hands[i].angle = parsed_values[i]; - } - parsed_values.clear(); - } - //执行器位置实际位置 - - controller_->read6("posAct",parsed_values); - //保存到status_结构体中 - if (!parsed_values.empty()) { - for (int i = 0; i < 6; i++) { - status_.hands[i].position = parsed_values[i]; - } - parsed_values.clear(); - } - - - controller_->read6("speedSet",parsed_values); - //保存到status_结构体中 - if (!parsed_values.empty()) { - for (int i = 0; i < 6; i++) { - status_.hands[i].speed = parsed_values[i]; - } - parsed_values.clear(); - } - - //手指实际受力 - - controller_->read6("forceAct",parsed_values); - //保存到status_结构体中 - if (!parsed_values.empty()) - { - for (int i = 0; i < 6; i++) { - status_.hands[i].force = parsed_values[i]; - } - parsed_values.clear(); - } - - - //执行器电流值 - - controller_->read6("current",parsed_values); - //保存到status_结构体中 - if (!parsed_values.empty()) { - for (int i = 0; i < 6; i++) { - status_.hands[i].current = parsed_values[i]; - } - parsed_values.clear(); - } - - - //执行器故障信息 - - controller_->read6("error",parsed_values); - //保存到status_结构体中 - if (!parsed_values.empty()) - { - for (int i = 0; i < 6; i++) { - status_.hands[i].error = parsed_values[i]; - //error信息要解析为中文吗？ - auto eeror_message = parse_error(status_.hands[i].error); - if (!eeror_message.empty()) { - status_.hands[i].error_message = eeror_message; - } - } - parsed_values.clear(); - } - - //执行器温度 - controller_->read6("temperature",parsed_values); - //保存到status_结构体中 - if (!parsed_values.empty()) { - for (int i = 0; i < 6; i++) { - status_.hands[i].temperature = parsed_values[i]; - } - } -} -void printPinkySensorData(const HandTactileSensors& sensors) { - // 打印小拇指指端数据 - std::cout << "=== 小拇指指端数据 (" << sensors.pinky.tip.rows << "x" << sensors.pinky.tip.cols << ") ===\n"; - for (const auto& row : sensors.pinky.tip.data) { - for (TactilePoint value : row) { - std::cout << std::setw(5) << value; // 每个值占5个字符宽度 - } - std::cout << "\n"; - } - std::cout << "\n"; - - // 打印小拇指指尖数据 - std::cout << "=== 小拇指指尖数据 (" << sensors.pinky.finger.rows << "x" << sensors.pinky.finger.cols << ") ===\n"; - for (const auto& row : sensors.pinky.finger.data) { - for (TactilePoint value : row) { - std::cout << std::setw(5) << value; - } - std::cout << "\n"; - } - std::cout << "\n"; - - // 打印小拇指指腹数据 - std::cout << "=== 小拇指指腹数据 (" << sensors.pinky.pad.rows << "x" << sensors.pinky.pad.cols << ") ===\n"; - for (const auto& row : sensors.pinky.pad.data) { - for (TactilePoint value : row) { - std::cout << std::setw(5) << value; - } - std::cout << "\n"; - } -} - -void RH56DFTPDexhand::updateSensorData() { - //触觉传感器数据。 - // 读取各部分数据 - auto pinky_register_values = controller_->readRegisterRange(TOUCH_SENSOR_BASE_ADDR_PINKY, TOUCH_SENSOR_END_ADDR_PINKY); - auto ring_register_values = controller_->readRegisterRange(TOUCH_SENSOR_BASE_ADDR_RING, TOUCH_SENSOR_END_ADDR_RING); - auto middle_register_values = controller_->readRegisterRange(TOUCH_SENSOR_BASE_ADDR_MIDDLE, TOUCH_SENSOR_END_ADDR_MIDDLE); - auto index_register_values = controller_->readRegisterRange(TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX); - auto thumb_register_values = controller_->readRegisterRange(TOUCH_SENSOR_BASE_ADDR_THUMB, TOUCH_SENSOR_END_ADDR_THUMB); - auto palm_register_values = controller_->readRegisterRange(TOUCH_SENSOR_BASE_ADDR_PALM, TOUCH_SENSOR_END_ADDR_PALM); - - hand_tactile_sensors_.parseFinger(FingerType::PINKY,pinky_register_values); - hand_tactile_sensors_.parseFinger(FingerType::RING,ring_register_values); - hand_tactile_sensors_.parseFinger(FingerType::MIDDLE,middle_register_values); - hand_tactile_sensors_.parseFinger(FingerType::INDEX,index_register_values); - hand_tactile_sensors_.parseFinger(FingerType::THUMB,thumb_register_values); - hand_tactile_sensors_.parsePalm(palm_register_values); - //LOG(INFO)<< "[RH56DFTPDexhand](updateSensorData) get sensor data success"; - } void RH56DFTPDexhand::start() { + if (tactile_thread_running_.exchange(true, std::memory_order_acq_rel)) { + transitionTo(Status::STREAMING); + return; + } -} + try { + if (tactile_thread_.joinable()) { + tactile_thread_.join(); + } -void RH56DFTPDexhand::stop() { + ensureConnected(); -} + RegionMask initial_mask; + { + std::lock_guard lock(polling_mutex_); + initial_mask = requested_polling_mask_; + } + if (initial_mask.any()) { + refreshTactileData(initial_mask); + } -void RH56DFTPDexhand::getState(DexHandState &state) { - updateState(); - - state.is_initialized = status_.is_initialized; - for (int i = 0; i < 6; i++) - { - state.hands[i].angle = status_.hands[i].angle; - state.hands[i].position = status_.hands[i].position; - state.hands[i].speed = status_.hands[i].speed; - state.hands[i].force = status_.hands[i].force; - state.hands[i].current = status_.hands[i].current; - state.hands[i].error = status_.hands[i].error; - state.hands[i].error_message = status_.hands[i].error_message; - state.hands[i].temperature = status_.hands[i].temperature; + tactile_thread_ = std::thread(&RH56DFTPDexhand::tactilePollingLoop, this); + transitionTo(Status::STREAMING); + polling_cv_.notify_all(); + } catch (const std::exception& e) { + tactile_thread_running_.store(false, std::memory_order_release); + enterFault("[RH56DFTPDexhand](start): " + std::string(e.what())); + throw; } } -// control -void RH56DFTPDexhand::setPositions(const std::vector& finger_joint_targets) { - //分别对应的6个自由度 - controller_->write6("posSet",finger_joint_targets); +void RH56DFTPDexhand::stop() { + tactile_thread_running_.store(false, std::memory_order_release); + polling_cv_.notify_all(); + + if (tactile_thread_.joinable() && tactile_thread_.get_id() != std::this_thread::get_id()) { + tactile_thread_.join(); + } + + if (controller_) { + controller_->close(); + } + + if (state() != Status::FAULT) { + transitionTo(Status::STOPPED); + } +} + +RH56DFTPDexhand::Status RH56DFTPDexhand::state() const { + std::lock_guard lock(lifecycle_mutex_); + return lifecycle_state_; +} + +std::string RH56DFTPDexhand::lastError() const { + std::lock_guard lock(lifecycle_mutex_); + return last_error_; +} + +void RH56DFTPDexhand::getState(DexHandState& state_out) { + DexHandState next_state{}; + next_state.is_initialized = isOperationalState(state()); + + { + std::lock_guard lock(command_mutex_); + for (size_t i = 0; i < last_commanded_angles_.size(); ++i) { + next_state.hands[i].angle = last_commanded_angles_[i]; + } + } + + const auto error = lastError(); + if (!error.empty()) { + next_state.hands[0].error_message.push_back(error); + } + + state_out = std::move(next_state); } void RH56DFTPDexhand::setAngles(const std::vector& finger_joint_angles) { - //分别对应的6个自由度 - controller_->write6("angleSet",finger_joint_angles); -} -void RH56DFTPDexhand::setVelocities(const std::vector& finger_joint_velocities) { - controller_->write6("speedSet",finger_joint_velocities); -} -void RH56DFTPDexhand::setPresetAct(int action_id) { - LOG(WARNING) << "[RH56DFTPDexhand](setPresetAct): The current interface is not yet implemented."; - throw runtime_error("[RH56DFTPDexhand](setPresetAct): The current interface is not yet implemented."); -} -void RH56DFTPDexhand::execPresetAct(int action_id) { + const auto registers = encodeAngleCommand(finger_joint_angles); + try { + ensureConnected(); + controller_->writeRegisters( + toRegisterAddress(kAngleSetByteAddress), + registers.data(), + static_cast(registers.size())); + + std::lock_guard lock(command_mutex_); + for (size_t i = 0; i < finger_joint_angles.size(); ++i) { + last_commanded_angles_[i] = finger_joint_angles[i]; + } + } catch (const std::exception& e) { + enterFault("[RH56DFTPDexhand](setAngles): " + std::string(e.what())); + throw; + } } -void RH56DFTPDexhand::setForce(const std::vector& finger_joint_force){ - controller_->write6("forceSet",finger_joint_force); +void RH56DFTPDexhand::setTactilePollingRegions(const std::vector& regions) { + const RegionMask next_mask = regions.empty() ? RegionMask{} : buildRegionMask(regions); + { + std::lock_guard lock(polling_mutex_); + requested_polling_mask_ = next_mask; + } + polling_cv_.notify_all(); + + if (next_mask.any() && isOperationalState(state())) { + refreshTactileData(next_mask); + } } -HandTactileSensors& RH56DFTPDexhand::getSensorData() -{ - updateSensorData(); - return hand_tactile_sensors_; - +std::vector RH56DFTPDexhand::getSensorData() { + ensureTactileMaskReady(buildFullRegionMask(), true); + return acquireAllRegionData(); } -HandTactileSensors& RH56DFTPDexhand::getSensorData(const FingerType finger_type, - const TactileRegion tactile_region) -{ - (void)tactile_region; - const auto [start_addr, end_addr] = fingerSensorRegisterRange(finger_type); - const auto finger_register_values = controller_->readRegisterRange(start_addr, end_addr); - hand_tactile_sensors_.parseFinger(finger_type, finger_register_values); - return hand_tactile_sensors_; +TactileRegionData RH56DFTPDexhand::getSensorData(FingerType finger, TactileRegion region) { + ensureTactileMaskReady(buildSingleRegionMask(finger, region), true); + return acquireRegionData(finger, region); +} + +void RH56DFTPDexhand::parseXmlConfig() { + id_ = cfg_.getAttrString("id"); + ip_address_ = cfg_.getAttrString("ip_address"); + port_ = cfg_.getAttrDefault("port", kDefaultPort); +} + +void RH56DFTPDexhand::initializeTactileBuffers() { + for (auto& tactile_buffer : tactile_buffers_) { + tactile_buffer.clear(); + } + tactile_buffer_masks_.fill(RegionMask{}); + requested_polling_mask_ = buildFullRegionMask(); + active_buffer_index_.store(0, std::memory_order_release); +} + +void RH56DFTPDexhand::ensureConnected() { + if (ip_address_.empty()) { + throw std::runtime_error("Dexhand IP address is not configured."); + } + + if (!controller_) { + controller_ = std::make_unique(); + } + + if (controller_->isOpen()) { + if (!isOperationalState(state())) { + transitionTo(Status::INITIALIZED); + } + return; + } + + controller_->open(ip_address_, port_); + transitionTo(Status::INITIALIZED); +} + +void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { + if (mask.none()) { + return; + } + + try { + ensureConnected(); + + const auto read_plan = buildReadPlan(mask); + std::lock_guard refresh_lock(tactile_refresh_mutex_); + + const int active_index = active_buffer_index_.load(std::memory_order_acquire); + const int write_index = 1 - active_index; + auto& write_buffer = tactile_buffers_[write_index]; + RegionMask next_mask; + std::vector segment_values; + + for (const auto& segment : read_plan) { + controller_->readRegisterBlock(segment.start_register, segment.register_count, segment_values); + for (const size_t region_index : segment.region_indexes) { + const auto& config = kTactileRegionConfigs[region_index]; + const int offset = config.start_register - segment.start_register; + write_buffer.assignRegionData( + config.finger, + config.region, + segment_values.data() + offset, + config.register_count); + next_mask.set(region_index); + } + } + + std::unique_lock lock(tactile_buffer_mutex_); + tactile_buffer_masks_[write_index] = next_mask; + active_buffer_index_.store(write_index, std::memory_order_release); + } catch (const std::exception& e) { + enterFault("[RH56DFTPDexhand](refreshTactileData): " + std::string(e.what())); + throw; + } +} + +void RH56DFTPDexhand::tactilePollingLoop() { + std::unique_lock lock(polling_mutex_); + while (tactile_thread_running_.load(std::memory_order_acquire)) { + if (requested_polling_mask_.none()) { + polling_cv_.wait(lock, [this]() { + return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any(); + }); + continue; + } + + const RegionMask mask = requested_polling_mask_; + lock.unlock(); + try { + refreshTactileData(mask); + } catch (const std::exception& e) { + LOG(ERROR) << e.what(); + } + lock.lock(); + + polling_cv_.wait_for(lock, tactile_poll_interval_, [this, mask]() { + return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask; + }); + } +} + +std::shared_ptr RH56DFTPDexhand::acquireActiveBufferLease() const { + std::shared_lock lock(tactile_buffer_mutex_); + const int active_index = active_buffer_index_.load(std::memory_order_acquire); + auto lease = std::make_shared(); + lease->lock = std::move(lock); + lease->buffer = &tactile_buffers_[active_index]; + return lease; +} + +std::vector RH56DFTPDexhand::acquireAllRegionData() const { + auto lease = acquireActiveBufferLease(); + std::vector regions; + regions.reserve(RH56TactileBuffer::REGION_COUNT); + + for (const auto& layout : lease->buffer->layouts()) { + regions.emplace_back( + layout.finger, + layout.region, + layout.view(lease->buffer->data()), + layout.name, + lease); + } + return regions; +} + +TactileRegionData RH56DFTPDexhand::acquireRegionData(FingerType finger, TactileRegion region) const { + auto lease = acquireActiveBufferLease(); + const auto& layout = lease->buffer->regionLayout(finger, region); + return TactileRegionData( + layout.finger, + layout.region, + layout.view(lease->buffer->data()), + layout.name, + lease); +} + +bool RH56DFTPDexhand::isRequestedMaskCovered(const RegionMask& mask) const { + std::lock_guard lock(polling_mutex_); + return (requested_polling_mask_ & mask) == mask; +} + +bool RH56DFTPDexhand::isActiveBufferReady(const RegionMask& mask) const { + std::shared_lock lock(tactile_buffer_mutex_); + const int active_index = active_buffer_index_.load(std::memory_order_acquire); + return (tactile_buffer_masks_[active_index] & mask) == mask; +} + +void RH56DFTPDexhand::ensureTactileMaskReady(const RegionMask& mask, const bool allow_background) { + if (mask.none()) { + return; + } + + const bool background_ready = allow_background && + tactile_thread_running_.load(std::memory_order_acquire) && + isRequestedMaskCovered(mask) && + isActiveBufferReady(mask); + + if (!background_ready) { + refreshTactileData(mask); + } +} + +void RH56DFTPDexhand::transitionTo(const Status next_state) { + std::lock_guard lock(lifecycle_mutex_); + lifecycle_state_ = next_state; + if (next_state == Status::INITIALIZED || + next_state == Status::STREAMING) { + last_error_.clear(); + } +} + +void RH56DFTPDexhand::enterFault(const std::string& error) { + { + std::lock_guard lock(lifecycle_mutex_); + lifecycle_state_ = Status::FAULT; + last_error_ = error; + } + + tactile_thread_running_.store(false, std::memory_order_release); + polling_cv_.notify_all(); + + if (controller_) { + controller_->close(); + } + + LOG(ERROR) << error; +} + +bool RH56DFTPDexhand::isOperationalState(const Status lifecycle) const { + return lifecycle == Status::INITIALIZED || + lifecycle == Status::STREAMING; } diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp new file mode 100644 index 00000000..e014323e --- /dev/null +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp @@ -0,0 +1,137 @@ +#include "gtest/gtest.h" + +#include "../include/rh56dftp_dexhand.h" +#include "cmvr/config/dexhand_config/dexhand_config.pb.h" +#include "common/utils/config_helper/include/config_helper.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using DexHand = cmvr::device::AbstractDexHand; +using RH56DexHand = cmvr::device::RH56DFTPDexhand; + +const char* tactileRegionToString(const DexHand::TactileRegion region) { + switch (region) { + case DexHand::TactileRegion::TIP: return "TIP"; + case DexHand::TactileRegion::FINGER: return "FINGER"; + case DexHand::TactileRegion::PAD: return "PAD"; + case DexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE"; + case DexHand::TactileRegion::PALM_PAD: return "PALM_PAD"; + } + return "UNKNOWN"; +} + +struct StopGuard { + std::shared_ptr hand; + + ~StopGuard() { + if (!hand) { + return; + } + try { + hand->stop(); + } catch (...) { + } + } +}; + +} // namespace + +TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) { + cmvr::config::DexHandConfig dexhand_config; + ASSERT_TRUE(cmvr::ConfigHelper::getDexHandsConfig(dexhand_config)); + + const cmvr::config::RH56DFTPDexHandConfig* hand_config = nullptr; + for (const auto& config : dexhand_config.rh56dftp_dexhands()) { + if (config.enable() && !config.ip().empty()) { + hand_config = &config; + break; + } + if (hand_config == nullptr && !config.ip().empty()) { + hand_config = &config; + } + } + + ASSERT_NE(hand_config, nullptr); + + const auto finger = DexHand::FingerType::INDEX; + const auto region = DexHand::TactileRegion::FINGER; + const int iterations = 20000; + const int warmup_ms = 200; + const int read_interval_ms = 10; + + auto hand = std::make_shared(*hand_config); + ASSERT_NO_THROW(hand->init()); + ASSERT_NO_THROW(hand->start()); + StopGuard stop_guard{hand}; + + EXPECT_EQ(hand->state(), DexHand::Status::STREAMING); + ASSERT_NO_THROW(hand->setTactilePollingRegion(finger, region)); + + if (warmup_ms > 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms)); + } + + double last_pressure_sum = 0.0; + double last_pressure_peak = 0.0; + int point_count = 0; + std::string sensor_name; + + for (int i = 0; i < iterations; ++i) { + const auto region_data = hand->getSensorData(finger, region); + + ASSERT_TRUE(region_data.valid()); + EXPECT_EQ(region_data.finger, finger); + EXPECT_EQ(region_data.region, region); + + if (point_count == 0) { + point_count = region_data.view.pointCount(); + sensor_name = region_data.name == nullptr ? "" : region_data.name; + } + + 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(region_data.view.data[index]); + last_pressure_sum += pressure; + if (pressure > last_pressure_peak) { + last_pressure_peak = pressure; + } + } + + std::cout << std::fixed << std::setprecision(3) + << "[RH56DFTPDexhandLatencyTest] iter=" << (i + 1) + << "/" << iterations + << " sensor=" << sensor_name + << " pressure_sum=" << last_pressure_sum + << " pressure_peak=" << last_pressure_peak + << std::endl; + + if (read_interval_ms > 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms)); + } + } + + std::cout << std::fixed << std::setprecision(3) + << "[RH56DFTPDexhandLatencyTest] id=" << hand_config->id() + << " ip=" << hand_config->ip() + << " port=" << hand_config->port() + << " finger=INDEX" + << " region=" << tactileRegionToString(region) + << " sensor=" << sensor_name + << " iterations=" << iterations + << " points=" << point_count + << " last_pressure_sum=" << last_pressure_sum + << " last_pressure_peak=" << last_pressure_peak + << " read_interval_ms=" << read_interval_ms + << "\n"; + + EXPECT_GT(point_count, 0); +} \ No newline at end of file diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 823caf44..5759c3af 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -12,6 +12,7 @@ #include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h" #include "cmvr/msgs/geometry.pb.h" #include "cmvr/msgs/motor.pb.h" +#include namespace cmvr::device{ @@ -103,18 +104,23 @@ namespace cmvr::device{ virtual void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } - virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } - - virtual void moveL(std::string &base_link, std::vector &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } - - virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } - virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } + virtual bool moveL(const std::vector &pose, + double speed = 0.25, + double acceleration = 1.2, + double jerk = 5.0, + const std::vector &qd_max = std::vector(7, 2.5), + bool asynchronous = false) { + throw std::runtime_error("Not implemented"); + } virtual bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0, cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) { throw std::runtime_error("Not implemented"); } + virtual Eigen::Matrix getSpeedLCommandTwistBase() { + return Eigen::Matrix::Zero(); + } virtual void stopSpeedL() { throw std::runtime_error("Not implemented"); } virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } @@ -173,8 +179,10 @@ namespace cmvr::device{ { toolFrame_ = toolFrame; } + virtual std::string getToolFrame() const { return toolFrame_; } virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; + virtual msgs::Pose3d fk(bool is_tcp = true) = 0; virtual std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0; protected: int dof_{}; diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 51bf2d4e..7770997b 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -86,10 +86,12 @@ namespace cmvr::device{ void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override; void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, double acc) override ; void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; - - void moveL(std::string &base_link, std::vector &targets, double vel, double acc) override; - void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) override; - void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) override; + bool moveL(const std::vector &pose, + double speed = 0.25, + double acceleration = 1.2, + double jerk = 5.0, + const std::vector &qd_max = std::vector(7, 2.5), + bool asynchronous = false) override; void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override; @@ -99,7 +101,7 @@ namespace cmvr::device{ double time = 0.0, cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override; void stopSpeedL() override; - Eigen::Matrix getSpeedLCommandTwistBase(); + Eigen::Matrix getSpeedLCommandTwistBase() override; @@ -124,6 +126,14 @@ namespace cmvr::device{ void ensureSpeedLWorkerStarted_(); void stopSpeedLWorker_(); void speedLWorkerLoop_(); + bool executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input, + double v_tcp_max, + double a_tcp_max, + double j_tcp_max, + const std::vector& qd_max, + double dt_real, + bool is_tcp, + cmvr::CartesianFrame input_frame); @@ -148,16 +158,10 @@ namespace cmvr::device{ static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz); static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R); msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override; + msgs::Pose3d fk(bool is_tcp) override; std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override; - double calculateMoveTime(double distance, double vel, double acc); - void generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc, - double total_time, size_t num_points, - std::vector& time_points, - std::vector& distance_ratios); - - private: std::string id_; int upd_freq_; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 0e4341ce..0146c053 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -899,181 +899,6 @@ void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vecto } } -template -void HumanoidRobot::moveL(std::string &base_link, std::vector &targets, double vel, - double acc) { - try { - stopSpeedLWorker_(); - // 获取当前关节状态 - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - // 获取基座链接索引 - auto base_idx = m_robot_->GetLinkIdx(base_link); - - // 获取当前末端位姿 - 使用前向运动学计算 - std::vector current_poses; - for (const auto &target: targets) { - auto ee_idx = m_robot_->GetLinkIdx(target.link_name); - - // 使用正向运动学计算当前位姿 - Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); - current_poses.push_back(T); - - // 打印当前末端执行器的 XYZ 和欧拉角 - if (&target == &targets.front()) { - Eigen::Vector3d position = T.block<3, 1>(0, 3); - Eigen::Matrix3d rotation = T.block<3, 3>(0, 0); - Eigen::Vector3d euler = rotationMatrixToEulerZYX(rotation); - - LOG(INFO) << "Starting point (Initial position): " - << "X: " << position[0] << ", Y: " << position[1] << ", Z: " << position[2]; - LOG(INFO) << "Starting orientation (Euler angles): " - << "RX: " << euler[0] << ", RY: " << euler[1] << ", RZ: " << euler[2]; - } - } - - // 计算最大距离和插值点数 - double max_distance = 0.0; - for (size_t i = 0; i < targets.size(); i++) { - Eigen::Vector3d current_pos = current_poses[i].block<3, 1>(0, 3); - Eigen::Vector3d target_pos = targets[i].T_target.block<3, 1>(0, 3); - double distance = (target_pos - current_pos).norm(); - max_distance = std::max(max_distance, distance); - } - - // 基于速度和距离计算插值点数 - double move_time = max_distance / vel; - int num_points = static_cast(move_time * 100); // 100Hz控制频率 - - // 存储所有插值点的关节角度 - std::vector > joint_trajectory; - joint_trajectory.reserve(num_points + 1); - - // 记录上一次成功的关节角度 - Eigen::Vector last_success_q = q_init; - - // 预先计算所有插值点的逆运动学 - for (int i = 0; i <= num_points; i++) { - if (flash_cmd_.load()) { - flash_cmd_.store(false); - rsm_.store(ROBOT_READY); - return; - } - - double t = static_cast(i) / num_points; - - // 创建插值后的目标（只做位置插值，旋转保持不变） - std::vector interpolated_targets = targets; - for (size_t j = 0; j < targets.size(); j++) { - // 位置线性插值 - Eigen::Vector3d current_pos = current_poses[j].block<3, 1>(0, 3); - Eigen::Vector3d target_pos = targets[j].T_target.block<3, 1>(0, 3); - Eigen::Vector3d interp_pos = current_pos + t * (target_pos - current_pos); - - // 保持旋转不变 - Eigen::Matrix3d current_rot_matrix = current_poses[j].block<3, 3>(0, 0); - interpolated_targets[j].T_target.setIdentity(); - interpolated_targets[j].T_target.block<3, 3>(0, 0) = current_rot_matrix; - interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos; - } - - // 求解逆运动学 - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - - if (!ok) { - LOG(WARNING) << "IK failed at point " << i << ", using last successful configuration"; - q_cmd = last_success_q; - } else { - last_success_q = q_cmd; - } - - joint_trajectory.push_back(q_cmd); - - // 获取当前末端执行器的位置 (通过正向运动学) - m_state_->SetQ(q_cmd); - m_robot_->ComputeForwardKinematics(m_state_); - - // 获取当前末端执行器的位姿 (变换矩阵 T) - auto ee_idx = m_robot_->GetLinkIdx(targets[0].link_name); - Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); - - // 从变换矩阵中提取 XYZ 坐标 - Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3); - - // 打印 IK 解算出的 XYZ 位置 - if (i % 10 == 0) { - // 每10个点打印一次，避免日志过多 - LOG(INFO) << "IK solution at point " << i << " : " - << "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos - [2]; - } - } - - // 执行轨迹 - for (int i = 0; i <= num_points; i++) { - if (flash_cmd_.load()) { - flash_cmd_.store(false); - break; - } - - // 获取当前时间点的关节角度 - Eigen::Vector q_cmd = joint_trajectory[i]; - - // 发送关节命令 - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; - - // 设置每个关节的速度和位置 - for (size_t j = 0; j < joint_points.size(); j++) { - auto &joint_point = joint_points[j]; - auto motor = motor_manager_->getMotor(joint_point.joint_name); - if (motor != nullptr) { - motor->setQ(joint_point.rad); - } - } - - // 等待一段时间，控制频率 - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - // 等待最终位置到达 - 检查所有关节 - bool completion = true; - do { - completion = true; - for (const auto &name: joint_names_) { - auto motor = motor_manager_->getMotor(name); - if (motor != nullptr && !motor->reachedTargetQ()) { - completion = false; - break; - } - } - if (flash_cmd_.load()) { - flash_cmd_.store(false); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } while (!completion); - } catch (std::exception &e) { - throw std::runtime_error(e.what()); - } -} - template void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) { @@ -1121,8 +946,7 @@ void HumanoidRobot::servoJ(std::vector &joints, double dt) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } - motor->setQd(j.vel); - motor->setQ(j.rad); + motor->setTarget(j.rad,j.vel); } } rsm_.store(ROBOT_READY); @@ -1137,8 +961,7 @@ void HumanoidRobot::servoJ(std::vector &joints, double vel, dou if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } - motor->setQd(vel); - motor->setQ(j.rad); + motor->setTarget(j.rad,vel); } } } @@ -1383,36 +1206,95 @@ std::vector HumanoidRobot< template cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const std::string &ee_link) { cmvr::msgs::Pose3d pose; + static const std::vector kRightArmJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", + "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" + }; try { - // 获取当前关节角度 - Eigen::Vector q; - auto q_map = getJointQ(); // 类似 moveJ 中获取关节角度 - q << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; + if (!ik_solver_) { + throw std::runtime_error("ik_solver_ is not initialized"); + } - LOG(INFO) << "q_current: " << q; - // 更新状态并计算前向运动学 - m_state_->SetQ(q); - m_robot_->ComputeForwardKinematics(m_state_); + const auto q_map = getJointQ(); + std::vector q_current; + q_current.reserve(kRightArmJointNames.size()); + for (const auto& joint_name : kRightArmJointNames) { + const auto it = q_map.find(joint_name); + if (it == q_map.end()) { + throw std::runtime_error("missing joint state for " + joint_name); + } + q_current.push_back(it->second); + } - // 获取基座和末端索引 - auto base_idx = m_robot_->GetLinkIdx(base_link); - auto ee_idx = m_robot_->GetLinkIdx(ee_link); + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + if (!ik_solver_->fk(base_link, ee_link, q_current, T)) { + throw std::runtime_error("PinocchioDlsIKSolver::fk failed"); + } - // 获取变换矩阵 - Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); - - // 填充 Pose3d pose.mutable_position()->set_x(T(0, 3)); pose.mutable_position()->set_y(T(1, 3)); pose.mutable_position()->set_z(T(2, 3)); - // 将旋转矩阵转换为欧拉角 Eigen::Matrix3d R = T.block<3, 3>(0, 0); - Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); // 你需要实现或已有此工具函数 + const Eigen::Quaterniond q(R); + pose.mutable_quaternion()->set_w(q.w()); + pose.mutable_quaternion()->set_x(q.x()); + pose.mutable_quaternion()->set_y(q.y()); + pose.mutable_quaternion()->set_z(q.z()); + + Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); + pose.mutable_euler()->set_rx(euler(0)); + pose.mutable_euler()->set_ry(euler(1)); + pose.mutable_euler()->set_rz(euler(2)); + } catch (const std::exception &e) { + throw std::runtime_error(std::string("FK计算失败: ") + e.what()); + } + + return pose; +} + +template +cmvr::msgs::Pose3d HumanoidRobot::fk(bool is_tcp) { + cmvr::msgs::Pose3d pose; + static const std::vector kRightArmJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", + "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" + }; + + try { + if (!ik_solver_) { + throw std::runtime_error("ik_solver_ is not initialized"); + } + + const auto q_map = getJointQ(); + std::vector q_current; + q_current.reserve(kRightArmJointNames.size()); + for (const auto& joint_name : kRightArmJointNames) { + const auto it = q_map.find(joint_name); + if (it == q_map.end()) { + throw std::runtime_error("missing joint state for " + joint_name); + } + q_current.push_back(it->second); + } + + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + if (!ik_solver_->fk(q_current, T, is_tcp)) { + throw std::runtime_error("PinocchioDlsIKSolver::fk failed"); + } + + pose.mutable_position()->set_x(T(0, 3)); + pose.mutable_position()->set_y(T(1, 3)); + pose.mutable_position()->set_z(T(2, 3)); + + const Eigen::Matrix3d R = T.block<3, 3>(0, 0); + const Eigen::Quaterniond q(R); + pose.mutable_quaternion()->set_w(q.w()); + pose.mutable_quaternion()->set_x(q.x()); + pose.mutable_quaternion()->set_y(q.y()); + pose.mutable_quaternion()->set_z(q.z()); + + const Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); pose.mutable_euler()->set_rx(euler(0)); pose.mutable_euler()->set_ry(euler(1)); pose.mutable_euler()->set_rz(euler(2)); @@ -1424,397 +1306,161 @@ cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const st } -void printTrajectoryInfo( - const std::vector &trajectory, - const std::vector ×, - const std::vector &velocities, - double total_distance) { - std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl; - std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; - std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl; - std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << - std::endl; - std::cout << std::setw(4) << "序号" << " | " - << std::setw(8) << "时间(s)" << " | " - << std::setw(10) << "x(m)" << " | " - << std::setw(10) << "y(m)" << " | " - << std::setw(10) << "z(m)" << " | " - << std::setw(12) << "速度(m/s)" << " | " - << std::setw(16) << "到起点距离(m)" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << - std::endl; - - Eigen::Vector3d start_pos(trajectory[0](0, 3), trajectory[0](1, 3), trajectory[0](2, 3)); - for (size_t idx = 0; idx < trajectory.size(); ++idx) { - const auto &T = trajectory[idx]; - Eigen::Vector3d pos(T(0, 3), T(1, 3), T(2, 3)); - double dist_from_start = (pos - start_pos).norm(); - - std::cout << std::setw(4) << idx << " | " - << std::fixed << std::setprecision(3) << std::setw(8) << times[idx] << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | " - << std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | " - << std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl; - } - std::cout << "=========================================================================================\n" << - std::endl; -} - - template -void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::string &ee_link, - msgs::Pose3d delta_pose, double vel, double acc) { - try { - Eigen::Vector q_current_for_ik; - auto q_map_current = getJointQ(); - q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], - q_map_current["L_ELBOW_R"], - q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], - q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], - q_map_current["R_ELBOW_R"], - q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; - - LOG(INFO) << "q_current_for_ik: " << q_current_for_ik; - - // 1. 计算末端当前位姿（通过FK） - msgs::Pose3d current_pose = fk(base_link, ee_link); - - LOG(INFO) << current_pose.mutable_position()->x() << " " << current_pose.mutable_position()->y() << " " - << current_pose.mutable_position()->z() << " " << current_pose.mutable_euler()->rx() << " " - << current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz(); - - // 2. 计算目标位姿 = 当前位姿 + 相对偏移（位置/姿态分别叠加） - msgs::Pose3d target_pose; - target_pose.mutable_position()->set_x(current_pose.position().x() + delta_pose.position().x()); - target_pose.mutable_position()->set_y(current_pose.position().y() + delta_pose.position().y()); - target_pose.mutable_position()->set_z(current_pose.position().z() + delta_pose.position().z()); - target_pose.mutable_euler()->set_rx(current_pose.euler().rx() + delta_pose.euler().rx()); - target_pose.mutable_euler()->set_ry(current_pose.euler().ry() + delta_pose.euler().ry()); - target_pose.mutable_euler()->set_rz(current_pose.euler().rz() + delta_pose.euler().rz()); - - // 3. 调用moveL执行直线运动到目标位姿 - moveL(base_link, ee_link, target_pose, vel, acc); - } catch (const std::exception &e) { - LOG(ERROR) << "moveDeltaL failed: " << e.what(); - throw std::runtime_error(std::string("moveDeltaL error: ") + e.what()); - } -} - -template -void HumanoidRobot::moveL(const std::string &base_link, const std::string &ee_link, - msgs::Pose3d target_pose, double vel, double acc) { +bool HumanoidRobot::moveL(const std::vector &pose, + double speed, + double acceleration, + double jerk, + const std::vector &qd_max, + bool asynchronous) { stopSpeedLWorker_(); - if (vel <= 0 || acc <= 0) { - throw std::runtime_error("moveL: vel and acc must be positive"); + + if (asynchronous) { + LOG(ERROR) << "moveL: asynchronous=true is not supported yet"; + return false; } + if (!ik_solver_) { + LOG(ERROR) << "moveL: ik_solver_ is not initialized"; + return false; + } + if (pose.size() != 6) { + LOG(ERROR) << "moveL: pose size must be 6 [x, y, z, rx, ry, rz], got " << pose.size(); + return false; + } + if (speed <= 0.0 || acceleration <= 0.0) { + LOG(ERROR) << "moveL: speed and acceleration must be positive"; + return false; + } + if (jerk <= 0.0) { + LOG(ERROR) << "moveL: jerk must be positive"; + return false; + } + + Eigen::Matrix4d target_pose_base = Eigen::Matrix4d::Identity(); + target_pose_base.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose[3], pose[4], pose[5]); + target_pose_base(0, 3) = pose[0]; + target_pose_base(1, 3) = pose[1]; + target_pose_base(2, 3) = pose[2]; + + const double dt_real = 0.001; + return executeMoveLScurve_(target_pose_base, + speed, + acceleration, + jerk, + qd_max, + dt_real, + true, + cmvr::CartesianFrame::Tool); +} + +template +bool HumanoidRobot::executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input, + double v_tcp_max, + double a_tcp_max, + double j_tcp_max, + const std::vector& qd_max, + double dt_real, + bool is_tcp, + cmvr::CartesianFrame input_frame) { + static const std::vector kRightArmJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", + "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" + }; + + if (!ik_solver_) { + LOG(ERROR) << "executeMoveLScurve_: ik_solver_ is not initialized"; + return false; + } + if (v_tcp_max <= 0.0 || a_tcp_max <= 0.0 || j_tcp_max <= 0.0 || dt_real <= 0.0) { + LOG(ERROR) << "executeMoveLScurve_: invalid parameters, v=" << v_tcp_max + << ", a=" << a_tcp_max + << ", j=" << j_tcp_max + << ", dt=" << dt_real; + return false; + } + + std::lock_guard lock(right_arm_mutex_); + if (is_right_arm_busy_.exchange(true)) { + LOG(ERROR) << "executeMoveLScurve_: right arm is busy"; + return false; + } + + struct BusyGuard { + std::atomic& busy_flag; + ~BusyGuard() { busy_flag.store(false); } + } busy_guard{is_right_arm_busy_}; try { - const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变 - msgs::Pose3d current_pose = fk(base_link, ee_link); - - // 1. 初始化当前和目标位姿矩阵 - Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity(); - T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( - current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz() - ); - T_current(0, 3) = current_pose.position().x(); - T_current(1, 3) = current_pose.position().y(); - T_current(2, 3) = current_pose.position().z(); - - Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); - T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( - target_pose.euler().rx(), target_pose.euler().ry(), target_pose.euler().rz() - ); - T_target(0, 3) = target_pose.position().x(); - T_target(1, 3) = target_pose.position().y(); - T_target(2, 3) = target_pose.position().z(); - - // 保存起始姿态，确保整个运动过程中姿态保持不变 - Eigen::Matrix3d start_orientation = T_current.block<3, 3>(0, 0); - - // 2. 获取当前关节配置并验证目标可达性 - Eigen::Vector q_current; - auto q_map_current = getJointQ(); - q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], - q_map_current["L_ELBOW_R"], - q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], - q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], - q_map_current["R_ELBOW_R"], - q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; - - LOG(INFO) << "Current joint configuration: " << q_current; - m_state_->SetQ(q_current); - m_robot_->ComputeForwardKinematics(m_state_); - - // 验证目标点可达性 - 使用起始姿态，确保姿态不变 - cmvr::ctrl::PoseTarget target_ik_check; - target_ik_check.T_target = T_target; - target_ik_check.T_target.block<3, 3>(0, 0) = start_orientation; // 使用起始姿态 - target_ik_check.w_posrot = 0.5; - target_ik_check.weight = 1.0; - target_ik_check.link_name = ee_link; - - Eigen::Vector q_cmd_check; - bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd_check, 10000, 1e-6); - if (!ik_solvable) { - throw std::runtime_error("moveL: Target pose is unreachable with constant orientation"); + std::vector q_start; + q_start.reserve(kRightArmJointNames.size()); + const auto q_map = getJointQ(); + for (const auto& joint_name : kRightArmJointNames) { + const auto it = q_map.find(joint_name); + if (it == q_map.end()) { + LOG(ERROR) << "executeMoveLScurve_: missing joint state for " << joint_name; + return false; + } + q_start.push_back(it->second); } - // 3. 计算位置差值（保持姿态不变） - Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3); - double total_distance = delta_pos.norm(); - - if (total_distance < 1e-6) { - LOG(INFO) << "moveL: Target is already reached"; - return; + if (!qd_max.empty() && qd_max.size() != kRightArmJointNames.size()) { + LOG(ERROR) << "executeMoveLScurve_: qd_max size mismatch, expect " + << kRightArmJointNames.size() << ", got " << qd_max.size(); + return false; } - // 4. 基于S曲线速度规划的时间规划 - // 计算总时间和插值点数 - double move_time = calculateMoveTime(total_distance, vel, acc); - size_t num_points = std::max(2ul, static_cast(ceil(move_time / CONTROL_PERIOD))); - - // 生成时间轴和距离比例 - std::vector time_points; - std::vector distance_ratios; - generateSTrapezoidalProfile(total_distance, vel, acc, move_time, num_points, - time_points, distance_ratios); - - LOG(INFO) << "moveL: Planning trajectory - points=" << num_points - << ", total distance=" << total_distance << "m, move time=" << move_time << "s"; - - // 5. 生成轨迹点（位置线性插值，姿态保持不变） - std::vector cartesian_trajectory; - for (size_t i = 0; i <= num_points; ++i) { - double s = distance_ratios[i]; // 使用S曲线规划的距离比例 - - Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity(); - T_interp.block<3, 3>(0, 0) = start_orientation; // 保持起始姿态不变 - // 仅位置按比例插值 - T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x(); - T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y(); - T_interp(2, 3) = T_current(2, 3) + s * delta_pos.z(); - - cartesian_trajectory.push_back(T_interp); + std::vector> q_traj; + std::vector> qdot_traj; + std::vector t_traj; + if (!ik_solver_->moveL_SCurveLocal(target_pose_input, + q_start, + q_traj, + t_traj, + dt_real, + v_tcp_max, + a_tcp_max, + j_tcp_max, + qd_max, + is_tcp, + input_frame, + &qdot_traj)) { + LOG(ERROR) << "executeMoveLScurve_: moveL_SCurveLocal failed"; + return false; + } + if (q_traj.empty() || t_traj.size() != q_traj.size()) { + LOG(ERROR) << "executeMoveLScurve_: invalid trajectory output"; + return false; + } + if (qdot_traj.size() != q_traj.size()) { + LOG(WARNING) << "executeMoveLScurve_: qdot_traj size mismatch, use zero fallback"; + qdot_traj.assign(q_traj.size(), std::vector(kRightArmJointNames.size(), 0.0)); + } + if (q_traj.size() == 1) { + return true; } - // 6. 预先计算所有轨迹点的关节位置 - std::vector > joint_positions; - joint_positions.push_back(q_current); // 起始位置 - - // 预先计算所有关节位置 - for (size_t i = 1; i < cartesian_trajectory.size(); ++i) { - const auto &T_interp = cartesian_trajectory[i]; - - // 构造当前目标 - cmvr::ctrl::PoseTarget current_target; - current_target.T_target = T_interp; - current_target.link_name = ee_link; - current_target.w_posrot = 0.5; - current_target.weight = 1.0; - - // 使用前一点的位置作为初始值求解IK - Eigen::Vector q_next; - bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, - q_next, 10000, 1e-6); - - if (!ok) { - throw std::runtime_error("Pre-computation IK failed"); - // LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point"; - // q_next = joint_positions.back(); + auto next_deadline = std::chrono::steady_clock::now(); + for (size_t i = 1; i < q_traj.size(); ++i) { + const double dt_segment = std::max(1e-4, t_traj[i] - t_traj[i - 1]); + std::vector joint_cmd; + joint_cmd.reserve(kRightArmJointNames.size()); + for (size_t j = 0; j < kRightArmJointNames.size(); ++j) { + const double qd_abs = (j < qdot_traj[i].size()) ? std::abs(qdot_traj[i][j]) : 0.0; + joint_cmd.emplace_back(kRightArmJointNames[j], q_traj[i][j], qd_abs); } - joint_positions.push_back(q_next); + servoJ(joint_cmd, dt_segment); + next_deadline += std::chrono::duration_cast( + std::chrono::duration(dt_segment)); + std::this_thread::sleep_until(next_deadline); } - // 7. 计算每个点的关节速度 - std::vector > joint_velocities; - joint_velocities.push_back(Eigen::Vector::Zero()); // 起始速度为零 - - for (size_t i = 1; i < joint_positions.size(); ++i) { - double dt = time_points[i] - time_points[i - 1]; - Eigen::Vector vel = (joint_positions[i] - joint_positions[i - 1]) / dt; - joint_velocities.push_back(vel); - } - - // 8. 打印轨迹信息 - std::cout << "\n===================================== 轨迹规划信息 =====================================" << - std::endl; - std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl; - std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; - std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl; - std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl; - std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl; - std::cout << "起点位置: (x=" << T_current(0, 3) << ", y=" << T_current(1, 3) << ", z=" << T_current(2, 3) << ")" << - std::endl; - std::cout << "终点位置: (x=" << T_target(0, 3) << ", y=" << T_target(1, 3) << ", z=" << T_target(2, 3) << ")" << - std::endl; - std::cout << "保持姿态不变" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << - std::endl; - - // 9. 执行轨迹 - auto loop_start_time = std::chrono::high_resolution_clock::now(); - - for (size_t i = 0; i < cartesian_trajectory.size(); ++i) { - // 获取当前点的关节位置和速度 - Eigen::Vector q_cmd = joint_positions[i]; - Eigen::Vector q_vel = joint_velocities[i]; - - // 更新状态 - m_state_->SetQ(q_cmd); - m_robot_->ComputeForwardKinematics(m_state_); - - // 发送关节命令 - 为每个电机单独设置位置和速度 - std::vector joint_command; - for (size_t j = 0; j < DOF; ++j) { - JointPoint jp; - jp.joint_name = joint_names_[j]; - jp.rad = q_cmd[j]; - jp.vel = std::abs(q_vel[j]); // 使用计算出的关节速度 - joint_command.push_back(jp); - } - - // 计算当前点应该执行的时间 - double expected_time = time_points[i]; - // servoJ(joint_command, vel, expected_time); - for (const auto &j: joint_command) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - motor->setQd(j.vel); - motor->setQ(j.rad); - } - } - - // 检查中断 - if (flash_cmd_.load()) { - flash_cmd_.store(false); - LOG(INFO) << "moveL: Interrupted by external command"; - return; - } - - // 控制时间节奏 - 使用精确的时间规划 - auto expected_time_point = loop_start_time + std::chrono::nanoseconds( - static_cast(expected_time * 1e9) - ); - auto now = std::chrono::high_resolution_clock::now(); - if (now < expected_time_point) { - std::this_thread::sleep_until(expected_time_point); - } else { - LOG(WARNING) << "moveL: Behind schedule at point " << i - << " by " << std::chrono::duration_cast(now - expected_time_point). - count() << "ms"; - } - } - - // 最终状态更新 - m_state_->SetQ(joint_positions.back()); - m_robot_->ComputeForwardKinematics(m_state_); - rsm_.store(ROBOT_READY); - LOG(INFO) << "moveL: Trajectory completed successfully"; - } catch (const std::exception &e) { - LOG(ERROR) << "moveL failed: " << e.what(); - rsm_.store(ROBOT_ERROR); - throw std::runtime_error(std::string("moveL error: ") + e.what()); - } -} - - -// 辅助函数：计算运动时间 -template -double HumanoidRobot::calculateMoveTime(double distance, double vel, double acc) { - // 计算加速和减速所需的时间和距离 - double acc_time = vel / acc; - double acc_distance = 0.5 * acc * acc_time * acc_time; - - // 如果加速距离超过总距离的一半，需要调整最大速度 - if (2 * acc_distance > distance) { - // 三角形速度曲线：加速然后直接减速 - double max_reachable_vel = std::sqrt(acc * distance); - return 2 * max_reachable_vel / acc; - } else { - // 梯形速度曲线：加速-匀速-减速 - double constant_distance = distance - 2 * acc_distance; - double constant_time = constant_distance / vel; - return 2 * acc_time + constant_time; - } -} - -// 辅助函数：生成S曲线轨迹规划 -template -void HumanoidRobot::generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc, - double total_time, size_t num_points, - std::vector &time_points, - std::vector &distance_ratios) { - time_points.clear(); - distance_ratios.clear(); - - // 计算加速和减速阶段的时间 - double acc_time = max_vel / max_acc; - double acc_distance = 0.5 * max_acc * acc_time * acc_time; - - // 确定实际的速度曲线形状 - if (2 * acc_distance > total_distance) { - // 三角形速度曲线 - double actual_max_vel = std::sqrt(max_acc * total_distance); - acc_time = actual_max_vel / max_acc; - acc_distance = 0.5 * max_acc * acc_time * acc_time; - - double dec_time = acc_time; - - // 生成时间点和距离比例 - for (size_t i = 0; i <= num_points; ++i) { - double t = static_cast(i) / num_points * total_time; - time_points.push_back(t); - - if (t <= acc_time) { - // 加速阶段 - double s = 0.5 * max_acc * t * t; - distance_ratios.push_back(s / total_distance); - } else { - // 减速阶段 - double dec_start_time = total_time - dec_time; - double dec_elapsed = t - dec_start_time; - double s = acc_distance + actual_max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed; - distance_ratios.push_back(s / total_distance); - } - } - } else { - // 梯形速度曲线 - double constant_time = (total_distance - 2 * acc_distance) / max_vel; - double dec_time = acc_time; - - // 生成时间点和距离比例 - for (size_t i = 0; i <= num_points; ++i) { - double t = static_cast(i) / num_points * total_time; - time_points.push_back(t); - - if (t <= acc_time) { - // 加速阶段 - double s = 0.5 * max_acc * t * t; - distance_ratios.push_back(s / total_distance); - } else if (t <= acc_time + constant_time) { - // 匀速阶段 - double s = acc_distance + max_vel * (t - acc_time); - distance_ratios.push_back(s / total_distance); - } else { - // 减速阶段 - double dec_start_time = acc_time + constant_time; - double dec_elapsed = t - dec_start_time; - double s = acc_distance + max_vel * constant_time + - max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed; - distance_ratios.push_back(s / total_distance); - } - } + LOG(INFO) << "executeMoveLScurve_: completed, samples=" << q_traj.size() + << ", duration_s=" << t_traj.back(); + return true; + } catch (const std::exception& e) { + LOG(ERROR) << "executeMoveLScurve_ failed: " << e.what(); + return false; } } diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index eb26e14c..a74c59e6 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -81,6 +81,23 @@ std::unordered_map makeRightArmQMap() { return q_map; } +bool readRightArmQ(const std::shared_ptr& robot, std::vector& q_now) { + if (!robot) { + return false; + } + auto q_map_now = makeRightArmQMap(); + robot->getJointQ(q_map_now); + q_now.assign(kRightArmJointNames.size(), 0.0); + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + const auto it = q_map_now.find(kRightArmJointNames[i]); + if (it == q_map_now.end()) { + return false; + } + q_now[i] = it->second; + } + return true; +} + } // namespace @@ -210,6 +227,243 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { // show(fig); } +TEST(HumanoidRobotTest, moveLSCurveLocalRealRobot) { + const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; + auto dmgr_cfg = config.getChild("DeviceManager"); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot_abs = dmgr.getDevice("hc01"); + auto robot = std::dynamic_pointer_cast>(robot_abs); + ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; + + std::vector init_cmd = { + {"R_SHOULDER_P", -0.2423}, + {"R_SHOULDER_R", 1.2929}, + {"R_SHOULDER_Y", 1.61}, + {"R_ELBOW_R", 1.58}, + {"R_WRIST_P", -2.8792}, + {"R_WRIST_Y", 0.1150}, + {"R_WRIST_R", 0.1}, + }; + robot->moveJ(init_cmd, 1.0, 2.0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + struct ReturnHomeGuard { + std::shared_ptr> robot; + std::vector home; + ~ReturnHomeGuard() { + if (!robot) { + return; + } + try { + robot->moveJ(home, 1.0, 2.0); + } catch (...) { + } + } + } guard{robot, init_cmd}; + + std::vector q_start; + ASSERT_TRUE(readRightArmQ(robot_abs, q_start)) << "Failed to read right arm joint state"; + + cmvr::PinocchioDlsIKSolver solver( + "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + "PELVIS_S", + "R_WRIST_R_S", + "R_FINGER_TIP_FIXED"); + ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed"; + + Eigen::Matrix4d target_pose_input = Eigen::Matrix4d::Identity(); + const cmvr::CartesianFrame input_frame = cmvr::CartesianFrame::Tool; + target_pose_input(1, 3) = -0.08; + + const double dt_gen = 0.001; + const double v_tcp = 0.08; + const double a_tcp = 5; + const double j_tcp = 5; + const bool is_tcp = true; + std::vector qd_max(7, 3.0); + + std::vector> q_traj; + std::vector> qdot_traj; + std::vector t_traj; + ASSERT_TRUE(solver.moveL_SCurveLocal(target_pose_input, + q_start, + q_traj, + t_traj, + dt_gen, + v_tcp, + a_tcp, + j_tcp, + qd_max, + is_tcp, + input_frame, + &qdot_traj)) + << "moveL_SCurveLocal failed"; + + ASSERT_EQ(q_traj.size(), t_traj.size()) << "q_traj and t_traj size mismatch"; + ASSERT_EQ(q_traj.size(), qdot_traj.size()) << "q_traj and qdot_traj size mismatch"; + ASSERT_FALSE(q_traj.empty()) << "Generated trajectory is empty"; + + LOG(INFO) << "moveLSCurveLocalRealRobot generated samples=" << q_traj.size() + << ", duration_s=" << (t_traj.empty() ? 0.0 : t_traj.back()) + << ", frame=" << (input_frame == cmvr::CartesianFrame::Tool ? "Tool" : "Base") + << ", delta_xyz=[" << target_pose_input(0, 3) << ", " + << target_pose_input(1, 3) << ", " + << target_pose_input(2, 3) << "]"; + + auto next_deadline = std::chrono::steady_clock::now(); + for (size_t i = 1; i < q_traj.size(); ++i) { + const double dt_segment = std::max(0.0, t_traj[i] - t_traj[i - 1]); + std::vector qd_abs = qdot_traj[i]; + for (double& v : qd_abs) { + v = std::abs(v); + } + auto joint_cmd = buildRightArmJointCmdWithVel(q_traj[i], qd_abs, 0.05); + ASSERT_NO_THROW(robot->servoJ(joint_cmd, dt_segment)); + + next_deadline += std::chrono::duration_cast( + std::chrono::duration(dt_segment)); + std::this_thread::sleep_until(next_deadline); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + std::vector q_end; + ASSERT_TRUE(readRightArmQ(robot_abs, q_end)) << "Failed to read right arm joint state at end"; + + Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_end, T_end, is_tcp)) << "Failed to compute FK for end joint state"; + + Eigen::Matrix4d T_start = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_start, T_start, is_tcp)) << "Failed to compute FK for start joint state"; + + Eigen::Matrix4d T_plan_end = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_traj.back(), T_plan_end, is_tcp)) << "Failed to compute FK for planned end joint state"; + + const Eigen::Matrix3d R_start = T_start.block<3,3>(0, 0); + const Eigen::Matrix3d R_plan_end = T_plan_end.block<3,3>(0, 0); + const Eigen::Matrix3d R_end = T_end.block<3,3>(0, 0); + + const Eigen::Vector3d delta_tcp_plan = T_plan_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); + const Eigen::Vector3d delta_tcp = T_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); + const Eigen::Vector3d delta_tcp_error = delta_tcp - delta_tcp_plan; + const double rot_err_start_to_actual_rad = + Eigen::AngleAxisd(R_start.transpose() * R_end).angle(); + const double rot_err_plan_to_actual_rad = + Eigen::AngleAxisd(R_plan_end.transpose() * R_end).angle(); + LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_plan=[" << delta_tcp_plan.x() << ", " + << delta_tcp_plan.y() << ", " << delta_tcp_plan.z() << "]"; + LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_actual=[" << delta_tcp.x() << ", " + << delta_tcp.y() << ", " << delta_tcp.z() << "]"; + LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_error=[" << delta_tcp_error.x() << ", " + << delta_tcp_error.y() << ", " << delta_tcp_error.z() << "]"; + LOG(INFO) << "moveLSCurveLocalRealRobot rot_err_start_to_actual_deg=" + << rot_err_start_to_actual_rad * 180.0 / M_PI; + LOG(INFO) << "moveLSCurveLocalRealRobot rot_err_plan_to_actual_deg=" + << rot_err_plan_to_actual_rad * 180.0 / M_PI; +} + +TEST(HumanoidRobotTest, moveLRealRobot) { + const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; + auto dmgr_cfg = config.getChild("DeviceManager"); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot_abs = dmgr.getDevice("hc01"); + auto robot = std::dynamic_pointer_cast>(robot_abs); + ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; + + std::vector init_cmd = { + {"R_SHOULDER_P", -0.2423}, + {"R_SHOULDER_R", 1.2929}, + {"R_SHOULDER_Y", 1.61}, + {"R_ELBOW_R", 1.58}, + {"R_WRIST_P", -2.8792}, + {"R_WRIST_Y", 0.1150}, + {"R_WRIST_R", 0.1}, + }; + robot->moveJ(init_cmd, 1.0, 2.0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + std::vector q_start; + ASSERT_TRUE(readRightArmQ(robot_abs, q_start)) << "Failed to read right arm joint state"; + + cmvr::PinocchioDlsIKSolver solver( + "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + "PELVIS_S", + "R_WRIST_R_S", + "R_FINGER_TIP_FIXED"); + ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed"; + + Eigen::Matrix4d target_pose_input = Eigen::Matrix4d::Identity(); + const cmvr::CartesianFrame input_frame = cmvr::CartesianFrame::Tool; + target_pose_input(1, 3) = -0.08; + + const double v_tcp = 0.08; + const double a_tcp = 5; + const double j_tcp = 5; + const bool is_tcp = true; + + const std::vector pose_cmd = { + target_pose_input(0, 3), + target_pose_input(1, 3), + target_pose_input(2, 3), + 0.0, + 0.0, + 0.0 + }; + + LOG(INFO) << "moveLRealRobot command" + << ", frame=" << (input_frame == cmvr::CartesianFrame::Tool ? "Tool" : "Base") + << ", delta_xyz=[" << target_pose_input(0, 3) << ", " + << target_pose_input(1, 3) << ", " + << target_pose_input(2, 3) << "]"; + + ASSERT_TRUE(robot->moveL(pose_cmd, v_tcp, a_tcp, j_tcp)) << "robot->moveL failed"; + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + std::vector q_end; + ASSERT_TRUE(readRightArmQ(robot_abs, q_end)) << "Failed to read right arm joint state at end"; + + Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_end, T_end, is_tcp)) << "Failed to compute FK for end joint state"; + + Eigen::Matrix4d T_start = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_start, T_start, is_tcp)) << "Failed to compute FK for start joint state"; + + Eigen::Matrix4d T_plan_end = Eigen::Matrix4d::Identity(); + T_plan_end = T_start; + if (input_frame == cmvr::CartesianFrame::Tool) { + T_plan_end.block<3,1>(0,3) = (T_start * target_pose_input).block<3,1>(0,3); + } else { + T_plan_end.block<3,1>(0,3) = target_pose_input.block<3,1>(0,3); + } + + const Eigen::Matrix3d R_start = T_start.block<3,3>(0, 0); + const Eigen::Matrix3d R_plan_end = T_plan_end.block<3,3>(0, 0); + const Eigen::Matrix3d R_end = T_end.block<3,3>(0, 0); + + const Eigen::Vector3d delta_tcp_plan = T_plan_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); + const Eigen::Vector3d delta_tcp = T_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); + const Eigen::Vector3d delta_tcp_error = delta_tcp - delta_tcp_plan; + const double rot_err_start_to_actual_rad = + Eigen::AngleAxisd(R_start.transpose() * R_end).angle(); + const double rot_err_plan_to_actual_rad = + Eigen::AngleAxisd(R_plan_end.transpose() * R_end).angle(); + LOG(INFO) << "moveLRealRobot tcp_delta_plan=[" << delta_tcp_plan.x() << ", " + << delta_tcp_plan.y() << ", " << delta_tcp_plan.z() << "]"; + LOG(INFO) << "moveLRealRobot tcp_delta_actual=[" << delta_tcp.x() << ", " + << delta_tcp.y() << ", " << delta_tcp.z() << "]"; + LOG(INFO) << "moveLRealRobot tcp_delta_error=[" << delta_tcp_error.x() << ", " + << delta_tcp_error.y() << ", " << delta_tcp_error.z() << "]"; + LOG(INFO) << "moveLRealRobot rot_err_start_to_actual_deg=" + << rot_err_start_to_actual_rad * 180.0 / M_PI; + LOG(INFO) << "moveLRealRobot rot_err_plan_to_actual_deg=" + << rot_err_plan_to_actual_rad * 180.0 / M_PI; +} + TEST(HumanoidRobotTest,IBVSWithRealRobot) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); diff --git a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h index 8b75c623..145ca0b5 100644 --- a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h +++ b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h @@ -37,7 +37,7 @@ public: double angular_acceleration_max{3.0}; double angular_jerk_max{15.0}; - std::vector joint_velocity_max; ///< size=chain_v_dof_，为空则仅用 URDF limit + std::vector joint_velocity_max = std::vector(7, 2.5); ///< size=chain_v_dof_，为空则仅用 URDF limit std::vector joint_acceleration_max; ///< size=chain_v_dof_，为空则不做 joint accel 限制 bool enable_joint_soft_limit_velocity{true}; double joint_soft_limit_margin{0.05}; @@ -72,7 +72,7 @@ public: const Eigen::Matrix4d& target_pose, std::vector& joints_angle); - bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base, + bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_input, const std::vector& q_start, std::vector>& q_traj, std::vector& t_traj, @@ -80,8 +80,10 @@ public: double v_tcp_max, double a_tcp_max, double j_tcp_max, - const std::vector& qd_max, - bool is_tcp = true); + const std::vector& qd_max, // empty => use URDF joint velocity limits only + bool is_tcp = true, + CartesianFrame input_frame = CartesianFrame::Base, + std::vector>* qdot_traj = nullptr); bool ik(const std::string& base_link, const std::string& ee_link, diff --git a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp index 191668be..60864c33 100644 --- a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp +++ b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp @@ -441,7 +441,7 @@ bool PinocchioDlsIKSolver::ik(const std::string& base_link, // ============================================================================ // MoveL with tau time-scaling // ============================================================================ -bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base, +bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_input, const std::vector& q_start, std::vector>& q_traj, std::vector& t_traj, @@ -450,11 +450,13 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_ double a_tcp_max, double j_tcp_max, const std::vector& qd_max, - bool is_tcp) + bool is_tcp, + CartesianFrame input_frame, + std::vector>* qdot_traj) { if (!initialized_) return false; if ((int)q_start.size() != chain_dof_) return false; - if ((int)qd_max.size() != chain_dof_) return false; + if (!qd_max.empty() && (int)qd_max.size() != chain_dof_) return false; dt_real = std::max(dt_real, 1e-4); @@ -467,13 +469,28 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_ const Eigen::Vector3d p0 = T0_base.block<3,1>(0,3); const Eigen::Matrix3d R_fixed = T0_base.block<3,3>(0,0); + // moveL_SCurveLocal 当前只对目标平移做直线规划，姿态仍保持起始末端姿态。 + Eigen::Matrix4d target_pose_base = target_pose_input; + if (input_frame == CartesianFrame::Tool) { + target_pose_base = T0_base * target_pose_input; + } + const Eigen::Vector3d pg = target_pose_base.block<3,1>(0,3); const Eigen::Vector3d dp = pg - p0; const double L = dp.norm(); - if (L < 1e-9) { q_traj = {q_start}; t_traj = {0.0}; return true; } + if (L < 1e-9) { + q_traj = {q_start}; + t_traj = {0.0}; + if (qdot_traj) { + qdot_traj->assign(1, std::vector(static_cast(chain_dof_), 0.0)); + } + return true; + } const Eigen::Vector3d dir_base = dp / L; - std::cerr << "dp(base)= " << dp.transpose() << "\n"; + std::cerr << "dp(base)= " << dp.transpose() + << " frame=" << (input_frame == CartesianFrame::Tool ? "tool" : "base") + << "\n"; SCurve curve(v_tcp_max, a_tcp_max, j_tcp_max); const SCurveProfile prof = curve.calculateProfile(0.0, L); @@ -488,7 +505,22 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_ const double eps_dq = 1e-9; Eigen::VectorXd qd_lim(chain_dof_); - for (int i = 0; i < chain_dof_; ++i) qd_lim[i] = std::max(1e-6, std::abs(qd_max[i])); + if (qd_max.empty()) { + for (int i = 0; i < chain_dof_; ++i) { + double limit_i = std::numeric_limits::infinity(); + if (joint_vel_limits_.size() == chain_dof_) { + const double urdf_limit = std::abs(joint_vel_limits_[i]); + if (std::isfinite(urdf_limit) && urdf_limit > 0.0) { + limit_i = urdf_limit; + } + } + qd_lim[i] = limit_i; + } + } else { + for (int i = 0; i < chain_dof_; ++i) { + qd_lim[i] = std::max(1e-6, std::abs(qd_max[i])); + } + } Eigen::VectorXd q_ref = Eigen::Map(q_start.data(), chain_dof_); Eigen::VectorXd q_chain = Eigen::Map(q_start.data(), chain_dof_); @@ -502,11 +534,11 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_ t_traj.reserve((size_t)(prof.total_time / dt_real) + 10); q_traj.push_back(q_start); t_traj.push_back(0.0); - - const double log_period = 0.05; - double next_log_t = 0.0; - const double sigma_min_th = 1e-4; - const double cond_th = 1e4; + if (qdot_traj) { + qdot_traj->clear(); + qdot_traj->reserve((size_t)(prof.total_time / dt_real) + 10); + qdot_traj->push_back(std::vector(static_cast(chain_dof_), 0.0)); + } double gamma_prev = 1.0; const double gamma_tau = 0.03; @@ -622,55 +654,35 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_ return false; } - q_chain += (gamma_tot * dq_raw) * step_real; - q_chain = q_chain.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_); + const Eigen::VectorXd qdot_cmd = gamma_tot * dq_raw; + + const Eigen::VectorXd q_before_clamp = q_chain + qdot_cmd * step_real; + const Eigen::VectorXd q_after_clamp = + q_before_clamp.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_); + for (int i = 0; i < chain_dof_; ++i) { + if (std::abs(q_after_clamp[i] - q_before_clamp[i]) > 1e-12) { + std::cerr << "[moveL_SCurveLocal] clamp triggered: step=" << q_traj.size() + << ", joint_index=" << i + << ", before=" << q_before_clamp[i] + << ", after=" << q_after_clamp[i] + << std::endl; + } + } + q_chain = q_after_clamp; tau = std::min(prof.total_time, tau + gamma_tot * step_real); t += step_real; - if (t >= next_log_t) { - next_log_t += log_period; - - bool near_limit = false; - int near_cnt = 0; - for (int i = 0; i < chain_dof_; ++i) { - const bool nl = (joint_pos_upper_limits_[i] - q_chain[i] < 1e-8) || - (q_chain[i] - joint_pos_lower_limits_[i] < 1e-8); - if (nl) { near_limit = true; near_cnt++; } - } - - std::cerr - << "[MoveL ABC] t=" << t - << " tau=" << tau << "/" << prof.total_time - << " s=" << s << "/" << L - << " sd=" << sd - << " gamma_speed=" << gamma_speed - << " gamma_lim=" << gamma_lim - << " gamma_tot=" << gamma_tot - << " near_limit=" << (near_limit?1:0) - << " near_cnt=" << near_cnt - << " |e_perp|=" << e_perp.norm() - << " sigma_min=" << sigma_min - << " cond=" << cond - << " w_end=" << w_end - << "\n"; - - if (sigma_min < sigma_min_th || cond > cond_th) { - std::cerr << " [C] near singular! sigma_min=" << sigma_min - << " cond=" << cond << "\n"; - } - if (gamma_lim < 0.999) { - std::cerr << " [A2] limit scaling gamma_lim=" << gamma_lim << "\n"; - } - if (gamma_speed < 0.999) { - std::cerr << " [B] speed scaling gamma_speed=" << gamma_speed << "\n"; - } - } - std::vector q_out(chain_dof_); Eigen::Map(q_out.data(), chain_dof_) = q_chain; q_traj.push_back(std::move(q_out)); t_traj.push_back(t); + + if (qdot_traj) { + std::vector qdot_out(static_cast(chain_dof_)); + Eigen::Map(qdot_out.data(), chain_dof_) = qdot_cmd; + qdot_traj->push_back(std::move(qdot_out)); + } } return !q_traj.empty(); diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index c7f07fb2..18779b1a 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -89,7 +89,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - // LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; + LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; cv::Mat image; const auto dev = dmgr_.getDevice(dev_id); Rs2Intrinsics intrinsics = {0}; diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index 699831ee..37b053fa 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -3,6 +3,16 @@ // #include "../include/grpc_dexhand_service.h" + +#include +#include +#include +#include +#include +#include + +#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" + using namespace std; using namespace cmvr::service; using namespace cmvr::device; @@ -12,6 +22,147 @@ using namespace cmvr::device; #define DEXHAND_MAX_FORCE 3000 #define DEXHAND_MAX_SPEED 1000 +namespace { + +constexpr int kDexHandDofCount = 6; + +cmvr::api::SensorData::FingerType toProtoFingerType(const AbstractDexHand::FingerType finger) { + switch (finger) { + case AbstractDexHand::FingerType::PINKY: + return cmvr::api::SensorData::PINKY; + case AbstractDexHand::FingerType::RING: + return cmvr::api::SensorData::RING; + case AbstractDexHand::FingerType::MIDDLE: + return cmvr::api::SensorData::MIDDLE_FINGER; + case AbstractDexHand::FingerType::INDEX: + return cmvr::api::SensorData::INDEX; + case AbstractDexHand::FingerType::THUMB: + return cmvr::api::SensorData::THUMB; + case AbstractDexHand::FingerType::PALM: + return cmvr::api::SensorData::PALM; + } + return cmvr::api::SensorData::PINKY; +} + +cmvr::api::SensorData::PartType toProtoPartType(const AbstractDexHand::TactileRegion region) { + switch (region) { + case AbstractDexHand::TactileRegion::TIP: + return cmvr::api::SensorData::TIP; + case AbstractDexHand::TactileRegion::FINGER: + return cmvr::api::SensorData::FINGER; + case AbstractDexHand::TactileRegion::PAD: + return cmvr::api::SensorData::PAD; + case AbstractDexHand::TactileRegion::THUMB_MIDDLE: + return cmvr::api::SensorData::THUMB_MIDDLE; + case AbstractDexHand::TactileRegion::PALM_PAD: + return cmvr::api::SensorData::PALM_PAD; + } + return cmvr::api::SensorData::TIP; +} + +void fillSensorData(const AbstractDexHand::TactileRegionData& tactile_data, + cmvr::api::SensorData* sensor_data) { + sensor_data->set_rows(tactile_data.view.rows); + sensor_data->set_cols(tactile_data.view.cols); + sensor_data->set_finger_type(toProtoFingerType(tactile_data.finger)); + sensor_data->set_part_type(toProtoPartType(tactile_data.region)); + sensor_data->set_sensor_name(tactile_data.name == nullptr ? "" : tactile_data.name); + + for (int row = 0; row < tactile_data.view.rows; ++row) { + 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(values[col])); + } + } +} + +template +void appendSensorData(const std::vector& tactile_regions, + ResponseT* response) { + for (const auto& tactile_region : tactile_regions) { + if (!tactile_region.valid()) { + continue; + } + fillSensorData(tactile_region, response->add_sensor()); + } +} + +template +void applyFreedomValues(const FreedomCollection& freedoms, + const int scale, + std::vector& targets) { + for (const auto& freedom : freedoms) { + if (freedom.id() < 0 || freedom.id() >= static_cast(targets.size())) { + throw std::invalid_argument("Invalid dexhand DOF id: " + std::to_string(freedom.id())); + } + if (!std::isfinite(freedom.value())) { + throw std::invalid_argument("Invalid dexhand command value: not finite."); + } + targets[static_cast(freedom.id())] = static_cast(freedom.value() * scale); + } +} + +std::vector readCurrentAngles(const std::shared_ptr& dev) { + DexHandState state{}; + dev->getState(state); + + std::vector current_angles(static_cast(kDexHandDofCount), 0); + for (int i = 0; i < kDexHandDofCount; ++i) { + current_angles[static_cast(i)] = state.hands[i].angle; + } + return current_angles; +} + +bool respondUnsupportedForRh56(const std::shared_ptr& dev, + const char* rpc_name, + const char* hint, + cmvr::api::CommandHeader_Feedback* header) { + if (std::dynamic_pointer_cast(dev) == nullptr) { + return false; + } + + header->set_success(false); + header->set_error_message(std::string(rpc_name) + " is not supported by RH56DFTPDexhand. " + hint); + setCurrentTimestamp(header->mutable_timestamp()); + return true; +} + +std::vector buildRh56AllTactileRegions() { + using FingerType = AbstractDexHand::FingerType; + using TactileRegion = AbstractDexHand::TactileRegion; + + return { + {FingerType::PINKY, TactileRegion::TIP}, + {FingerType::PINKY, TactileRegion::FINGER}, + {FingerType::PINKY, TactileRegion::PAD}, + {FingerType::RING, TactileRegion::TIP}, + {FingerType::RING, TactileRegion::FINGER}, + {FingerType::RING, TactileRegion::PAD}, + {FingerType::MIDDLE, TactileRegion::TIP}, + {FingerType::MIDDLE, TactileRegion::FINGER}, + {FingerType::MIDDLE, TactileRegion::PAD}, + {FingerType::INDEX, TactileRegion::TIP}, + {FingerType::INDEX, TactileRegion::FINGER}, + {FingerType::INDEX, TactileRegion::PAD}, + {FingerType::THUMB, TactileRegion::TIP}, + {FingerType::THUMB, TactileRegion::FINGER}, + {FingerType::THUMB, TactileRegion::THUMB_MIDDLE}, + {FingerType::THUMB, TactileRegion::PAD}, + {FingerType::PALM, TactileRegion::PALM_PAD} + }; +} + +void maybeConfigureRh56FullTactilePolling(const std::shared_ptr& dev) { + auto rh56 = std::dynamic_pointer_cast(dev); + if (!rh56) { + return; + } + rh56->setTactilePollingRegions(buildRh56AllTactileRegions()); +} + +} // namespace + gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, @@ -20,10 +171,10 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - DexHandState state; + DexHandState state{}; dev->getState(state); response->mutable_state()->set_is_initialized(state.is_initialized); - for (int i = 0; i < 6; i++) { + for (int i = 0; i < kDexHandDofCount; i++) { auto hand = response->mutable_state()->add_hands(); hand->set_dof_id(i); hand->set_angle(state.hands[i].angle); @@ -33,7 +184,7 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, hand->set_speed(state.hands[i].speed); hand->set_temperature(state.hands[i].temperature); hand->set_error(state.hands[i].error); - for (auto& errormessage :state.hands[i].error_message) { + for (auto& errormessage : state.hands[i].error_message) { hand->add_error_message(errormessage); } } @@ -48,21 +199,23 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, return grpc::Status::OK; } } + grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context , const cmvr::api::SetDexHandPositionsCommand_Request* request - , cmvr::api::SetDexHandPositionsCommand_Feedback* response){ + , cmvr::api::SetDexHandPositionsCommand_Feedback* response) { try { string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - auto freedoms = request->values(); - - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); - for (auto& freedom : freedoms) { - //遍历拿到需要配置的自由度,传入的是百分比0-1 - finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_POSITION; + if (respondUnsupportedForRh56(dev, + "SetDexHandPos", + "Use SetDexHandAngle for RH56 joint commands.", + response->mutable_header())) { + return grpc::Status::OK; } + + std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); + applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets); dev->setPositions(finger_joint_targets); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -83,21 +236,17 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - auto freedoms = request->values(); - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); - for (auto& freedom : freedoms) { - //遍历拿到需要配置的自由度,传入的是百分比0-1 - finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_ANGLE; + if (const auto rh56 = std::dynamic_pointer_cast(dev)) { + std::vector finger_joint_targets = readCurrentAngles(dev); + applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets); + rh56->setAngles(finger_joint_targets); + } else { + std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); + applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets); + dev->setAngles(finger_joint_targets); } - // std::cout << "steAngle: "; - // for (auto& angle : finger_joint_targets) - // { - // std::cout << " " << angle; - // } - // std::cout << std::endl; - dev->setAngles(finger_joint_targets); + response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -117,14 +266,15 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - auto freedoms = request->values(); - - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); - for (auto& freedom : freedoms) { - //遍历拿到需要配置的自由度,传入的是百分比0-1 - finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_FORCE; + if (respondUnsupportedForRh56(dev, + "SetDexHandForce", + "RH56DFTPDexhand currently exposes angle and tactile APIs only.", + response->mutable_header())) { + return grpc::Status::OK; } + + std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); + applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets); dev->setForce(finger_joint_targets); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -145,14 +295,15 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - auto freedoms = request->values(); - - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); - for (auto& freedom : freedoms) { - //遍历拿到需要配置的自由度,传入的是百分比0-1 - finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_SPEED; + if (respondUnsupportedForRh56(dev, + "SetDexHandSpeed", + "RH56DFTPDexhand currently exposes angle and tactile APIs only.", + response->mutable_header())) { + return grpc::Status::OK; } + + std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); + applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets); dev->setVelocities(finger_joint_targets); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -173,6 +324,13 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (respondUnsupportedForRh56(dev, + "SetDexHandPresetAct", + "RH56DFTPDexhand currently exposes angle and tactile APIs only.", + response->mutable_header())) { + return grpc::Status::OK; + } + auto presetActId = request->presetactid(); dev->setPresetAct(presetActId); response->mutable_header()->set_success(true); @@ -195,60 +353,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context string dev_id = request->header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - //从串口获取传感器数据并返回 - auto sensors = dev->getSensorData(); - // 辅助函数：将FingerTactileData或PalmTactileData转换为Proto的SensorData - auto fillSensorData = [](const auto& tactileData, - cmvr::api::SensorData::FingerType fingerType, - cmvr::api::SensorData::PartType partType, - cmvr::api::SensorData* sensorData) { - // 设置行列数 - sensorData->set_rows(tactileData.rows); - sensorData->set_cols(tactileData.cols); - // 设置传感器类型 - sensorData->set_finger_type(fingerType); - sensorData->set_part_type(partType); - sensorData->set_sensor_name(tactileData.name); // 使用原结构体中的name字段 - - // 填充数据 - for (const auto& row : tactileData.data) { - cmvr::api::SensorData_RowData* rowData = sensorData->add_data(); - for (TactilePoint value : row) { - rowData->add_values(static_cast(value)); - } - } - }; - - // 1. 填充小拇指数据 - fillSensorData(sensors.pinky.tip, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::TIP, response->add_sensor()); - fillSensorData(sensors.pinky.finger, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::FINGER, response->add_sensor()); - fillSensorData(sensors.pinky.pad, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::PAD, response->add_sensor()); - - // 2. 填充无名指数据 - fillSensorData(sensors.ring.tip, cmvr::api::SensorData::RING, cmvr::api::SensorData::TIP, response->add_sensor()); - fillSensorData(sensors.ring.finger, cmvr::api::SensorData::RING, cmvr::api::SensorData::FINGER, response->add_sensor()); - fillSensorData(sensors.ring.pad, cmvr::api::SensorData::RING, cmvr::api::SensorData::PAD, response->add_sensor()); - - // 3. 填充中指数据（修改为MIDDLE_FINGER） - fillSensorData(sensors.middle.tip, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::TIP, response->add_sensor()); - fillSensorData(sensors.middle.finger, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::FINGER, response->add_sensor()); - fillSensorData(sensors.middle.pad, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::PAD, response->add_sensor()); - - // 4. 填充食指数据 - fillSensorData(sensors.index.tip, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::TIP, response->add_sensor()); - fillSensorData(sensors.index.finger, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::FINGER, response->add_sensor()); - fillSensorData(sensors.index.pad, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::PAD, response->add_sensor()); - - // 5. 填充大拇指数据（修改为THUMB_MIDDLE） - fillSensorData(sensors.thumb.tip, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::TIP, response->add_sensor()); - fillSensorData(sensors.thumb.finger, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::FINGER, response->add_sensor()); - fillSensorData(sensors.thumb.middle, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::THUMB_MIDDLE, response->add_sensor()); - fillSensorData(sensors.thumb.pad, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::PAD, response->add_sensor()); - - // 6. 填充掌心数据（使用PALM_PAD） - fillSensorData(sensors.palm, cmvr::api::SensorData::PALM, cmvr::api::SensorData::PALM_PAD, response->add_sensor()); - - + maybeConfigureRh56FullTactilePolling(dev); + appendSensorData(dev->getSensorData(), response); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -266,65 +372,21 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co { try { api::GetSensorDataStreamCommand_Request request; - stream->Read(&request); + if (!stream->Read(&request)) { + return grpc::Status::OK; + } + string dev_id = request.header().device_id(); LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - while (true) + maybeConfigureRh56FullTactilePolling(dev); + + while (!context->IsCancelled()) { api::GetSensorDataStreamCommand_Feedback response; - //从串口获取传感器数据并返回 - auto sensors = dev->getSensorData(); - // 辅助函数：将FingerTactileData或PalmTactileData转换为Proto的SensorData - auto fillSensorData = [](const auto& tactileData, - cmvr::api::SensorData::FingerType fingerType, - cmvr::api::SensorData::PartType partType, - cmvr::api::SensorData* sensorData) { - // 设置行列数 - sensorData->set_rows(tactileData.rows); - sensorData->set_cols(tactileData.cols); - // 设置传感器类型 - sensorData->set_finger_type(fingerType); - sensorData->set_part_type(partType); - sensorData->set_sensor_name(tactileData.name); // 使用原结构体中的name字段 - - // 填充数据 - for (const auto& row : tactileData.data) { - cmvr::api::SensorData_RowData* rowData = sensorData->add_data(); - for (TactilePoint value : row) { - rowData->add_values(static_cast(value)); - } - } - }; - - // 1. 填充小拇指数据 - fillSensorData(sensors.pinky.tip, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::TIP, response.add_sensor()); - fillSensorData(sensors.pinky.finger, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::FINGER, response.add_sensor()); - fillSensorData(sensors.pinky.pad, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::PAD, response.add_sensor()); - - // 2. 填充无名指数据 - fillSensorData(sensors.ring.tip, cmvr::api::SensorData::RING, cmvr::api::SensorData::TIP, response.add_sensor()); - fillSensorData(sensors.ring.finger, cmvr::api::SensorData::RING, cmvr::api::SensorData::FINGER, response.add_sensor()); - fillSensorData(sensors.ring.pad, cmvr::api::SensorData::RING, cmvr::api::SensorData::PAD, response.add_sensor()); - - // 3. 填充中指数据（修改为MIDDLE_FINGER） - fillSensorData(sensors.middle.tip, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::TIP, response.add_sensor()); - fillSensorData(sensors.middle.finger, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::FINGER, response.add_sensor()); - fillSensorData(sensors.middle.pad, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::PAD, response.add_sensor()); - - // 4. 填充食指数据 - fillSensorData(sensors.index.tip, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::TIP, response.add_sensor()); - fillSensorData(sensors.index.finger, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::FINGER, response.add_sensor()); - fillSensorData(sensors.index.pad, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::PAD, response.add_sensor()); - - // 5. 填充大拇指数据（修改为THUMB_MIDDLE） - fillSensorData(sensors.thumb.tip, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::TIP, response.add_sensor()); - fillSensorData(sensors.thumb.finger, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::FINGER, response.add_sensor()); - fillSensorData(sensors.thumb.middle, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::THUMB_MIDDLE, response.add_sensor()); - fillSensorData(sensors.thumb.pad, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::PAD, response.add_sensor()); - - // 6. 填充掌心数据（使用PALM_PAD） - fillSensorData(sensors.palm, cmvr::api::SensorData::PALM, cmvr::api::SensorData::PALM_PAD, response.add_sensor()); + appendSensorData(dev->getSensorData(), &response); + response.mutable_header()->set_success(true); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); if (!stream->Write(response)) { LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id; @@ -336,8 +398,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co return grpc::Status::OK; } catch (const std::exception& e) { + LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what(); return grpc::Status::OK; } } - diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index bafd84d9..c60d7a20 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -60,12 +60,13 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: std::lock_guard lock(touch_mutex_); try { + const double forward_l = request->max_force(); LOG(INFO) << "[gRPCHlcServiceImpl] touch: request.device_id=" << request->header().device_id() << " (ignored, using TouchScreenApp config)" << ", u=" << request->u() << ", v=" << request->v() - << ", max_force ignored"; + << ", forward_l(max_force)=" << forward_l; std::string init_error; if (!ensureTouchAppInitialized(init_error)) { @@ -73,6 +74,16 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: return grpc::Status(grpc::StatusCode::INTERNAL, init_error); } + if (forward_l != -1.0) { + if (forward_l < 0.0) { + throw std::runtime_error("invalid forward_l(max_force): must be -1 or >= 0"); + } + if (!touch_app_.setTouchSpeedlForwardL(forward_l)) { + throw std::runtime_error( + buildTouchFailureMessage(touch_app_, "TouchScreenApp set speedl forward L failed")); + } + } + if (!touch_app_.startFromPixel(request->u(), request->v())) { throw std::runtime_error( buildTouchFailureMessage(touch_app_, "TouchScreenApp startFromPixel failed")); diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index 15aa037e..487fe705 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp @@ -102,7 +102,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::moveL(grpc::ServerContext* context, c pose.mutable_euler()->set_ry(request->targetpose().ry()); pose.mutable_euler()->set_rz(request->targetpose().rz()); - robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc()); + // robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc()); response->mutable_header()->set_success(true); response->mutable_header()->set_error_message(""); }catch (const std::exception& e) { diff --git a/compile_commands.json b/compile_commands.json index 747587a1..585d1203 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -503,6 +503,12 @@ "file": "/home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp", "output": "cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeFiles/rh56dftp_dexhand.dir/src/rh56dftp_dexhand.cpp.o" }, +{ + "directory": "/home/lgv/cmvr/cmvr-es/cmake-build-debug", + "command": "/usr/bin/g++-11 -DPROTOBUF_USE_DLLS -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/coal/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/console_bridge/v1.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/eigenpy/v3.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/Eigen3/v3.4.0 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/example-robot-data/v4.3.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/fcl/v0.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x264/v165/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x265/v215/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg-jetson/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg/v4.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gflags/v2.2.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/glog/v0.5.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gtest/v1.17.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jrl-cmakemodules/v1.1.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jsoncpp/v1.9.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/libccd/v2.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/lodepng/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mujoco/v3.3.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp-eigen/v0.10.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/PCAN-Basic/v4.10.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/peak-linux-driver/v8.20.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pinocchio/v3.6.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pybind11/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/realsense/v2.56.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/tinyxml2/v11.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/toppra/v0.6.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom/v5.0.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom_headers/v2.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include/opencv4 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/modbus/3.1.11/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/visp/3.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mainif/0.0.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/matplotplusplus/1.2.0/include -I/home/lgv/cmvr/cmvr-es/cmake-build-debug/_protobuf -I/home/lgv/cmvr/cmvr-es/include -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices -I/home/lgv/cmvr/cmvr-es/cmvr-es/utils -I/home/lgv/cmvr/cmvr-es/cmvr-es -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include/osqp -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand -I/home/lgv/cmvr/cmvr-es/cmvr-es/common -I/home/lgv/cmvr/cmvr-es/protos -isystem /home/lgv/cmvr/cmvr-es/dependency/x86/third_party/grpc/v1.76.0/include -g -std=gnu++17 -fPIE -fdiagnostics-color=always -o cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeFiles/rh56dftp_dexhand_test.dir/src/rh56dftp_dexhand_test.cpp.o -c /home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp", + "file": "/home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp", + "output": "cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeFiles/rh56dftp_dexhand_test.dir/src/rh56dftp_dexhand_test.cpp.o" +}, { "directory": "/home/lgv/cmvr/cmvr-es/cmake-build-debug", "command": "/usr/bin/g++-11 -Dhead_esp32_EXPORTS -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/coal/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/console_bridge/v1.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/eigenpy/v3.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/Eigen3/v3.4.0 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/example-robot-data/v4.3.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/fcl/v0.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x264/v165/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x265/v215/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg-jetson/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg/v4.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gflags/v2.2.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/glog/v0.5.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/grpc/v1.76.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gtest/v1.17.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jrl-cmakemodules/v1.1.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jsoncpp/v1.9.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/libccd/v2.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/lodepng/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mujoco/v3.3.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp-eigen/v0.10.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/PCAN-Basic/v4.10.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/peak-linux-driver/v8.20.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pinocchio/v3.6.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pybind11/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/realsense/v2.56.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/tinyxml2/v11.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/toppra/v0.6.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom/v5.0.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom_headers/v2.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include/opencv4 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/modbus/3.1.11/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/visp/3.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mainif/0.0.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/matplotplusplus/1.2.0/include -I/home/lgv/cmvr/cmvr-es/cmake-build-debug/_protobuf -I/home/lgv/cmvr/cmvr-es/include -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices -I/home/lgv/cmvr/cmvr-es/cmvr-es/utils -I/home/lgv/cmvr/cmvr-es/cmvr-es -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include/osqp -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices/biohead/biohead_esp32 -g -std=gnu++17 -fPIC -fdiagnostics-color=always -o cmvr-es/devices/biohead/biohead_esp32/CMakeFiles/head_esp32.dir/src/biohead_esp32.cpp.o -c /home/lgv/cmvr/cmvr-es/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp", diff --git a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto index 8b8bf178..9557a861 100644 --- a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto +++ b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto @@ -107,8 +107,14 @@ message TouchScreenAppConfig { optional bool pause_after_align_reached = 28; TouchScreenTwist6 touch_twist_base = 29; - optional double touch_acceleration = 30; - optional double touch_forward_duration_s = 31; + optional double touch_speedl_acceleration = 30; + optional double touch_speedl_forward_l = 31; + optional bool touch_use_speedl = 48; + optional double touch_forward_l = 49; + optional double touch_movel_speed = 50; + optional double touch_movel_acceleration = 51; + optional double touch_movel_jerk = 52; + repeated double touch_movel_qd_max = 53; optional double dwell_time_s = 32; TouchScreenTwist6 retract_twist_base = 33;