diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index 8ab1cade..586065ba 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -106,6 +106,9 @@ 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}; // IBVS 参数。 // 视觉伺服增益 lambda。 @@ -136,13 +139,12 @@ public: "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; - // 视觉对准收敛判据。 - // 目标点在相机坐标系 x/y 方向的允许误差，单位米。 - double align_xy_threshold_m{0.003}; - // 目标点在相机坐标系 z 方向的允许误差，单位米。 - double align_z_threshold_m{0.010}; - // tag 当前姿态与目标姿态的允许夹角误差，单位弧度。 - double align_rot_threshold_rad{0.08726646259971647}; + // 视觉对准收敛判据 `[x, y, z, rx, ry, rz]`。 + // 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。 + std::array align_error_threshold6{{0.003, 0.003, 0.010, + 0.08726646259971647, + 0.08726646259971647, + 0.08726646259971647}}; // 连续多少帧都满足阈值，才认为对准完成。 int align_stable_frames{5}; // 对准阶段超时时间，单位秒。 @@ -300,6 +302,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 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 f20db377..9bfa5ce6 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -119,6 +119,14 @@ double rotationErrorRad(const Eigen::Matrix3d& R_current, return std::acos(cos_angle); } +Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) { + const Eigen::AngleAxisd aa(R); + if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) { + return Eigen::Vector3d::Zero(); + } + return aa.axis() * aa.angle(); +} + bool appendRequestedTactileRegions( const device::AbstractDexHand::FingerType finger, const TouchScreenApp::TactileRegion region, @@ -241,6 +249,28 @@ void applyTwist6FromConfig(const cmvr::config::TouchScreenTwist6& src, } } +void applyErrorThreshold6FromConfig(const cmvr::config::TouchScreenErrorThreshold6& src, + Eigen::Matrix& dst) { + if (src.has_x()) { + dst[0] = src.x(); + } + if (src.has_y()) { + dst[1] = src.y(); + } + if (src.has_z()) { + dst[2] = src.z(); + } + if (src.has_rx()) { + dst[3] = src.rx(); + } + if (src.has_ry()) { + dst[4] = src.ry(); + } + if (src.has_rz()) { + dst[5] = src.rz(); + } +} + void applyMatrix3dFromConfig(const cmvr::config::TouchScreenMatrix3d& src, Eigen::Matrix3d& dst) { if (src.has_m00()) { @@ -499,6 +529,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; touch_start_position_valid_ = false; touch_start_position_base_.setZero(); phase_ = Phase::ALIGNING; @@ -578,6 +610,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; touch_start_position_valid_ = false; touch_start_position_base_.setZero(); last_status_ = Status::STOPPED; @@ -684,6 +718,9 @@ 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_ibvs_lambda()) { options.ibvs_lambda = config.ibvs_lambda(); @@ -703,6 +740,28 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& options.ibvs_vmax6[static_cast(i)] = ibvs_vmax[i]; } } + if (config.has_align_error_threshold6()) { + Eigen::Matrix align_err; + align_err << options.align_error_threshold6[0], options.align_error_threshold6[1], options.align_error_threshold6[2], + options.align_error_threshold6[3], options.align_error_threshold6[4], options.align_error_threshold6[5]; + applyErrorThreshold6FromConfig(config.align_error_threshold6(), align_err); + for (int i = 0; i < 6; ++i) { + options.align_error_threshold6[static_cast(i)] = align_err[i]; + } + } else { + if (config.has_align_xy_threshold_m()) { + options.align_error_threshold6[0] = config.align_xy_threshold_m(); + options.align_error_threshold6[1] = config.align_xy_threshold_m(); + } + if (config.has_align_z_threshold_m()) { + options.align_error_threshold6[2] = config.align_z_threshold_m(); + } + if (config.has_align_rot_threshold_rad()) { + options.align_error_threshold6[3] = config.align_rot_threshold_rad(); + options.align_error_threshold6[4] = config.align_rot_threshold_rad(); + options.align_error_threshold6[5] = config.align_rot_threshold_rad(); + } + } if (config.has_enable_joint_limit_avoidance()) { options.enable_joint_limit_avoidance = config.enable_joint_limit_avoidance(); } @@ -729,13 +788,16 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& } if (config.has_align_xy_threshold_m()) { - options.align_xy_threshold_m = config.align_xy_threshold_m(); + options.align_error_threshold6[0] = config.align_xy_threshold_m(); + options.align_error_threshold6[1] = config.align_xy_threshold_m(); } if (config.has_align_z_threshold_m()) { - options.align_z_threshold_m = config.align_z_threshold_m(); + options.align_error_threshold6[2] = config.align_z_threshold_m(); } if (config.has_align_rot_threshold_rad()) { - options.align_rot_threshold_rad = config.align_rot_threshold_rad(); + options.align_error_threshold6[3] = config.align_rot_threshold_rad(); + options.align_error_threshold6[4] = config.align_rot_threshold_rad(); + options.align_error_threshold6[5] = config.align_rot_threshold_rad(); } if (config.has_align_stable_frames()) { options.align_stable_frames = config.align_stable_frames(); @@ -816,6 +878,11 @@ bool TouchScreenApp::applyOptions() { options_.touch_speedl_forward_l < 0.0) { return false; } + for (const double threshold_i : options_.align_error_threshold6) { + if (!std::isfinite(threshold_i) || threshold_i < 0.0) { + return false; + } + } if (!options_.touch_use_speedl) { Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); if (!computeLinearMoveDeltaTool(options_.touch_twist_base, @@ -947,13 +1014,40 @@ bool TouchScreenApp::stepAligning() { return true; } + const auto* current_tag = perception_->findTag(tag_id); + if (!current_tag) { + sendZeroJointVelocity(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + 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; + } + 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); + } + ibvs_.setTrackedTagId(tag_id); if (!ibvs_target_initialized_ || tracker_.lastSwitched()) { if (!ibvs_.setTargetFromPointInTag(p_t_target, options_.hover_target_in_camera, - options_.target_rx, - options_.target_ry, - options_.target_rz)) { + target_rotvec.x(), + target_rotvec.y(), + target_rotvec.z())) { enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); return false; } @@ -994,22 +1088,15 @@ bool TouchScreenApp::stepAligning() { } last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera; - const auto* current_tag = perception_->findTag(tag_id); - if (!current_tag) { - sendZeroJointVelocity(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - } - const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); - const Eigen::Matrix3d R_target = - rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz); - const double rot_error_rad = rotationErrorRad(R_current, R_target); - if (std::abs(last_align_error_camera_.x()) <= options_.align_xy_threshold_m && - std::abs(last_align_error_camera_.y()) <= options_.align_xy_threshold_m && - std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m && - rot_error_rad <= options_.align_rot_threshold_rad) { + 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] && + 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]) { ++align_stable_count_; } else { align_stable_count_ = 0; 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 695bf873..df778a62 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,6 +51,7 @@ hover_target_in_camera { target_rx: 3.14159265358979323846 target_ry: 0.0 target_rz: 0.0 +lock_tag_yaw_on_target_start: true ibvs_lambda: 0.3 ibvs_mu: 0.1 @@ -87,9 +88,14 @@ control_joint_names: "R_WRIST_P" control_joint_names: "R_WRIST_Y" control_joint_names: "R_WRIST_R" -align_xy_threshold_m: 0.003 -align_z_threshold_m: 0.003 -align_rot_threshold_rad: 0.1026646259971647 +align_error_threshold6 { + x: 0.003 + y: 0.003 + z: 0.003 + rx: 0.1026646259971647 + ry: 0.1026646259971647 + rz: 0.1026646259971647 +} align_stable_frames: 3 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 9557a861..3dcb6324 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 @@ -29,6 +29,15 @@ message TouchScreenMatrix3d { optional double m22 = 9; } +message TouchScreenErrorThreshold6 { + optional double x = 1; + optional double y = 2; + optional double z = 3; + optional double rx = 4; + optional double ry = 5; + optional double rz = 6; +} + message TouchScreenInitJointPoint { optional string joint_name = 1; optional double rad = 2; @@ -84,6 +93,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 double ibvs_lambda = 12; optional double ibvs_mu = 13; @@ -102,6 +112,7 @@ message TouchScreenAppConfig { optional double align_xy_threshold_m = 23; optional double align_z_threshold_m = 24; optional double align_rot_threshold_rad = 25; + TouchScreenErrorThreshold6 align_error_threshold6 = 55; optional int32 align_stable_frames = 26; optional double align_timeout_s = 27; optional bool pause_after_align_reached = 28;