From 9faa2a38522b9d32066beef716ba2ddda280b5f7 Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 27 Mar 2026 15:19:30 +0800 Subject: [PATCH] feat:add touch align_mode --- .../applications/include/touch_screen_app.h | 15 ++- cmvr-es/applications/src/touch_screen_app.cpp | 92 +++++++++++++------ cmvr-es/common/config/cabin_robot.xml | 36 ++++---- .../touch_screen_app_config.pb.txt | 18 ++-- .../touch_screen_app_config.proto | 8 +- 5 files changed, 109 insertions(+), 60 deletions(-) diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index 586065ba..6b23e7d1 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -65,6 +65,12 @@ public: THUMB_MIDDLE }; + enum class AlignMode { + POSE_AND_POSITION = 0, // 使用配置里的固定 rx/ry/rz 与位置一起对齐。 + RX_RY_AND_POSITION, // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。 + POSITION_ONLY // 保留锁定时看到的完整 tag 姿态，只按位置对齐。 + }; + struct Options { // 是否在触控流程开始前先回到指定初始关节位姿。 bool move_to_init_position_before_start{false}; @@ -106,9 +112,8 @@ public: double target_ry{0.0}; // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 double target_rz{0.0}; - // 为 true 时，在开始锁定目标时记录当前 tag 相对目标法向姿态的平面内旋转角， - // 后续继续使用配置里的 target_rx/target_ry，仅把 target_rz 替换为这次任务锁定到的 yaw。 - bool lock_tag_yaw_on_target_start{false}; + // 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。 + AlignMode align_mode{AlignMode::POSE_AND_POSITION}; // IBVS 参数。 // 视觉伺服增益 lambda。 @@ -302,8 +307,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 locked_target_yaw_valid_{false}; - double locked_target_yaw_rad_{0.0}; + bool locked_target_rotation_valid_{false}; + Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()}; bool touch_start_position_valid_{false}; Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index 9bfa5ce6..d8a4155e 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -214,6 +214,18 @@ TouchScreenApp::TactileRegion toTactileRegion( return TouchScreenApp::TactileRegion::TIP; } +TouchScreenApp::AlignMode toAlignMode(const cmvr::config::TouchScreenAlignMode mode) { + switch (mode) { + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: + return TouchScreenApp::AlignMode::POSE_AND_POSITION; + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: + return TouchScreenApp::AlignMode::RX_RY_AND_POSITION; + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY: + return TouchScreenApp::AlignMode::POSITION_ONLY; + } + return TouchScreenApp::AlignMode::POSE_AND_POSITION; +} + void applyVec3FromConfig(const cmvr::config::TouchScreenVec3& src, Eigen::Vector3d& dst) { if (src.has_x()) { @@ -529,8 +541,8 @@ bool TouchScreenApp::startFromPixel(int u, int v) { last_touch_pressure_peak_ = 0.0; last_active_tag_id_ = -1; last_align_error_camera_.setZero(); - locked_target_yaw_valid_ = false; - locked_target_yaw_rad_ = 0.0; + locked_target_rotation_valid_ = false; + locked_target_rotation_.setIdentity(); touch_start_position_valid_ = false; touch_start_position_base_.setZero(); phase_ = Phase::ALIGNING; @@ -610,8 +622,8 @@ void TouchScreenApp::stop() { last_touch_pressure_sum_ = 0.0; last_touch_pressure_peak_ = 0.0; last_align_error_camera_.setZero(); - locked_target_yaw_valid_ = false; - locked_target_yaw_rad_ = 0.0; + locked_target_rotation_valid_ = false; + locked_target_rotation_.setIdentity(); touch_start_position_valid_ = false; touch_start_position_base_.setZero(); last_status_ = Status::STOPPED; @@ -718,8 +730,8 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& if (config.has_target_rz()) { options.target_rz = config.target_rz(); } - if (config.has_lock_tag_yaw_on_target_start()) { - options.lock_tag_yaw_on_target_start = config.lock_tag_yaw_on_target_start(); + if (config.has_align_mode()) { + options.align_mode = toAlignMode(config.align_mode()); } if (config.has_ibvs_lambda()) { @@ -1022,27 +1034,40 @@ bool TouchScreenApp::stepAligning() { return true; } - Eigen::Vector3d target_rotvec(options_.target_rx, options_.target_ry, options_.target_rz); Eigen::Matrix3d R_target = rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz); - if (options_.lock_tag_yaw_on_target_start) { - if (!locked_target_yaw_valid_) { - const Eigen::Matrix3d R_delta = R_target.transpose() * current_tag->T_c_t.block<3, 3>(0, 0); - locked_target_yaw_rad_ = std::atan2(R_delta(1, 0), R_delta(0, 0)); - if (!std::isfinite(locked_target_yaw_rad_)) { - enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); - return false; + const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); + switch (options_.align_mode) { + case AlignMode::POSE_AND_POSITION: + break; + case AlignMode::RX_RY_AND_POSITION: + if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) { + const Eigen::Matrix3d R_delta = R_target.transpose() * R_current; + const double locked_yaw_rad = std::atan2(R_delta(1, 0), R_delta(0, 0)); + if (!std::isfinite(locked_yaw_rad)) { + enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); + return false; + } + const Eigen::Matrix3d Rz_locked = + Eigen::AngleAxisd(locked_yaw_rad, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + locked_target_rotation_ = R_target * Rz_locked; + locked_target_rotation_valid_ = true; } - locked_target_yaw_valid_ = true; - } - const Eigen::Matrix3d Rz_locked = - Eigen::AngleAxisd(locked_target_yaw_rad_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - R_target = R_target * Rz_locked; - target_rotvec = rotvecFromRotationMatrix(R_target); + R_target = locked_target_rotation_; + break; + case AlignMode::POSITION_ONLY: + // True position-only mode: keep the desired orientation equal to the + // current tag orientation every frame so IBVS does not actively try to + // correct rotational error. + R_target = R_current; + break; } + const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target); ibvs_.setTrackedTagId(tag_id); - if (!ibvs_target_initialized_ || tracker_.lastSwitched()) { + const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() || + options_.align_mode == AlignMode::POSITION_ONLY; + if (refresh_target) { if (!ibvs_.setTargetFromPointInTag(p_t_target, options_.hover_target_in_camera, target_rotvec.x(), @@ -1088,15 +1113,28 @@ bool TouchScreenApp::stepAligning() { } last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera; - const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); const Eigen::Vector3d rot_error_vec = rotvecFromRotationMatrix(R_target.transpose() * R_current); - if (std::abs(last_align_error_camera_.x()) <= options_.align_error_threshold6[0] && + bool align_ok = + std::abs(last_align_error_camera_.x()) <= options_.align_error_threshold6[0] && std::abs(last_align_error_camera_.y()) <= options_.align_error_threshold6[1] && - std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2] && - std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] && - std::abs(rot_error_vec.y()) <= options_.align_error_threshold6[4] && - std::abs(rot_error_vec.z()) <= options_.align_error_threshold6[5]) { + std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2]; + switch (options_.align_mode) { + case AlignMode::POSE_AND_POSITION: + align_ok = align_ok && + std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] && + std::abs(rot_error_vec.y()) <= options_.align_error_threshold6[4] && + std::abs(rot_error_vec.z()) <= options_.align_error_threshold6[5]; + break; + case AlignMode::RX_RY_AND_POSITION: + align_ok = align_ok && + std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] && + std::abs(rot_error_vec.y()) <= options_.align_error_threshold6[4]; + break; + case AlignMode::POSITION_ONLY: + break; + } + if (align_ok) { ++align_stable_count_; } else { align_stable_count_ = 0; diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index fa833a13..25928296 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -58,7 +58,7 @@ - + @@ -73,27 +73,27 @@ - + - - - - - + + - - - - - + + - - - - - + + - + 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 df778a62..410d517c 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 @@ -51,15 +51,15 @@ hover_target_in_camera { target_rx: 3.14159265358979323846 target_ry: 0.0 target_rz: 0.0 -lock_tag_yaw_on_target_start: true +align_mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION ibvs_lambda: 0.3 ibvs_mu: 0.1 -ibvs_qdot_max: 0.15 +ibvs_qdot_max: 0.35 ibvs_vmax6 { - vx: 0.15 - vy: 0.15 - vz: 0.20 + vx: 0.55 + vy: 0.55 + vz: 0.55 wx: 0.6 wy: 0.6 wz: 0.6 @@ -89,14 +89,14 @@ control_joint_names: "R_WRIST_Y" control_joint_names: "R_WRIST_R" align_error_threshold6 { - x: 0.003 - y: 0.003 - z: 0.003 + x: 0.006 + y: 0.006 + z: 0.006 rx: 0.1026646259971647 ry: 0.1026646259971647 rz: 0.1026646259971647 } -align_stable_frames: 3 +align_stable_frames: 2 align_timeout_s: 30.0 pause_after_align_reached: false 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 3dcb6324..3c4f2dc5 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 @@ -70,6 +70,12 @@ enum TouchScreenTactileRegion { TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4; } +enum TouchScreenAlignMode { + TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION = 0; + TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION = 1; + TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY = 2; +} + message TouchScreenAppConfig { optional string robot_id = 40; optional string dexhand_id = 41; @@ -93,7 +99,7 @@ message TouchScreenAppConfig { optional double target_rx = 9; optional double target_ry = 10; optional double target_rz = 11; - optional bool lock_tag_yaw_on_target_start = 54; + optional TouchScreenAlignMode align_mode = 54; optional double ibvs_lambda = 12; optional double ibvs_mu = 13;