fix:touch app tag rotation

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, z, rx, ry, rz]`。
-        // 目标点在相机坐标系 x/y 方向的允许误差，单位米。
+        // 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。
-        double align_xy_threshold_m{0.003};
+        std::array<double, 6> align_error_threshold6{{0.003, 0.003, 0.010,
-        // 目标点在相机坐标系 z 方向的允许误差，单位米。
+                                                       0.08726646259971647,
-        double align_z_threshold_m{0.010};
+                                                       0.08726646259971647,
-        // tag 当前姿态与目标姿态的允许夹角误差，单位弧度。
+                                                       0.08726646259971647}};
-        double align_rot_threshold_rad{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()};
 

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<double, 6, 1>& 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<size_t>(i)] = ibvs_vmax[i];
         }
     }
+    if (config.has_align_error_threshold6()) {
+        Eigen::Matrix<double, 6, 1> 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<size_t>(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,
+                                           target_rotvec.x(),
-                                           options_.target_ry,
+                                           target_rotvec.y(),
-                                           options_.target_rz)) {
+                                           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 =
+    const Eigen::Vector3d rot_error_vec =
-        rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz);
+        rotvecFromRotationMatrix(R_target.transpose() * R_current);
-    const double rot_error_rad = rotationErrorRad(R_current, R_target);
+    if (std::abs(last_align_error_camera_.x()) <= options_.align_error_threshold6[0] &&
-    if (std::abs(last_align_error_camera_.x()) <= options_.align_xy_threshold_m &&
+        std::abs(last_align_error_camera_.y()) <= options_.align_error_threshold6[1] &&
-        std::abs(last_align_error_camera_.y()) <= options_.align_xy_threshold_m &&
+        std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2] &&
-        std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m &&
+        std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] &&
-        rot_error_rad <= options_.align_rot_threshold_rad) {
+        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;

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_error_threshold6 {
-align_z_threshold_m: 0.003
+  x: 0.003
-align_rot_threshold_rad: 0.1026646259971647
+  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

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;