From 964b0457ca83d9fb9f1e75ed9f6407cfdff14bf1 Mon Sep 17 00:00:00 2001
From: lgv <lgv2016@qq.com>
Date: Tue, 15 Sep 2026 10:42:56 +0800
Subject: [PATCH] feat:add pbvs touch

---
 MUJOCO_LOG.TXT                                |   6 +
 .../include/cartesian_velocity_controller.h   |   2 +
 .../src/cartesian_velocity_controller.cpp     |   9 +
 .../kinematics/ik_solver/CMakeLists.txt       |  13 +-
 .../include/pinocchio_qp_ik_solver.h          |   5 +
 .../pinocchio/src/pinocchio_ik_base.cpp       |  20 +-
 .../pinocchio/src/pinocchio_qp_ik_solver.cpp  | 135 ++++++-
 .../src/pinocchio_qp_ik_solver_test.cpp       | 104 +++++
 .../apriltag/include/apriltag_perception.h    |   4 +
 cmvr-es/common/math/proto_geometry.h          |  24 ++
 cmvr-es/config/devices/arm/arm.pb.txt         |  11 +-
 .../config/devices/arm/arm_gen2_mujoco.pb.txt |  11 +-
 cmvr-es/config/devices/arm/arm_mujoco.pb.txt  |  11 +-
 .../config/devices/arm/arm_mujoco_qp.pb.txt   |  11 +-
 cmvr-es/config/devices/arm/arm_qp.pb.txt      |  11 +-
 .../touch_screen_task.pb.txt                  |  54 ++-
 .../touch_screen_task_mujoco.pb.txt           |  72 ++--
 .../motor_robot_arm/include/motor_robot_arm.h |   9 +
 .../motor_robot_arm/src/motor_robot_arm.cpp   | 187 ++++++++-
 cmvr-es/devices/camera/abstract_camera.h      |  27 +-
 .../common/include/camera_stream_encoder.h    |  26 ++
 .../common/include/camera_stream_overlay.h    |  25 ++
 .../common/src/camera_stream_encoder.cpp      | 141 ++++++-
 .../mujoco_camera/src/mujoco_camera.cpp       |   6 +-
 .../realsense_camera/src/realsense_camera.cpp |   8 +
 .../camera/uvc_camera/src/uvc_camera.cpp      |   8 +
 .../include/touch_screen_task.h               |   8 +
 .../src/touch_screen_task.cpp                 | 375 +++++++++++++-----
 .../src/touch_screen_task_test.cpp            |  59 ++-
 .../cmvr/config/arm_config/arm_config.proto   |  11 +
 protos/cmvr/config/joint_limits_config.proto  |   1 -
 .../touch_screen_task_config.proto            |  58 ++-
 32 files changed, 1256 insertions(+), 196 deletions(-)
 create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp
 create mode 100644 cmvr-es/devices/camera/common/include/camera_stream_overlay.h

diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT
index 55930968..74d127a1 100644
--- a/MUJOCO_LOG.TXT
+++ b/MUJOCO_LOG.TXT
@@ -4,3 +4,9 @@ ERROR: could not create window
 Fri Jul 24 15:40:37 2026
 ERROR: could not create window
 
+Fri Sep 11 13:10:38 2026
+ERROR: could not initialize GLFW
+
+Fri Sep 11 14:13:09 2026
+ERROR: could not initialize GLFW
+
diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
index 9d138abc..d8a728ed 100644
--- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
+++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
@@ -25,6 +25,7 @@ public:
         double stop_command_velocity_norm{1e-3};
         double stop_measured_velocity_norm{1e-2};
         double stop_acceleration{0.5};
+        double stop_timeout_s{2.0};
     };
 
     using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
@@ -48,6 +49,7 @@ public:
     void shutdown();
 
     bool busy() const { return busy_.load(); }
+    double stopTimeoutS() const { return config_.stop_timeout_s; }
     CartesianVelocity getCommandTwistBase() const;
 
 private:
diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
index daf69fb7..91ed55b3 100644
--- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
+++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
@@ -29,6 +29,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
     if (config.stop_acceleration <= 0.0) {
         config.stop_acceleration = defaults.stop_acceleration;
     }
+    if (!std::isfinite(config.stop_timeout_s) || config.stop_timeout_s <= 0.0) {
+        config.stop_timeout_s = defaults.stop_timeout_s;
+    }
     return config;
 }
 
@@ -108,6 +111,12 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
+    // A completed speedL command leaves the worker thread joinable but idle.
+    // Do not turn that idle worker into a new command just because a caller
+    // requests a stop during a task transition.
+    if (!busy_.load()) {
+        return Result::success();
+    }
     requestStop_(acceleration);
     return Result::success();
 }
diff --git a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt
index 310be1b7..1ade9e29 100644
--- a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt
+++ b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt
@@ -25,4 +25,15 @@ target_link_libraries(ik_solver PUBLIC
 
 add_library(cmvr_es::ik_solver ALIAS ik_solver)
 
-install(TARGETS ik_solver LIBRARY DESTINATION lib)
\ No newline at end of file
+install(TARGETS ik_solver LIBRARY DESTINATION lib)
+
+add_executable(pinocchio_qp_ik_solver_test
+        pinocchio/src/pinocchio_qp_ik_solver_test.cpp
+)
+
+target_link_libraries(pinocchio_qp_ik_solver_test PRIVATE
+        cmvr_es::ik_solver
+        cmvr_es::proto
+        gtest
+        gtest_main
+)
diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h
index 1fa37f8a..ffce089f 100644
--- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h
+++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h
@@ -45,6 +45,11 @@ public:
                            std::vector<double>& qdot_out,
                            double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
 
+    // Projects a secondary joint velocity into the Cartesian task null space.
+    static Eigen::VectorXd projectJointLimitAvoidanceToNullspace(
+        const Eigen::MatrixXd& jacobian_base,
+        const Eigen::VectorXd& qdot_avoid);
+
     /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
     void setVelocityLimits(const Eigen::VectorXd &qd_max);
     void setAccelerationLimits(const Eigen::VectorXd &qdd_max);
diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp
index 823073a0..3ceba79e 100644
--- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp
+++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp
@@ -164,7 +164,9 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
 
     const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
     const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
-    Eigen::VectorXd limited = qdot;
+    // Apply one common scale factor instead of changing individual joints.
+    // Per-joint scaling changes J*qdot and can disturb the Cartesian task.
+    double scale = 1.0;
     for (Eigen::Index i = 0; i < q_chain.size(); ++i) {
         const double lower = joint_pos_lower_limits_[i];
         const double upper = joint_pos_upper_limits_[i];
@@ -174,21 +176,15 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
 
         const double span = upper - lower;
         const double margin = std::max(min_margin_rad, margin_ratio * span);
-        if (limited[i] < 0.0 && q_chain[i] < lower + margin) {
+        if (qdot[i] < 0.0 && q_chain[i] < lower + margin) {
             const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
-            if (q_chain[i] <= lower) {
-                limited[i] = std::max(0.0, limited[i]);
-            }
-        } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) {
+            scale = std::min(scale, ratio);
+        } else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) {
             const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
-            if (q_chain[i] >= upper) {
-                limited[i] = std::min(0.0, limited[i]);
-            }
+            scale = std::min(scale, ratio);
         }
     }
-    return limited;
+    return scale * qdot;
 }
 
 void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {
diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp
index ce7af494..4825d0a5 100644
--- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp
+++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp
@@ -13,11 +13,66 @@
 #include <pinocchio/spatial/explog.hpp>
 
 #include <algorithm>   // std::clamp, std::max, std::min
+#include <atomic>
 #include <cmath>       // std::sqrt
+#include <cstdint>
 #include <limits>
+#include <sstream>
 #include <unordered_map>
+#include <Eigen/SVD>
 
 namespace cmvr {
+    namespace {
+
+    Eigen::MatrixXd moorePenrosePseudoInverse(const Eigen::MatrixXd& matrix)
+    {
+        if (matrix.rows() == 0 || matrix.cols() == 0 || !matrix.allFinite()) {
+            return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
+        }
+
+        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+            matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
+        if (svd.info() != Eigen::Success) {
+            return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
+        }
+
+        const Eigen::VectorXd singular_values = svd.singularValues();
+        const double max_singular = singular_values.size() > 0
+                                        ? singular_values.maxCoeff()
+                                        : 0.0;
+        const double tolerance =
+            std::numeric_limits<double>::epsilon() *
+            static_cast<double>(std::max(matrix.rows(), matrix.cols())) *
+            std::max(1.0, max_singular);
+        Eigen::VectorXd inverse_singular = singular_values;
+        for (Eigen::Index i = 0; i < inverse_singular.size(); ++i) {
+            inverse_singular[i] = singular_values[i] > tolerance
+                                      ? 1.0 / singular_values[i]
+                                      : 0.0;
+        }
+
+        const Eigen::Index rank_dimension = singular_values.size();
+        return svd.matrixV().leftCols(rank_dimension) *
+               inverse_singular.asDiagonal() *
+               svd.matrixU().leftCols(rank_dimension).transpose();
+    }
+
+    std::string vectorToString(const Eigen::VectorXd& value)
+    {
+        std::ostringstream stream;
+        stream << '[';
+        for (Eigen::Index i = 0; i < value.size(); ++i) {
+            if (i > 0) {
+                stream << ' ';
+            }
+            stream << value[i];
+        }
+        stream << ']';
+        return stream.str();
+    }
+
+    } // namespace
+
     using Eigen::Matrix4d;
     using Eigen::VectorXd;
     using Eigen::MatrixXd;
@@ -208,6 +263,24 @@ namespace cmvr {
         }
     }
 
+    Eigen::VectorXd PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
+        const Eigen::MatrixXd& jacobian_base,
+        const Eigen::VectorXd& qdot_avoid)
+    {
+        if (jacobian_base.cols() != qdot_avoid.size() ||
+            jacobian_base.rows() == 0 || jacobian_base.cols() == 0 ||
+            !jacobian_base.allFinite() || !qdot_avoid.allFinite()) {
+            return Eigen::VectorXd::Zero(qdot_avoid.size());
+        }
+
+        const Eigen::MatrixXd jacobian_pinv =
+            moorePenrosePseudoInverse(jacobian_base);
+        const Eigen::MatrixXd nullspace =
+            Eigen::MatrixXd::Identity(jacobian_base.cols(), jacobian_base.cols()) -
+            jacobian_pinv * jacobian_base;
+        return nullspace * qdot_avoid;
+    }
+
     bool PinocchioQpIKSolver::ik(const Matrix4d &target_pose,
                                  std::vector<double> &joints_angle,
                                  bool is_tcp) {
@@ -389,7 +462,7 @@ namespace cmvr {
         const int dof = chain_v_dof_;
         const auto& avoidance = jointLimitPolicy().avoidance();
         const bool use_joint_limit_avoidance =
-            !jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0;
+            !jointLimitsDisabled() && avoidance.enable() && avoidance.gain() > 0.0;
         const int avoidance_rows = use_joint_limit_avoidance ? dof : 0;
 
         MatrixXd cost(6 + dof + avoidance_rows, dof);
@@ -406,7 +479,7 @@ namespace cmvr {
         VectorXd upper(dof);
         const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
         if (use_joint_limit_avoidance) {
-            const VectorXd qdot_avoid =
+            const VectorXd qdot_avoid_raw =
                 cmvr::kinematics::computeJointLimitAvoidanceVelocity(
                     q_chain,
                     joint_pos_lower_limits_,
@@ -415,10 +488,32 @@ namespace cmvr {
                     positiveOr(avoidance.gain(), 0.2),
                     positiveOr(avoidance.margin_ratio(), 0.15),
                     positiveOr(avoidance.max_push(), 0.25));
-            const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05));
+            const Eigen::MatrixXd jacobian_pinv =
+                moorePenrosePseudoInverse(jacobian_base);
+            const bool jacobian_pinv_valid =
+                jacobian_pinv.rows() == dof && jacobian_pinv.cols() == 6 &&
+                jacobian_pinv.allFinite();
+            Eigen::MatrixXd nullspace = MatrixXd::Zero(dof, dof);
+            if (jacobian_pinv_valid) {
+                nullspace = MatrixXd::Identity(dof, dof) -
+                            jacobian_pinv * jacobian_base;
+            }
+            const VectorXd qdot_avoid_null = nullspace * qdot_avoid_raw;
             cost.middleRows(6 + dof, dof) =
-                sqrt_weight * MatrixXd::Identity(dof, dof);
-            target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid;
+                nullspace;
+            target.segment(6 + dof, dof) = qdot_avoid_null;
+
+            static std::atomic<std::uint64_t> avoidance_debug_counter{0};
+            const auto debug_index =
+                avoidance_debug_counter.fetch_add(1, std::memory_order_relaxed);
+            if (debug_index % 1000 == 0) {
+                CMVR_LOG(DEBUG)
+                    << "[PinocchioQpIKSolver][JOINT_LIMIT_AVOIDANCE]"
+                    << " qdot_avoid_raw=" << vectorToString(qdot_avoid_raw)
+                    << " qdot_avoid_null=" << vectorToString(qdot_avoid_null)
+                    << " norm(J*qdot_avoid_null)="
+                    << (jacobian_base * qdot_avoid_null).norm();
+            }
         }
         for (int i = 0; i < dof; ++i) {
             double limit = std::numeric_limits<double>::infinity();
@@ -452,6 +547,35 @@ namespace cmvr {
                     }
                 }
             }
+
+            const auto& soft_limit = jointLimitPolicy().soft_limit();
+            if (!jointLimitsDisabled() && soft_limit.enable() &&
+                joint_pos_lower_limits_.size() == dof &&
+                joint_pos_upper_limits_.size() == dof) {
+                const double q_min = joint_pos_lower_limits_[i];
+                const double q_max = joint_pos_upper_limits_[i];
+                if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) {
+                    const double span = q_max - q_min;
+                    const double margin = std::max(
+                        positiveOr(soft_limit.min_margin_rad(), 0.02),
+                        positiveOr(soft_limit.margin_ratio(), 0.08) * span);
+                    if (q_chain[i] < q_min + margin) {
+                        const double ratio = std::clamp(
+                            (q_chain[i] - q_min) / margin, 0.0, 1.0);
+                        lower[i] = std::max(lower[i], -limit * ratio);
+                        if (q_chain[i] <= q_min) {
+                            lower[i] = std::max(0.0, lower[i]);
+                        }
+                    } else if (q_chain[i] > q_max - margin) {
+                        const double ratio = std::clamp(
+                            (q_max - q_chain[i]) / margin, 0.0, 1.0);
+                        upper[i] = std::min(upper[i], limit * ratio);
+                        if (q_chain[i] >= q_max) {
+                            upper[i] = std::min(0.0, upper[i]);
+                        }
+                    }
+                }
+            }
         }
 
         QPSolver solver;
@@ -475,7 +599,6 @@ namespace cmvr {
         if (qdot.size() != dof) {
             return false;
         }
-        qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
         qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
         return true;
     }
diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp
new file mode 100644
index 00000000..9edbb089
--- /dev/null
+++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp
@@ -0,0 +1,104 @@
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
+
+#include <filesystem>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "common/io/proto_file_io.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+
+namespace cmvr {
+namespace {
+
+std::filesystem::path findProjectRoot()
+{
+    std::filesystem::path current = std::filesystem::current_path();
+    while (!current.empty()) {
+        if (std::filesystem::exists(
+                current / "model/xiaoyan_description/dual_arm.urdf")) {
+            return current;
+        }
+        const auto parent = current.parent_path();
+        if (parent == current) {
+            break;
+        }
+        current = parent;
+    }
+    return {};
+}
+
+config::PinocchioQpIKConfig loadQpConfig(const std::filesystem::path& root)
+{
+    config::ArmRootConfig root_config;
+    const auto config_path =
+        root / "cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt";
+    EXPECT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(config_path.string(), &root_config));
+    EXPECT_GT(root_config.arm().robot_arms_size(), 0);
+    auto solver_config =
+        root_config.arm().robot_arms(0).kinematics().pinocchio_qp_ik_solver();
+    solver_config.set_urdf_path(
+        (root / "model/xiaoyan_description/dual_arm.urdf").string());
+    return solver_config;
+}
+
+TEST(PinocchioQpIKSolverTest, JointLimitAvoidanceIsInCartesianNullspace)
+{
+    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
+    jacobian.leftCols(6).setIdentity();
+    jacobian.col(6) << 0.3, -0.2, 0.4, -0.1, 0.25, 0.15;
+    Eigen::VectorXd qdot_avoid(7);
+    qdot_avoid << 0.4, -0.3, 0.2, 0.1, -0.5, 0.6, -0.7;
+
+    const Eigen::VectorXd qdot_null =
+        PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
+            jacobian, qdot_avoid);
+
+    ASSERT_EQ(qdot_null.size(), 7);
+    EXPECT_LT((jacobian * qdot_null).norm(), 1e-12);
+    EXPECT_GT(qdot_null.norm(), 0.0);
+}
+
+TEST(PinocchioQpIKSolverTest, AvoidanceDoesNotDisturbReachableCartesianTwist)
+{
+    const auto root = findProjectRoot();
+    ASSERT_FALSE(root.empty());
+
+    auto disabled_config = loadQpConfig(root);
+    disabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(false);
+    auto enabled_config = disabled_config;
+    enabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(true);
+
+    PinocchioQpIKSolver solver_disabled(disabled_config);
+    PinocchioQpIKSolver solver_enabled(enabled_config);
+    ASSERT_TRUE(solver_disabled.init());
+    ASSERT_TRUE(solver_enabled.init());
+
+    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
+    jacobian.leftCols(6).setIdentity();
+    Eigen::Matrix<double, 6, 1> target_twist;
+    target_twist << 0.15, -0.10, 0.08, 0.05, -0.04, 0.03;
+
+    // The last joint is inside its configured soft-limit margin, while the
+    // first six columns fully span the Cartesian task.
+    const std::vector<double> q_chain = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.56};
+    std::vector<double> qdot_disabled;
+    std::vector<double> qdot_enabled;
+    ASSERT_TRUE(solver_disabled.solveVelocityBase(
+        jacobian, target_twist, q_chain, qdot_disabled, 10.0));
+    ASSERT_TRUE(solver_enabled.solveVelocityBase(
+        jacobian, target_twist, q_chain, qdot_enabled, 10.0));
+
+    const Eigen::Map<const Eigen::VectorXd> qdot_disabled_eigen(
+        qdot_disabled.data(), static_cast<Eigen::Index>(qdot_disabled.size()));
+    const Eigen::Map<const Eigen::VectorXd> qdot_enabled_eigen(
+        qdot_enabled.data(), static_cast<Eigen::Index>(qdot_enabled.size()));
+    const Eigen::VectorXd achieved_disabled = jacobian * qdot_disabled_eigen;
+    const Eigen::VectorXd achieved_enabled = jacobian * qdot_enabled_eigen;
+
+    EXPECT_LT((achieved_enabled - achieved_disabled).norm(), 1e-6);
+    EXPECT_LT((achieved_enabled - target_twist).norm(), 5e-5);
+}
+
+} // namespace
+} // namespace cmvr
diff --git a/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h b/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h
index a4486e56..6f4e57cb 100644
--- a/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h
+++ b/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h
@@ -84,6 +84,10 @@ public:
 
         // Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
         Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
+
+        // Semantic alias used by visualization and downstream consumers:
+        // `T_C_Tag` maps points in this tag frame into camera frame C.
+        const Eigen::Matrix4d& T_C_Tag() const { return T_c_t; }
     };
 
     struct FrameCache {
diff --git a/cmvr-es/common/math/proto_geometry.h b/cmvr-es/common/math/proto_geometry.h
index f065ef88..757e8616 100644
--- a/cmvr-es/common/math/proto_geometry.h
+++ b/cmvr-es/common/math/proto_geometry.h
@@ -27,6 +27,26 @@ inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
     return toEigenVec3(src, Eigen::Vector3d::Zero());
 }
 
+inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src,
+                                    Eigen::Vector3d defaults)
+{
+    if (src.has_rx()) {
+        defaults.x() = src.rx();
+    }
+    if (src.has_ry()) {
+        defaults.y() = src.ry();
+    }
+    if (src.has_rz()) {
+        defaults.z() = src.rz();
+    }
+    return defaults;
+}
+
+inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src)
+{
+    return toEigenEuler(src, Eigen::Vector3d::Zero());
+}
+
 inline Eigen::Matrix<double, 6, 1> toEigenVec6(
     const cmvr::common::Vec6& src,
     Eigen::Matrix<double, 6, 1> defaults)
@@ -102,6 +122,10 @@ inline bool hasVec3(const cmvr::common::Vec3& value) {
     return value.has_x() && value.has_y() && value.has_z();
 }
 
+inline bool hasEuler(const cmvr::common::Euler& value) {
+    return value.has_rx() && value.has_ry() && value.has_rz();
+}
+
 inline bool hasVec6(const cmvr::common::Vec6& value) {
     return value.has_x() && value.has_y() && value.has_z() &&
            value.has_rx() && value.has_ry() && value.has_rz();
diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt
index 6dd5fca9..8040ce6f 100644
--- a/cmvr-es/config/devices/arm/arm.pb.txt
+++ b/cmvr-es/config/devices/arm/arm.pb.txt
@@ -51,7 +51,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
-            weight: 0.05
           }
         }
       }
@@ -59,6 +58,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -144,6 +151,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 10
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }
diff --git a/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt
index bf9eae37..f5c8d8f5 100644
--- a/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt
+++ b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt
@@ -50,7 +50,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
-            weight: 2.0
           }
         }
       }
@@ -58,6 +57,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -143,6 +150,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 2.0
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }
diff --git a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt
index 31361224..6acc138b 100644
--- a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt
+++ b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt
@@ -51,7 +51,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
-            weight: 2.0
           }
         }
       }
@@ -59,6 +58,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -144,6 +151,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 10
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }
diff --git a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt
index 3960aacf..26b59ef0 100644
--- a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt
+++ b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt
@@ -52,7 +52,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.01
             max_push: 0.02
-            weight: 0.05
           }
         }
       }
@@ -60,6 +59,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -145,6 +152,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 5
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }
diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt
index 6e952b48..30b9cb6a 100644
--- a/cmvr-es/config/devices/arm/arm_qp.pb.txt
+++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt
@@ -52,7 +52,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.01
             max_push: 0.02
-            weight: 0.05
           }
         }
       }
@@ -60,6 +59,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -145,6 +152,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 0.5
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }
diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt
index 5625012d..5670dce6 100644
--- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt
+++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt
@@ -1,9 +1,14 @@
 touch_screen_task {
   id: "touch_screen"
+  # 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
+  debug_draw_coordinate_frames: true
+  # G/H 坐标轴长度，单位为米。
+  debug_coordinate_axis_length_m: 0.02
 
   devices {
     arm_id: "right_arm"
     dexhand_id: "paxini_tip_1"
+    # 手部相机和外部相机的 DeviceManager ID。
     camera_id: "right_hand_cam"
     external_camera_id: "cam5"
   }
@@ -20,27 +25,41 @@ touch_screen_task {
     joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
     velocity: 1.0
     acceleration: 2.0
+    # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
+    skip_position_tolerance_rad: 0.001
+    # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+    skip_velocity_tolerance_rad_s: 0.01
   }
 
   perception {
-    apriltag {
-      tag_size_m: 0.012
+    # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
+    tags {
+      screen {
+        id: 1
+        size_m: 0.03
+      }
+      hand {
+        id: 0
+        size_m: 0.03
+      }
+    }
+    # 手部相机：用于点击目标点和手部目标跟踪。
+    hand_camera {
       depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
       target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
     }
-    external_apriltag {
-      tag_size_m: 0.012
-      hand_tag_id: 1
-      t_h_p {
-        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0 m13: -0.03
-        m20: 0.0 m21: 0.0 m22: 1.0 m23: 0.0
-        m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
-      }
-    }
   }
 
   alignment {
+    calibration {
+      #  TCP P 相对于屏幕 Hand Tag H 的目标姿态
+      hand_tag_to_tcp {
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
+        m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
+        m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
+        m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
+      }
+    }
     pbvs {
       position_gain { x: 2.0 y: 2.0 z: 1.5 }
       rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
@@ -49,7 +68,13 @@ touch_screen_task {
       twist_filter_alpha: 1.0
     }
     target {
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
+      # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
+      # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
+      # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
+      # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
+      hand_orientation_G { rx: 0.0 ry: 0.0 rz: 3.141592653589793 }
+      # 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
+      position_offset_G { x: 0.0 y: 0.0 z: 0.05 }
       mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
     }
     error_threshold {
@@ -83,6 +108,7 @@ touch_screen_task {
   retract {
     twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
     acceleration: 8.0
-    duration_s: 0.45
+    # TCP 后退目标距离，单位为米。
+    distance_m: 0.02
   }
 }
diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt
index 329c12a2..5e1ffee2 100644
--- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt
+++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt
@@ -1,9 +1,14 @@
 touch_screen_task {
   id: "touch_screen"
+  # 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
+  debug_draw_coordinate_frames: true
+  # G/H 坐标轴长度，单位为米。
+  debug_coordinate_axis_length_m: 0.02
 
   devices {
     arm_id: "mujoco_right_arm"
     dexhand_id: "mujoco_zero_touch_dexhand"
+    # 手部相机和外部相机的 DeviceManager ID。
     camera_id: "mujoco_hand_cam"
     external_camera_id: "mujoco_external_touch_cam"
   }
@@ -18,30 +23,44 @@ touch_screen_task {
     joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 }
     joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 }
     joint_positions { joint_name: "R_WRIST_R" rad: -0.08 }
-    velocity: 2.8
-    acceleration: 20.0
+    velocity: 2.0
+    acceleration: 3.0
+    # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
+    skip_position_tolerance_rad: 0.001
+    # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+    skip_velocity_tolerance_rad_s: 0.01
   }
 
   perception {
-    apriltag {
-      tag_size_m: 0.03
+    # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
+    tags {
+      screen {
+        id: 1
+        size_m: 0.03
+      }
+      hand {
+        id: 0
+        size_m: 0.03
+      }
+    }
+    # 手部相机：用于点击目标点和手部目标跟踪。
+    hand_camera {
+
       depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
       target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
     }
-    external_apriltag {
-      tag_size_m: 0.03
-      hand_tag_id: 0
-      # Hand Tag H -> TCP P after flipping the tag to face external_touch_cam.
-      t_h_p {
+  }
+
+  alignment {
+    calibration {
+      # TCP P 相对于屏幕 Hand Tag H 的目标姿态
+      hand_tag_to_tcp {
         m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
         m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
         m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
         m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
       }
     }
-  }
-
-  alignment {
     pbvs {
       position_gain { x: 2.0 y: 2.0 z: 1.5 }
       rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
@@ -50,29 +69,27 @@ touch_screen_task {
       twist_filter_alpha: 1.0
     }
     target {
-      rotation_vector {
-        x: 3.141592653589793
-        y: 0.0
-        z: 0.0
+      # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
+      # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
+      # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
+      # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
+      hand_orientation_G {
+        rx: 0.0
+        ry: 0.0
+        rz: 3.141592653589793
       }
+      # 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
       position_offset_G {
         x: 0.0
         y: 0.0
         z: 0.05
       }
-      rotation_offset_G {
-        x: 0.0
-        y: 0.0
-        z: 0.0
-      }
-
-      mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
-
+      mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
     }
     error_threshold {
       x: 0.005
       y: 0.005
-      z: 0.010
+      z: 0.005
       rx: 0.08726646259971647
       ry: 0.08726646259971647
       rz: 0.08726646259971647
@@ -99,7 +116,8 @@ touch_screen_task {
 
   retract {
     twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 5.0
-    duration_s: 2.0
+    acceleration: 4.0
+    # TCP 后退目标距离，单位为米。
+    distance_m: 0.05
   }
 }
diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
index 73be9284..d8e37615 100644
--- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
+++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
@@ -106,6 +106,8 @@ private:
     std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
     bool readArmState_(std::vector<double>& q_now, std::vector<double>& qd_now) const;
     std::vector<double> readJointPosition_() const;
+    Result stopCartesianMotionAndWait_();
+    Result waitForJointTarget_(const std::vector<double>& target) const;
 
     bool configureAlgorithms_();
     bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
@@ -130,6 +132,13 @@ private:
     std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr};
     std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
 
+    // MoveJ post-trajectory settling criteria.  These defaults preserve the
+    // historical behavior when the optional arm configuration fields are absent.
+    double move_j_settle_timeout_s_{2.0};
+    double move_j_position_tolerance_rad_{2e-3};
+    double move_j_velocity_tolerance_rad_s_{2e-2};
+    int move_j_stable_sample_count_{3};
+
     mutable std::mutex mutex_;
     std::atomic<bool> busy_{false};
     double speed_scaling_{1.0};
diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
index 5acc3f39..3cebe17f 100644
--- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
+++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
@@ -516,6 +516,16 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     if (!joint_planner_) {
         return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized");
     }
+
+    // speedL runs in a worker thread and sends speedJ commands.  A moveJ
+    // trajectory writes cyclic-position commands directly, so allowing both
+    // paths to run concurrently can overwrite the motor mode/target and cause
+    // a short surge at the transition.  Finish the Cartesian worker before
+    // reading the planning start state.
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
+    }
     if (busy_.exchange(true)) {
         return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
     }
@@ -559,9 +569,14 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
             return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
         }
         std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
-        std::copy_n(sample.velocity.begin(),
-                    std::min(sample.velocity.size(), command_velocity.size()),
-                    command_velocity.begin());
+        // A MoveJ is a rest-to-rest command.  Do not let a non-zero numerical
+        // endpoint velocity from an alternate planner keep the drive moving
+        // while the position-mode trajectory is being handed back to the arm.
+        if (k + 1 < samples.size()) {
+            std::copy_n(sample.velocity.begin(),
+                        std::min(sample.velocity.size(), command_velocity.size()),
+                        command_velocity.begin());
+        }
         if (!motor_manager_->commandCyclicPositionsAtomic(
                 motors, sample.position, command_velocity)) {
             return Result::failure(ArmErrorCode::CommandFailed,
@@ -575,6 +590,24 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
                                                    std::chrono::duration<double>(next_t)));
         }
     }
+
+    // Repeat the final position with zero velocity before checking feedback.
+    // This seeds the position-mode target after a velocity-to-position switch
+    // and prevents a stale final velocity command from producing a short
+    // motion spike at the destination.
+    if (!motor_manager_->commandCyclicPositionsAtomic(
+            motors, target.position, std::vector<double>(motors.size(), 0.0))) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "failed to hold final moveJ target");
+    }
+    // Allow one servo cycle to consume the explicit hold command before
+    // evaluating feedback-based settling criteria.
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+    const auto settle_result = waitForJointTarget_(target.position);
+    if (!settle_result.ok()) {
+        return settle_result;
+    }
     return Result::success();
 }
 
@@ -638,8 +671,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
     if (const auto stopped = safetyStopResult_("moveL")) {
         return *stopped;
     }
-    if (cartesian_velocity_controller_) {
-        cartesian_velocity_controller_->shutdown();
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
     }
     if (options.asynchronous) {
         return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported");
@@ -715,7 +749,10 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
 
 Result MotorRobotArm::stopMotion()
 {
-    stopL(0.0);
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
+    }
     return stopJ(0.0);
 }
 
@@ -963,8 +1000,142 @@ std::vector<double> MotorRobotArm::readJointPosition_() const
     return q_start;
 }
 
+Result MotorRobotArm::stopCartesianMotionAndWait_()
+{
+    if (!cartesian_velocity_controller_) {
+        return Result::success();
+    }
+
+    if (cartesian_velocity_controller_->busy()) {
+        const auto stop_result = cartesian_velocity_controller_->stop();
+        if (!stop_result.ok()) {
+            return stop_result;
+        }
+
+        const auto deadline = std::chrono::steady_clock::now() +
+                              std::chrono::duration<double>(
+                                  cartesian_velocity_controller_->stopTimeoutS());
+        while (cartesian_velocity_controller_->busy() &&
+               std::chrono::steady_clock::now() < deadline) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        if (cartesian_velocity_controller_->busy()) {
+            // Do not start a position trajectory while the worker can still
+            // issue velocity commands.  Shutdown joins it and sends one final
+            // zero-velocity command before reporting the timeout.
+            cartesian_velocity_controller_->shutdown();
+            return Result::failure(
+                ArmErrorCode::Timeout,
+                "timed out waiting for Cartesian velocity motion to stop");
+        }
+    }
+
+    // The worker may be idle but still joinable.  Joining it here removes any
+    // last command/worker race before the next motion mode is selected.
+    cartesian_velocity_controller_->shutdown();
+    return Result::success();
+}
+
+Result MotorRobotArm::waitForJointTarget_(const std::vector<double>& target) const
+{
+    if (target.size() != joint_names_.size()) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "moveJ target size mismatch while settling");
+    }
+
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::duration<double>(move_j_settle_timeout_s_);
+    int stable_samples = 0;
+    while (std::chrono::steady_clock::now() < deadline) {
+        if (const auto stopped = safetyStopResult_("moveJ", true)) {
+            return *stopped;
+        }
+
+        bool settled = true;
+        for (std::size_t i = 0; i < joint_names_.size(); ++i) {
+            const auto motor = getMotor_(joint_names_[i]);
+            if (!motor) {
+                return Result::failure(
+                    ArmErrorCode::RobotNotReady,
+                    "motor not found while waiting for moveJ target: " + joint_names_[i]);
+            }
+            const double position = motor->getQ();
+            const double velocity = motor->getQd();
+            if (!std::isfinite(position) || !std::isfinite(velocity) ||
+                std::abs(position - target[i]) > move_j_position_tolerance_rad_ ||
+                std::abs(velocity) > move_j_velocity_tolerance_rad_s_) {
+                settled = false;
+                break;
+            }
+        }
+
+        stable_samples = settled ? stable_samples + 1 : 0;
+        if (stable_samples >= move_j_stable_sample_count_) {
+            return Result::success();
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+    CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_;
+    return Result::failure(ArmErrorCode::Timeout,
+                           "timed out waiting for moveJ target to settle");
+}
+
 bool MotorRobotArm::configureAlgorithms_()
 {
+    // Optional settling fields are read once during initialization so every
+    // MoveJ command uses one consistent set of safety criteria.
+    move_j_settle_timeout_s_ = 2.0;
+    move_j_position_tolerance_rad_ = 2e-3;
+    move_j_velocity_tolerance_rad_s_ = 2e-2;
+    move_j_stable_sample_count_ = 3;
+    const auto& move_j_config = cfg_.motion().move_j();
+    if (move_j_config.has_settle_timeout_s()) {
+        const double value = move_j_config.settle_timeout_s();
+        if (!std::isfinite(value) || value <= 0.0) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_timeout_s: " << value;
+            return false;
+        }
+        move_j_settle_timeout_s_ = value;
+    }
+    if (move_j_config.has_settle_position_tolerance_rad()) {
+        const double value = move_j_config.settle_position_tolerance_rad();
+        if (!std::isfinite(value) || value < 0.0) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_position_tolerance_rad: "
+                            << value;
+            return false;
+        }
+        move_j_position_tolerance_rad_ = value;
+    }
+    if (move_j_config.has_settle_velocity_tolerance_rad_s()) {
+        const double value = move_j_config.settle_velocity_tolerance_rad_s();
+        if (!std::isfinite(value) || value < 0.0) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_velocity_tolerance_rad_s: "
+                            << value;
+            return false;
+        }
+        move_j_velocity_tolerance_rad_s_ = value;
+    }
+    if (move_j_config.has_settle_stable_sample_count()) {
+        const int value = move_j_config.settle_stable_sample_count();
+        if (value < 1) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_stable_sample_count: "
+                            << value;
+            return false;
+        }
+        move_j_stable_sample_count_ = value;
+    }
+
+    const auto& cartesian_controller_config =
+        cfg_.motion().speed_l().speed_l_controller().cartesian_velocity_controller();
+    if (cartesian_controller_config.has_stop_timeout_s() &&
+        (!std::isfinite(cartesian_controller_config.stop_timeout_s()) ||
+         cartesian_controller_config.stop_timeout_s() <= 0.0)) {
+        CMVR_LOG(ERROR) << "[MotorRobotArm] invalid Cartesian stop_timeout_s: "
+                        << cartesian_controller_config.stop_timeout_s();
+        return false;
+    }
+
     joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j());
     if (!joint_planner_) {
         return false;
@@ -1077,6 +1248,10 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
     result.stop_acceleration =
         config.stop_acceleration() > 0.0 ? config.stop_acceleration()
                                          : result.stop_acceleration;
+    if (config.has_stop_timeout_s() && std::isfinite(config.stop_timeout_s()) &&
+        config.stop_timeout_s() > 0.0) {
+        result.stop_timeout_s = config.stop_timeout_s();
+    }
     return result;
 }
 
diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h
index a05281dc..525414a7 100644
--- a/cmvr-es/devices/camera/abstract_camera.h
+++ b/cmvr-es/devices/camera/abstract_camera.h
@@ -3,20 +3,22 @@
 #pragma once
 
 #include <opencv2/opencv.hpp>
+#include <mutex>
 #include "../abstract_device.h"
 #include <Eigen/Core>
 #include "cmvr/config/camera_config/camera_config.pb.h"
+#include "devices/camera/common/include/camera_stream_overlay.h"
 
 namespace cmvr::device {
     enum CameraMode {PHOTO_MODE, VIDEO_MODE};
 
     struct Rs2Intrinsics
     {
-        float   cx;
-        float   cy;
-        float   fx;
-        float   fy;
-        float   coeffs[5];
+        float cx{0.0F};
+        float cy{0.0F};
+        float fx{0.0F};
+        float fy{0.0F};
+        float coeffs[5]{};
 
     };
     struct StreamFrameData
@@ -64,11 +66,26 @@ namespace cmvr::device {
             return false;
         }
 
+        // Video overlay is a presentation-only snapshot.  It is deliberately
+        // kept on the camera so the encoding thread can consume it without
+        // coupling the camera to AprilTag or task code.
+        void setStreamOverlay(const CameraStreamOverlay& overlay) {
+            std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
+            stream_overlay_ = overlay;
+        }
+
+        CameraStreamOverlay streamOverlay() const {
+            std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
+            return stream_overlay_;
+        }
+
         virtual bool startStreaming() {return true;}
         virtual void stopStreaming() {}
         virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
     protected:
         CameraState state_{};
+        mutable std::mutex stream_overlay_mutex_;
+        CameraStreamOverlay stream_overlay_{};
         void clear_error_() {
             this->state_.is_error = false;
             this->state_.error_message.clear();
diff --git a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h
index 5001d23d..53222545 100644
--- a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h
+++ b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h
@@ -7,9 +7,12 @@
 #include <opencv2/opencv.hpp>
 
 #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
+#include "devices/camera/common/include/camera_stream_overlay.h"
 
 namespace cmvr::device {
 
+struct Rs2Intrinsics;
+
 struct FfmpegEncoderInfo {
     std::string codec_name;
     int width = 0;
@@ -27,8 +30,23 @@ struct FfmpegEncoderInfo {
 
 struct CameraStreamEncodeOptions {
     bool draw_timestamp = false;
+    CameraStreamOverlay overlay;
 };
 
+// Draw a frame whose pose is expressed as ^C T_Frame onto a BGR/BGRA image.
+// The image is modified in place and no camera/perception state is touched.
+void drawCoordinateFrame(cv::Mat& image,
+                         const Eigen::Matrix4d& T_C_Frame,
+                         const Rs2Intrinsics& intrinsics,
+                         double axis_length_m,
+                         const std::string& label);
+
+Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
+                              int source_width,
+                              int source_height,
+                              int target_width,
+                              int target_height);
+
 class CameraStreamEncoder {
 public:
     static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
@@ -37,6 +55,14 @@ public:
                      int height,
                      int fps);
 
+    static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                       const cv::Mat& frame,
+                       std::vector<uint8_t>& encoded_frame,
+                       bool& is_key,
+                       const Rs2Intrinsics& intrinsics,
+                       const CameraStreamEncodeOptions& options = {});
+
+    // Compatibility overload for callers that only need timestamp drawing.
     static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
                        const cv::Mat& frame,
                        std::vector<uint8_t>& encoded_frame,
diff --git a/cmvr-es/devices/camera/common/include/camera_stream_overlay.h b/cmvr-es/devices/camera/common/include/camera_stream_overlay.h
new file mode 100644
index 00000000..7ecd6cfe
--- /dev/null
+++ b/cmvr-es/devices/camera/common/include/camera_stream_overlay.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <Eigen/Dense>
+
+namespace cmvr::device {
+
+// A pose snapshot used only by the encoded video overlay.  The transform is
+// ^C T_Frame: it maps points in the named frame into the camera frame.
+struct CoordinateFrameOverlay {
+    Eigen::Matrix4d T_C_Frame{Eigen::Matrix4d::Identity()};
+    std::string label;
+    int tag_id{-1};
+    bool valid{false};
+};
+
+struct CameraStreamOverlay {
+    bool draw_coordinate_frames{false};
+    double coordinate_axis_length_m{0.02};
+    std::vector<CoordinateFrameOverlay> coordinate_frames;
+};
+
+} // namespace cmvr::device
diff --git a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp
index c08f9f37..8efd1fb6 100644
--- a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp
+++ b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp
@@ -1,6 +1,7 @@
 #include "devices/camera/common/include/camera_stream_encoder.h"
 
 #include <chrono>
+#include <cmath>
 #include <ctime>
 #include <iomanip>
 #include <sstream>
@@ -9,6 +10,7 @@
 #include <opencv2/imgproc.hpp>
 
 #include "common/base/logging/logger.h"
+#include "devices/camera/abstract_camera.h"
 
 namespace cmvr::device {
 namespace {
@@ -46,6 +48,41 @@ void drawTimeStamp(cv::Mat& image)
     cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
 }
 
+bool projectPoint(const Eigen::Vector3d& point,
+                 const Rs2Intrinsics& intrinsics,
+                 cv::Point& pixel)
+{
+    if (!point.allFinite() || point.z() <= 1e-9 ||
+        !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
+        !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
+        intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
+        return false;
+    }
+    const double u = static_cast<double>(intrinsics.fx) * point.x() / point.z() +
+                     static_cast<double>(intrinsics.cx);
+    const double v = static_cast<double>(intrinsics.fy) * point.y() / point.z() +
+                     static_cast<double>(intrinsics.cy);
+    if (!std::isfinite(u) || !std::isfinite(v)) {
+        return false;
+    }
+    pixel = cv::Point(cvRound(u), cvRound(v));
+    return true;
+}
+
+void drawOutlinedText(cv::Mat& image,
+                      const std::string& text,
+                      const cv::Point& origin,
+                      const cv::Scalar& color)
+{
+    constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
+    constexpr double font_scale = 0.55;
+    constexpr int thickness = 1;
+    cv::putText(image, text, origin, font_face, font_scale,
+                cv::Scalar(0, 0, 0), thickness + 2, cv::LINE_AA);
+    cv::putText(image, text, origin, font_face, font_scale,
+                color, thickness, cv::LINE_AA);
+}
+
 const AVCodec* findEncoder(const std::string& codec_name)
 {
     if (codec_name == "h264" || codec_name == "H264") {
@@ -75,6 +112,77 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
 
 } // namespace
 
+void drawCoordinateFrame(cv::Mat& image,
+                         const Eigen::Matrix4d& T_C_Frame,
+                         const Rs2Intrinsics& intrinsics,
+                         const double axis_length_m,
+                         const std::string& label)
+{
+    if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() ||
+        !std::isfinite(axis_length_m) || axis_length_m <= 0.0) {
+        return;
+    }
+
+    const Eigen::Vector4d origin_h(0.0, 0.0, 0.0, 1.0);
+    const Eigen::Vector4d x_h(axis_length_m, 0.0, 0.0, 1.0);
+    const Eigen::Vector4d y_h(0.0, axis_length_m, 0.0, 1.0);
+    const Eigen::Vector4d z_h(0.0, 0.0, axis_length_m, 1.0);
+    const Eigen::Vector3d origin = (T_C_Frame * origin_h).head<3>();
+    const Eigen::Vector3d x = (T_C_Frame * x_h).head<3>();
+    const Eigen::Vector3d y = (T_C_Frame * y_h).head<3>();
+    const Eigen::Vector3d z = (T_C_Frame * z_h).head<3>();
+
+    cv::Point origin_px;
+    if (!projectPoint(origin, intrinsics, origin_px)) {
+        return;
+    }
+
+    cv::Point x_px;
+    cv::Point y_px;
+    cv::Point z_px;
+    constexpr int thickness = 2;
+    if (projectPoint(x, intrinsics, x_px)) {
+        cv::arrowedLine(image, origin_px, x_px, cv::Scalar(0, 0, 255), thickness,
+                        cv::LINE_AA, 0, 0.15);
+        drawOutlinedText(image, "X", x_px + cv::Point(4, -4), cv::Scalar(0, 0, 255));
+    }
+    if (projectPoint(y, intrinsics, y_px)) {
+        cv::arrowedLine(image, origin_px, y_px, cv::Scalar(0, 255, 0), thickness,
+                        cv::LINE_AA, 0, 0.15);
+        drawOutlinedText(image, "Y", y_px + cv::Point(4, -4), cv::Scalar(0, 255, 0));
+    }
+    if (projectPoint(z, intrinsics, z_px)) {
+        cv::arrowedLine(image, origin_px, z_px, cv::Scalar(255, 0, 0), thickness,
+                        cv::LINE_AA, 0, 0.15);
+        drawOutlinedText(image, "Z", z_px + cv::Point(4, -4), cv::Scalar(255, 0, 0));
+    }
+
+    cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255), cv::MARKER_CROSS, 9, 1,
+                   cv::LINE_AA);
+    if (!label.empty()) {
+        drawOutlinedText(image, label, origin_px + cv::Point(7, -7),
+                         cv::Scalar(255, 255, 255));
+    }
+}
+
+Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
+                              const int source_width,
+                              const int source_height,
+                              const int target_width,
+                              const int target_height)
+{
+    Rs2Intrinsics scaled = intrinsics;
+    if (source_width > 0 && source_height > 0 && target_width > 0 && target_height > 0) {
+        const float sx = static_cast<float>(target_width) / static_cast<float>(source_width);
+        const float sy = static_cast<float>(target_height) / static_cast<float>(source_height);
+        scaled.fx *= sx;
+        scaled.cx *= sx;
+        scaled.fy *= sy;
+        scaled.cy *= sy;
+    }
+    return scaled;
+}
+
 FfmpegEncoderInfo::~FfmpegEncoderInfo()
 {
     if (frame) {
@@ -189,6 +297,7 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
                                  const cv::Mat& frame,
                                  std::vector<uint8_t>& encoded_frame,
                                  bool& is_key,
+                                 const Rs2Intrinsics& intrinsics,
                                  const CameraStreamEncodeOptions& options)
 {
     encoded_frame.clear();
@@ -205,9 +314,27 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
     }
 
     cv::Mat frame_to_encode = frame;
-    if (options.draw_timestamp) {
+    if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
         frame_to_encode = frame.clone();
-        drawTimeStamp(frame_to_encode);
+        if (options.draw_timestamp) {
+            drawTimeStamp(frame_to_encode);
+        }
+        if (options.overlay.draw_coordinate_frames) {
+            for (const auto& coordinate_frame : options.overlay.coordinate_frames) {
+                if (!coordinate_frame.valid) {
+                    continue;
+                }
+                std::string label = coordinate_frame.label;
+                if (coordinate_frame.tag_id >= 0) {
+                    label += " #" + std::to_string(coordinate_frame.tag_id);
+                }
+                drawCoordinateFrame(frame_to_encode,
+                                    coordinate_frame.T_C_Frame,
+                                    intrinsics,
+                                    options.overlay.coordinate_axis_length_m,
+                                    label);
+            }
+        }
     }
 
     const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
@@ -294,4 +421,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
     return true;
 }
 
+bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                                 const cv::Mat& frame,
+                                 std::vector<uint8_t>& encoded_frame,
+                                 bool& is_key,
+                                 const CameraStreamEncodeOptions& options)
+{
+    Rs2Intrinsics intrinsics{};
+    return encode(encoder, frame, encoded_frame, is_key, intrinsics, options);
+}
+
 } // namespace cmvr::device
diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp
index fc2d728d..abf9f499 100644
--- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp
+++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp
@@ -345,12 +345,17 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
     }
 
     frame_data.rgbImage = color.clone();
+    frame_data.intrinsics = intrinsics;
     CameraStreamEncodeOptions encode_options;
     encode_options.draw_timestamp = enable_stream_timestamp_;
+    encode_options.overlay = streamOverlay();
+    const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
+        intrinsics, color.cols, color.rows, color_to_encode.cols, color_to_encode.rows);
     if (!CameraStreamEncoder::encode(rgb_encoder_,
                                      color_to_encode,
                                      frame_data.rgbFrame,
                                      frame_data.bKey,
+                                     encode_intrinsics,
                                      encode_options)) {
         return false;
     }
@@ -360,7 +365,6 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
         const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
         frame_data.depthFrame.assign(depth_begin, depth_end);
     }
-    frame_data.intrinsics = intrinsics;
     frame_data.width = color_to_encode.cols;
     frame_data.height = color_to_encode.rows;
     frame_data.fps = fps;
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 6aad3be4..71565a78 100644
--- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
+++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
@@ -772,10 +772,18 @@ void RealsenseCamera::streaming_worker_() {
             }
             CameraStreamEncodeOptions encode_options;
             encode_options.draw_timestamp = enable_stream_timestamp_;
+            encode_options.overlay = streamOverlay();
+            const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
+                frame_data.intrinsics,
+                frame_data.rgbImage.cols,
+                frame_data.rgbImage.rows,
+                rgb_to_encode.cols,
+                rgb_to_encode.rows);
             success = CameraStreamEncoder::encode(rgbEncoder_,
                                                   rgb_to_encode,
                                                   frame_data.rgbFrame,
                                                   frame_data.bKey,
+                                                  encode_intrinsics,
                                                   encode_options);
             // 深度图编码
             // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);
diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
index bccc0204..8e8be98d 100644
--- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
+++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
@@ -504,10 +504,18 @@ void UVCCamera::streaming_worker_() {
             }
             CameraStreamEncodeOptions encode_options;
             encode_options.draw_timestamp = enable_stream_timestamp_;
+            encode_options.overlay = streamOverlay();
+            const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
+                frame_data.intrinsics,
+                frame_data.rgbImage.cols,
+                frame_data.rgbImage.rows,
+                rgb_to_encode.cols,
+                rgb_to_encode.rows);
             success = CameraStreamEncoder::encode(rgbEncoder_,
                                                   rgb_to_encode,
                                                   frame_data.rgbFrame,
                                                   frame_data.bKey,
+                                                  encode_intrinsics,
                                                   encode_options);
             if (success) {
                 frame_data.fps = fps_;
diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h
index 29f726c0..6d870740 100644
--- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h
+++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h
@@ -121,6 +121,7 @@ private:
 
     void stopPbvsMotion();
     bool buildInitJointPositions(std::vector<double>& positions_out) const;
+    bool isAtInitPosition(const std::vector<double>& positions) const;
     bool moveToInitPositionBeforeStartIfEnabled();
     bool moveToInitPositionIfEnabled() const;
     bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
@@ -132,6 +133,8 @@ private:
     void enterFailed(Status status);
 
     bool updateTouchPressure();
+    void publishCoordinateOverlay();
+    void refreshCoordinateOverlay();
 
 private:
     mutable std::mutex mutex_;
@@ -171,6 +174,10 @@ private:
     Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
     Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
     double pbvs_command_acceleration_{0.25};
+    // Initialization MoveJ is skipped only when both position and velocity
+    // are within these configured limits.
+    double init_skip_position_tolerance_rad_{1e-3};
+    double init_skip_velocity_tolerance_rad_s_{1e-2};
     bool locked_target_rotation_valid_{false};
     Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
     bool touch_start_position_valid_{false};
@@ -183,6 +190,7 @@ private:
     double max_T_B_G_rotation_delta_rad_{0.0};
 
     Clock::time_point phase_start_time_{};
+    Clock::time_point last_coordinate_overlay_update_time_{};
     Clock::time_point last_retract_log_time_{};
     Status final_status_after_retract_{Status::DONE};
 };
diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
index 7e20796a..772215ca 100644
--- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
+++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
@@ -6,6 +6,7 @@
 #include <limits>
 #include <sstream>
 #include <unordered_map>
+#include <utility>
 
 #include <opencv2/imgcodecs.hpp>
 
@@ -20,6 +21,9 @@ namespace cmvr::task {
 
 namespace {
 
+// The initialization move is a safety repositioning step.  Avoid replanning
+// an already settled arm, while still requiring a stopped joint velocity so a
+// moving arm is never mistaken for being at the initial pose.
 device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix<double, 6, 1>& twist) {
     return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]};
 }
@@ -123,15 +127,19 @@ double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
     return static_cast<double>(point.fz);
 }
 
-Eigen::Matrix3d rotationFromTargetRotvec(const double rx,
+Eigen::Matrix3d rotationFromTargetEuler(const double rx,
                                         const double ry,
                                         const double rz) {
-    const Eigen::Vector3d rotation_vector(rx, ry, rz);
-    const double angle = rotation_vector.norm();
-    if (!std::isfinite(angle) || angle <= 1e-12) {
-        return Eigen::Matrix3d::Identity();
-    }
-    return Eigen::AngleAxisd(angle, rotation_vector / angle).toRotationMatrix();
+    // The configured rotations are extrinsic rotations about the fixed G axes,
+    // applied in X -> Y -> Z order.  With column-vector transforms this is
+    // represented by left multiplication in reverse order.
+    const Eigen::Matrix3d R_x =
+        Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix();
+    const Eigen::Matrix3d R_y =
+        Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix();
+    const Eigen::Matrix3d R_z =
+        Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    return R_z * R_y * R_x;
 }
 
 bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target,
@@ -282,10 +290,14 @@ bool TouchScreenTask::init() {
     }
 
     const auto& devices = config_.devices();
+    const auto& perception_config = config_.perception();
     if (!devices.has_arm_id() || devices.arm_id().empty() ||
+        !devices.has_dexhand_id() || devices.dexhand_id().empty() ||
         !devices.has_camera_id() || devices.camera_id().empty() ||
         !devices.has_external_camera_id() || devices.external_camera_id().empty() ||
-        !devices.has_dexhand_id() || devices.dexhand_id().empty()) {
+        !perception_config.has_hand_camera() ||
+        !perception_config.hand_camera().has_depth_policy() ||
+        !perception_config.hand_camera().has_target_point_method()) {
         last_status_ = Status::INVALID_CONFIG;
         return false;
     }
@@ -293,6 +305,7 @@ bool TouchScreenTask::init() {
     auto& dm = device::DeviceManager::getInstance();
     auto arm = dm.getDevice<device::RobotArm>(devices.arm_id());
     auto dexhand = dm.getDevice<device::AbstractDexHand>(devices.dexhand_id());
+    const auto& hand_camera_config = perception_config.hand_camera();
     auto camera = dm.getDevice<device::AbstractCamera>(devices.camera_id());
     auto external_camera =
         dm.getDevice<device::AbstractCamera>(devices.external_camera_id());
@@ -344,23 +357,30 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
         return false;
     }
 
+    const auto& perception_config = config_.perception();
+    const auto& tags = perception_config.tags();
+    const auto& hand_camera_config = perception_config.hand_camera();
     perception_ = std::make_shared<perception::AprilTagPerception>(camera_);
-    perception_->setTagSize(config_.perception().apriltag().tag_size_m());
+    perception_->setTagSize(tags.screen().size_m());
     external_perception_ =
         std::make_shared<perception::AprilTagPerception>(external_camera_);
-    external_perception_->setTagSize(
-        config_.perception().external_apriltag().tag_size_m());
+    external_perception_->setTagSize(tags.screen().size_m());
 
     tracker_.setPerception(perception_);
     tracker_.setTargetPointMethod(toTargetPointMethod(
-        config_.perception().apriltag().target_point_method()));
+        hand_camera_config.target_point_method()));
 
     tcp_pose_tracker_.setPerception(external_perception_);
-    tcp_pose_tracker_.setHandTagId(
-        config_.perception().external_apriltag().hand_tag_id());
+    tcp_pose_tracker_.setScreenTagId(tags.screen().id());
+    tcp_pose_tracker_.setHandTagId(tags.hand().id());
+
+    // Publish the presentation settings immediately so a gRPC stream started
+    // before the first periodic task tick uses the configured behavior.
+    publishCoordinateOverlay();
 
     initialized_ = applyConfig();
     if (initialized_) {
+        refreshCoordinateOverlay();
         tracker_.clear();
         tracker_.resetActiveTagTracking();
         tcp_pose_tracker_.clear();
@@ -385,6 +405,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
         last_T_B_G_.setIdentity();
         max_T_B_G_translation_delta_m_ = 0.0;
         max_T_B_G_rotation_delta_rad_ = 0.0;
+        last_coordinate_overlay_update_time_ = Clock::time_point{};
         last_status_ = Status::IDLE;
     } else {
         last_status_ = Status::INVALID_CONFIG;
@@ -479,6 +500,13 @@ bool TouchScreenTask::step(const double dt) {
         //           << std::endl;
     }
 
+    // Keep the presentation snapshot alive outside ALIGNING as well.  This
+    // path only updates the debug video overlay; it is not consumed by PBVS or
+    // any motion computation.
+    if (phase_ != Phase::ALIGNING) {
+        refreshCoordinateOverlay();
+    }
+
     switch (phase_) {
         case Phase::IDLE:
             last_status_ = Status::IDLE;
@@ -515,6 +543,75 @@ bool TouchScreenTask::step(const double dt) {
     return false;
 }
 
+void TouchScreenTask::publishCoordinateOverlay()
+{
+    if (!external_camera_) {
+        return;
+    }
+
+    device::CameraStreamOverlay overlay;
+    overlay.draw_coordinate_frames =
+        !config_.has_debug_draw_coordinate_frames() ||
+        config_.debug_draw_coordinate_frames();
+    overlay.coordinate_axis_length_m = 0.02;
+    if (config_.has_debug_coordinate_axis_length_m() &&
+        std::isfinite(config_.debug_coordinate_axis_length_m()) &&
+        config_.debug_coordinate_axis_length_m() > 0.0) {
+        overlay.coordinate_axis_length_m = config_.debug_coordinate_axis_length_m();
+    }
+
+    if (overlay.draw_coordinate_frames && external_perception_) {
+        const auto& tags = config_.perception().tags();
+        const int screen_tag_id = tags.screen().id();
+        const int hand_tag_id = tags.hand().id();
+        const auto* screen_tag = external_perception_->findTag(screen_tag_id);
+
+        if (screen_tag) {
+            device::CoordinateFrameOverlay frame;
+            frame.T_C_Frame = screen_tag->T_C_Tag();
+            frame.label = "G";
+            frame.tag_id = screen_tag->id;
+            frame.valid = true;
+            overlay.coordinate_frames.push_back(std::move(frame));
+        }
+
+        if (const auto* hand_tag = external_perception_->findTag(hand_tag_id)) {
+            device::CoordinateFrameOverlay frame;
+            frame.T_C_Frame = hand_tag->T_C_Tag();
+            frame.label = "H";
+            frame.tag_id = hand_tag->id;
+            frame.valid = true;
+            overlay.coordinate_frames.push_back(std::move(frame));
+        }
+    }
+
+    external_camera_->setStreamOverlay(overlay);
+}
+
+void TouchScreenTask::refreshCoordinateOverlay()
+{
+    if (!external_perception_ || !external_camera_) {
+        return;
+    }
+    if (config_.has_debug_draw_coordinate_frames() &&
+        !config_.debug_draw_coordinate_frames()) {
+        return;
+    }
+    const auto now = Clock::now();
+    if (last_coordinate_overlay_update_time_.time_since_epoch().count() != 0 &&
+        std::chrono::duration<double>(now - last_coordinate_overlay_update_time_).count() <
+            (1.0 / 30.0)) {
+        return;
+    }
+    last_coordinate_overlay_update_time_ = now;
+    perception::AprilTagPerception::Options options;
+    options.depth_policy = perception::AprilTagPerception::DepthPolicy::NONE;
+    options.detect_tags = true;
+    options.fetch_encoded = false;
+    external_perception_->update(options);
+    publishCoordinateOverlay();
+}
+
 void TouchScreenTask::stop() {
     std::lock_guard<std::mutex> lock(mutex_);
     stopUnlocked();
@@ -699,9 +796,10 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         !config.initialization().has_velocity() ||
         !config.initialization().has_acceleration() ||
         !config.has_perception() ||
-        !config.perception().has_apriltag() ||
-        !config.perception().has_external_apriltag() ||
+        !config.perception().has_tags() ||
+        !config.perception().has_hand_camera() ||
         !config.has_alignment() ||
+        !config.alignment().has_calibration() ||
         !config.alignment().has_pbvs() ||
         !config.alignment().has_target() ||
         !config.alignment().has_error_threshold() ||
@@ -714,28 +812,33 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         !config.has_retract() ||
         !config.retract().has_twist_tool() ||
         !config.retract().has_acceleration() ||
-        !config.retract().has_duration_s()) {
+        !config.retract().has_distance_m()) {
         return false;
     }
 
-    const auto& apriltag = config.perception().apriltag();
-    const auto& external_apriltag = config.perception().external_apriltag();
+    const auto& perception = config.perception();
+    const auto& tags = perception.tags();
+    const auto& hand_tag = tags.hand();
+    const auto& screen_tag = tags.screen();
+    const auto& hand_camera = perception.hand_camera();
     const auto& alignment = config.alignment();
+    const auto& calibration = alignment.calibration();
     const auto& pbvs = alignment.pbvs();
     const auto& target = alignment.target();
     const auto& touch = config.touch();
     const auto& tactile = touch.tactile();
     const auto& retract = config.retract();
 
-    if (!apriltag.has_tag_size_m() ||
-        !apriltag.has_depth_policy() ||
-        !apriltag.has_target_point_method() ||
-        !external_apriltag.has_tag_size_m() ||
-        !external_apriltag.has_hand_tag_id() ||
-        !external_apriltag.has_t_h_p() ||
-        !hasMat4(external_apriltag.t_h_p()) ||
-        !target.has_rotation_vector() ||
-        !hasVec3(target.rotation_vector()) ||
+    if (!screen_tag.has_id() ||
+        !screen_tag.has_size_m() ||
+        !hand_tag.has_id() ||
+        !hand_tag.has_size_m() ||
+        !hand_camera.has_depth_policy() ||
+        !hand_camera.has_target_point_method() ||
+        !calibration.has_hand_tag_to_tcp() ||
+        !hasMat4(calibration.hand_tag_to_tcp()) ||
+        !target.has_hand_orientation_g() ||
+        !hasEuler(target.hand_orientation_g()) ||
         !target.has_mode() ||
         !hasVec6(alignment.error_threshold()) ||
         !pbvs.has_position_gain() ||
@@ -755,8 +858,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         return false;
     }
 
-    const Eigen::Vector3d target_rotation =
-        cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
+    const Eigen::Vector3d target_orientation =
+        cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g());
     const Eigen::Vector3d position_offset_G =
         target.has_position_offset_g()
             ? cmvr::common::math::toEigenVec3(target.position_offset_g())
@@ -775,14 +878,17 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         cmvr::common::math::toEigenVec6(pbvs.vmax6());
     const Eigen::Matrix<double, 6, 1> amax6 =
         cmvr::common::math::toEigenVec6(pbvs.amax6());
-    const Eigen::Matrix4d T_H_P = toEigenMat4(external_apriltag.t_h_p());
+    const Eigen::Matrix4d T_H_P = toEigenMat4(calibration.hand_tag_to_tcp());
 
-    if (!std::isfinite(apriltag.tag_size_m()) || apriltag.tag_size_m() <= 0.0 ||
-        !std::isfinite(external_apriltag.tag_size_m()) ||
-        external_apriltag.tag_size_m() <= 0.0 ||
-        external_apriltag.hand_tag_id() < 0 ||
+    if (screen_tag.id() < 0 || hand_tag.id() < 0 ||
+        screen_tag.id() == hand_tag.id() ||
+        !std::isfinite(screen_tag.size_m()) || screen_tag.size_m() <= 0.0 ||
+        !std::isfinite(hand_tag.size_m()) || hand_tag.size_m() <= 0.0 ||
+        std::abs(screen_tag.size_m() - hand_tag.size_m()) > 1e-12 ||
+        config.devices().camera_id().empty() ||
+        config.devices().external_camera_id().empty() ||
         !isHomogeneousTransform(T_H_P) ||
-        !target_rotation.allFinite() ||
+        !target_orientation.allFinite() ||
         !position_offset_G.allFinite() ||
         !rotation_offset_G.allFinite() ||
         !position_gain.allFinite() ||
@@ -797,7 +903,7 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         !std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 ||
         !cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
         !std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 ||
-        !std::isfinite(retract.duration_s()) || retract.duration_s() < 0.0) {
+        !std::isfinite(retract.distance_m()) || retract.distance_m() <= 0.0) {
         return false;
     }
     for (int i = 0; i < 3; ++i) {
@@ -834,6 +940,18 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         }
     }
 
+    const auto& initialization = config.initialization();
+    if (initialization.has_skip_position_tolerance_rad() &&
+        (!std::isfinite(initialization.skip_position_tolerance_rad()) ||
+         initialization.skip_position_tolerance_rad() < 0.0)) {
+        return false;
+    }
+    if (initialization.has_skip_velocity_tolerance_rad_s() &&
+        (!std::isfinite(initialization.skip_velocity_tolerance_rad_s()) ||
+         initialization.skip_velocity_tolerance_rad_s() < 0.0)) {
+        return false;
+    }
+
     std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
     if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
                                        toTactileRegion(tactile.region()),
@@ -899,6 +1017,15 @@ bool TouchScreenTask::applyConfig() {
         return false;
     }
 
+    init_skip_position_tolerance_rad_ =
+        config_.initialization().has_skip_position_tolerance_rad()
+            ? config_.initialization().skip_position_tolerance_rad()
+            : 1e-3;
+    init_skip_velocity_tolerance_rad_s_ =
+        config_.initialization().has_skip_velocity_tolerance_rad_s()
+            ? config_.initialization().skip_velocity_tolerance_rad_s()
+            : 1e-2;
+
     if (dexhand_) {
         const auto& tactile = config_.touch().tactile();
         std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
@@ -914,15 +1041,18 @@ bool TouchScreenTask::applyConfig() {
         }
     }
 
-    const auto& apriltag = config_.perception().apriltag();
-    const auto& external_apriltag = config_.perception().external_apriltag();
+    const auto& perception = config_.perception();
+    const auto& tags = perception.tags();
+    const auto& hand_camera_config = perception.hand_camera();
+    const auto& calibration = config_.alignment().calibration();
     const auto& pbvs = config_.alignment().pbvs();
-    perception_->setTagSize(apriltag.tag_size_m());
-    external_perception_->setTagSize(external_apriltag.tag_size_m());
-    tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method()));
+    perception_->setTagSize(tags.screen().size_m());
+    external_perception_->setTagSize(tags.screen().size_m());
+    tracker_.setTargetPointMethod(toTargetPointMethod(hand_camera_config.target_point_method()));
     tcp_pose_tracker_.setPerception(external_perception_);
-    tcp_pose_tracker_.setHandTagId(external_apriltag.hand_tag_id());
-    T_H_P_ = toEigenMat4(external_apriltag.t_h_p());
+    tcp_pose_tracker_.setScreenTagId(tags.screen().id());
+    tcp_pose_tracker_.setHandTagId(tags.hand().id());
+    T_H_P_ = toEigenMat4(calibration.hand_tag_to_tcp());
 
     pbvs_.setPositionGain(cmvr::common::math::toEigenVec3(pbvs.position_gain()));
     pbvs_.setRotationGain(cmvr::common::math::toEigenVec3(pbvs.rotation_gain()));
@@ -950,16 +1080,17 @@ bool TouchScreenTask::applyConfig() {
                     << ", arm_id=" << config_.devices().arm_id()
                     << ", hand_camera_id=" << config_.devices().camera_id()
                     << ", external_camera_id=" << config_.devices().external_camera_id()
-                    << ", hand_camera_tag_size_m=" << apriltag.tag_size_m()
-                    << ", external_camera_tag_size_m=" << external_apriltag.tag_size_m()
-                    << ", hand_tag_id=" << external_apriltag.hand_tag_id();
+                    << ", hand_tag_size_m=" << tags.hand().size_m()
+                    << ", screen_tag_size_m=" << tags.screen().size_m()
+                    << ", hand_tag_id=" << tags.hand().id()
+                    << ", screen_tag_id=" << tags.screen().id();
     return true;
 }
 
 bool TouchScreenTask::stepAligning(const double dt) {
     const auto& alignment = config_.alignment();
-    const auto target_rotation_vector =
-        cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
+    const auto target_orientation =
+        cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g());
     const auto& target = alignment.target();
     const Eigen::Vector3d position_offset_G =
         target.has_position_offset_g()
@@ -986,7 +1117,7 @@ bool TouchScreenTask::stepAligning(const double dt) {
     if (hand_camera_needed) {
         perception::AprilTagPerception::Options hand_options;
         hand_options.depth_policy =
-            toDepthPolicy(config_.perception().apriltag().depth_policy());
+            toDepthPolicy(config_.perception().hand_camera().depth_policy());
         hand_options.detect_tags = true;
         hand_options.fetch_encoded = false;
         hand_perception_ok = perception_->update(hand_options);
@@ -1048,7 +1179,8 @@ bool TouchScreenTask::stepAligning(const double dt) {
 
     const int tag_id = tracker_.activeTagId();
     last_active_tag_id_ = tag_id;
-    if (tag_id < 0) {
+    const int screen_tag_id = config_.perception().tags().screen().id();
+    if (tag_id != screen_tag_id) {
         stopPbvsMotion();
         align_stable_count_ = 0;
         last_status_ = Status::ALIGN_WAITING_TRACK;
@@ -1068,13 +1200,16 @@ bool TouchScreenTask::stepAligning(const double dt) {
     external_options.detect_tags = true;
     external_options.fetch_encoded = false;
     if (!external_perception_->update(external_options)) {
+        publishCoordinateOverlay();
         stopPbvsMotion();
         align_stable_count_ = 0;
         last_status_ = Status::ALIGN_WAITING_PERCEPTION;
         return true;
     }
 
-    tcp_pose_tracker_.setScreenTagId(tag_id);
+    publishCoordinateOverlay();
+
+    tcp_pose_tracker_.setScreenTagId(screen_tag_id);
     if (!tcp_pose_tracker_.update(T_H_P_)) {
         if (align_debug_count_ == 0 && !external_perception_->color().empty()) {
             cv::imwrite("/tmp/cmvr_external_camera_perception.png",
@@ -1104,9 +1239,14 @@ bool TouchScreenTask::stepAligning(const double dt) {
     }
 
     const Eigen::Matrix4d& T_G_P = tcp_pose_tracker_.T_G_P();
-    Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(),
-                                                        target_rotation_vector.y(),
-                                                        target_rotation_vector.z());
+    const Eigen::Matrix3d R_G_H_target =
+        rotationFromTargetEuler(target_orientation.x(),
+                                target_orientation.y(),
+                                target_orientation.z());
+    // PBVS controls P, while hand_orientation_G configures H.  Convert the
+    // configured target through the fixed hand-tag-to-TCP calibration.
+    Eigen::Matrix3d R_target =
+        R_G_H_target * T_H_P_.block<3, 3>(0, 0);
     if (rotation_offset_G.norm() > 1e-12) {
         const double offset_angle = rotation_offset_G.norm();
         R_target = Eigen::AngleAxisd(
@@ -1406,37 +1546,34 @@ bool TouchScreenTask::stepRetracting() {
 
     const auto now = Clock::now();
     const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
-    const double retract_duration_s = config_.retract().duration_s();
-    if (elapsed < retract_duration_s) {
+    const double retract_distance_m = config_.retract().distance_m();
+    Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
+    if (!retract_start_position_valid_ ||
+        !readCurrentTouchPointPositionBase(current_position_base)) {
+        try {
+            arm_->stopL();
+        } catch (...) {
+        }
+        enterFailed(Status::ROBOT_STATE_FAILED);
+        return false;
+    }
+
+    const Eigen::Vector3d delta_base =
+        current_position_base - retract_start_position_base_;
+    const double traveled_distance_m = delta_base.norm();
+    if (traveled_distance_m < retract_distance_m) {
         if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) {
             last_retract_log_time_ = now;
             const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
-            Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
-            const bool have_current = readCurrentTouchPointPositionBase(current_position_base);
-            const bool have_delta = retract_start_position_valid_ && have_current;
-            Eigen::Vector3d delta_base = Eigen::Vector3d::Zero();
-            if (have_delta) {
-                delta_base = current_position_base - retract_start_position_base_;
-            }
-            if (have_delta) {
-                CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
-                               << "/" << retract_duration_s
-                               << ", cmd_base=[" << cmd_base.vx << ", "
-                               << cmd_base.vy << ", " << cmd_base.vz << ", "
-                               << cmd_base.wx << ", " << cmd_base.wy << ", "
-                               << cmd_base.wz << "]"
-                               << ", tcp_delta_base=[" << delta_base.x() << ", "
-                               << delta_base.y() << ", " << delta_base.z()
-                               << "], tcp_dist=" << delta_base.norm();
-            } else {
-                CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
-                               << "/" << retract_duration_s
-                               << ", cmd_base=[" << cmd_base.vx << ", "
-                               << cmd_base.vy << ", " << cmd_base.vz << ", "
-                               << cmd_base.wx << ", " << cmd_base.wy << ", "
-                               << cmd_base.wz << "]"
-                               << ", tcp_delta_base=unavailable";
-            }
+            CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
+                           << ", distance=" << traveled_distance_m
+                           << "/" << retract_distance_m
+                           << ", cmd_base=[" << cmd_base.vx << ", "
+                           << cmd_base.vy << ", " << cmd_base.vz << ", "
+                           << cmd_base.wx << ", " << cmd_base.wy << ", "
+                           << cmd_base.wz << "]"
+                           << ", tcp_delta_base=[" << delta_base.x() << ", "
+                           << delta_base.y() << ", " << delta_base.z() << "]";
         }
         last_status_ = Status::RETRACTING;
         return true;
@@ -1451,19 +1588,23 @@ bool TouchScreenTask::stepRetracting() {
     }
     if (have_final_delta) {
         CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
+                       << ", target_distance_m=" << retract_distance_m
                        << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
                        << ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
                        << final_delta_base.y() << ", " << final_delta_base.z()
                        << "], final_tcp_dist=" << final_delta_base.norm();
     } else {
         CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
+                       << ", target_distance_m=" << retract_distance_m
                        << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
                        << ", final_tcp_delta_base=unavailable";
     }
 
-    try {
-        arm_->stopL();
-    } catch (...) {
+    // Stop the retract worker synchronously before switching to the final
+    // joint-position trajectory.  stopL() only requests deceleration and can
+    // return while the worker still owns the velocity-control mode.
+    const auto stop_result = arm_->stopMotion();
+    if (!stop_result.ok()) {
         enterFailed(Status::ROBOT_COMMAND_FAILED);
         return false;
     }
@@ -1545,6 +1686,27 @@ bool TouchScreenTask::buildInitJointPositions(std::vector<double>& positions_out
         config_, arm_->getRobotModel().joint_names, positions_out);
 }
 
+bool TouchScreenTask::isAtInitPosition(const std::vector<double>& positions) const {
+    if (!arm_ || arm_->busy()) {
+        return false;
+    }
+
+    const auto state = arm_->getJointState();
+    if (state.position.size() != positions.size() ||
+        state.velocity.size() != positions.size()) {
+        return false;
+    }
+
+    for (std::size_t i = 0; i < positions.size(); ++i) {
+        if (!std::isfinite(state.position[i]) || !std::isfinite(state.velocity[i]) ||
+            std::abs(state.position[i] - positions[i]) > init_skip_position_tolerance_rad_ ||
+            std::abs(state.velocity[i]) > init_skip_velocity_tolerance_rad_s_) {
+            return false;
+        }
+    }
+    return true;
+}
+
 bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
     if (!config_.initialization().before_start()) {
         return true;
@@ -1560,6 +1722,12 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
         return false;
     }
 
+    if (isAtInitPosition(init_positions)) {
+        CMVR_LOG(INFO) << "[TouchScreenTask] initial position already reached; skipping moveJ"
+                       << ", arm=" << id_;
+        return true;
+    }
+
     device::JointPositionCommand init_cmd{init_positions};
     device::MotionOptions options;
     options.velocity = config_.initialization().velocity();
@@ -1581,6 +1749,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
         return false;
     }
 
+    if (isAtInitPosition(init_positions)) {
+        CMVR_LOG(INFO) << "[TouchScreenTask] final initial position already reached; skipping moveJ"
+                       << ", arm=" << id_;
+        return true;
+    }
+
     device::JointPositionCommand init_cmd{init_positions};
     device::MotionOptions options;
     options.velocity = config_.initialization().velocity();
@@ -1688,25 +1862,20 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
 
     const auto& retract = config_.retract();
     retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_);
+    if (!retract_start_position_valid_) {
+        CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] cannot read TCP start pose";
+        return false;
+    }
     const auto retract_cmd = toCartesianVelocity(
         cmvr::common::math::toEigenVec6(retract.twist_tool()));
-    if (retract_start_position_valid_) {
-        CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
-                       << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
-                       << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
-                       << "], acceleration=" << retract.acceleration()
-                       << ", duration_s=" << retract.duration_s()
-                       << ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
-                       << retract_start_position_base_.y() << ", "
-                       << retract_start_position_base_.z() << "]";
-    } else {
-        CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
-                       << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
-                       << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
-                       << "], acceleration=" << retract.acceleration()
-                       << ", duration_s=" << retract.duration_s()
-                       << ", start_tcp_base=unavailable";
-    }
+    CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
+                   << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
+                   << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
+                   << "], acceleration=" << retract.acceleration()
+                   << ", distance_m=" << retract.distance_m()
+                   << ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
+                   << retract_start_position_base_.y() << ", "
+                   << retract_start_position_base_.z() << "]";
 
     const auto result = arm_->speedL(retract_cmd,
                                      retract.acceleration(),
diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp
index f848a792..8e815968 100644
--- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp
+++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp
@@ -27,6 +27,7 @@
 #include "cmvr/config/motor_config/motor_config.pb.h"
 #include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
 #include "devices/arm/robot_arm_factory.h"
+#include "devices/camera/common/include/camera_stream_encoder.h"
 #include "devices/camera/mujoco_camera/include/mujoco_camera.h"
 #include "devices/motor/manager/include/motor_manager.h"
 #include "manager/device_manager/include/device_manager.h"
@@ -76,6 +77,7 @@ struct TagCenterPixel {
 };
 
 bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& camera,
+                          const int target_tag_id,
                           const double tag_size_m,
                           const std::chrono::milliseconds timeout,
                           TagCenterPixel& pixel_out)
@@ -105,6 +107,9 @@ bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& c
             TagCenterPixel best_pixel;
 
             for (const auto& tag : perception.tags()) {
+                if (tag.id != target_tag_id) {
+                    continue;
+                }
                 const Eigen::Vector3d p_c_tag_center = tag.T_c_t.block<3, 1>(0, 3);
                 Eigen::Vector2d uv = Eigen::Vector2d::Zero();
                 if (!cmvr::ImageProcess::projectCameraPointToPixel(
@@ -266,8 +271,25 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
         const auto& config = root_config.touch_screen_task();
         EXPECT_TRUE(config.has_initialization());
         EXPECT_TRUE(config.has_perception());
-        EXPECT_TRUE(config.perception().has_external_apriltag());
+        ASSERT_TRUE(config.perception().has_tags());
+        EXPECT_EQ(config.perception().tags().screen().id(), 1);
+        EXPECT_EQ(config.perception().tags().hand().id(), 0);
+        EXPECT_DOUBLE_EQ(config.perception().tags().screen().size_m(), 0.03);
+        EXPECT_DOUBLE_EQ(config.perception().tags().hand().size_m(), 0.03);
+        EXPECT_TRUE(config.perception().has_hand_camera());
+        const bool is_mujoco = std::string(file_name).find("mujoco") != std::string::npos;
+        EXPECT_EQ(config.devices().camera_id(),
+                  is_mujoco ? "mujoco_hand_cam" : "right_hand_cam");
+        EXPECT_EQ(config.devices().external_camera_id(),
+                  is_mujoco ? "mujoco_external_touch_cam" : "cam5");
         EXPECT_TRUE(config.has_alignment());
+        ASSERT_TRUE(config.alignment().has_calibration());
+        EXPECT_TRUE(config.alignment().calibration().has_hand_tag_to_tcp());
+        ASSERT_TRUE(config.alignment().target().has_hand_orientation_g());
+        EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rx(), 0.0);
+        EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().ry(), 0.0);
+        EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rz(),
+                         3.141592653589793);
         ASSERT_TRUE(config.alignment().has_pbvs());
         const auto& pbvs = config.alignment().pbvs();
         EXPECT_DOUBLE_EQ(pbvs.position_gain().x(), 2.0);
@@ -279,9 +301,38 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
         EXPECT_EQ(config.touch().motion_case(),
                   cmvr::config::TouchScreenTaskTouchConfig::kSpeedL);
         EXPECT_TRUE(config.has_retract());
+        EXPECT_TRUE(config.retract().has_distance_m());
+        EXPECT_GT(config.retract().distance_m(), 0.0);
     }
 }
 
+TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) {
+    cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3);
+    cmvr::device::Rs2Intrinsics intrinsics{};
+    intrinsics.fx = 200.0F;
+    intrinsics.fy = 200.0F;
+    intrinsics.cx = 160.0F;
+    intrinsics.cy = 120.0F;
+
+    Eigen::Matrix4d T_C_Frame = Eigen::Matrix4d::Identity();
+    T_C_Frame(2, 3) = 1.0;
+    cmvr::device::drawCoordinateFrame(image,
+                                      T_C_Frame,
+                                      intrinsics,
+                                      0.2,
+                                      "G");
+
+    // X projects right in red and Y projects down in green for the camera
+    // convention used by the pinhole projection. Z is blue but projects onto
+    // the origin for this fronto-parallel pose.
+    const cv::Vec3b x_pixel = image.at<cv::Vec3b>(120, 180);
+    const cv::Vec3b y_pixel = image.at<cv::Vec3b>(140, 160);
+    EXPECT_GT(x_pixel[2], x_pixel[1]);
+    EXPECT_GT(x_pixel[2], x_pixel[0]);
+    EXPECT_GT(y_pixel[1], y_pixel[2]);
+    EXPECT_GT(y_pixel[1], y_pixel[0]);
+}
+
 TEST(TouchScreenTaskTest, RunEyeToHandTouchInMujoco) {
     const auto project_root = findProjectRoot();
     ASSERT_FALSE(project_root.empty());
@@ -696,7 +747,8 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
     std::thread scenario([&] {
         try {
             const auto touch_config = loadMujocoTouchConfig(project_root);
-            device_manager.registerDevice(touch_config.devices().camera_id(), camera);
+            device_manager.registerDevice(
+                touch_config.devices().camera_id(), camera);
 
             cmvr::config::TaskManagerConfig task_manager_config;
             auto* task_entry = task_manager_config.add_tasks();
@@ -718,7 +770,8 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
 
             TagCenterPixel target_pixel;
             if (!detectTagCenterPixel(camera,
-                                      touch_config.perception().apriltag().tag_size_m(),
+                                      touch_config.perception().tags().screen().id(),
+                                      touch_config.perception().tags().screen().size_m(),
                                       std::chrono::seconds(3),
                                       target_pixel)) {
                 throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel");
diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto
index 06c0c33b..3db2c23a 100644
--- a/protos/cmvr/config/arm_config/arm_config.proto
+++ b/protos/cmvr/config/arm_config/arm_config.proto
@@ -68,6 +68,8 @@ message CartesianVelocityControllerConfig {
   double stop_command_velocity_norm = 3;
   double stop_measured_velocity_norm = 4;
   double stop_acceleration = 5;
+  // 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+  optional double stop_timeout_s = 6;
 }
 
 message ToppraJointMotionPlannerConfig {
@@ -81,6 +83,15 @@ message MoveJConfig {
   oneof algorithm {
     ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1;
   }
+
+  // MoveJ 轨迹发送完成后，等待关节实际状态稳定的最长时间，单位为秒。
+  optional double settle_timeout_s = 2;
+  // MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+  optional double settle_position_tolerance_rad = 3;
+  // MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+  optional double settle_velocity_tolerance_rad_s = 4;
+  // 位置和速度连续满足条件的采样次数。
+  optional int32 settle_stable_sample_count = 5;
 }
 
 message MoveLPlannerConfig {
diff --git a/protos/cmvr/config/joint_limits_config.proto b/protos/cmvr/config/joint_limits_config.proto
index c3da751b..268c05af 100644
--- a/protos/cmvr/config/joint_limits_config.proto
+++ b/protos/cmvr/config/joint_limits_config.proto
@@ -33,7 +33,6 @@ message JointLimitAvoidanceConfig {
   double gain = 2;
   double margin_ratio = 3;
   double max_push = 4;
-  double weight = 5;
 }
 
 message JointLimitPolicyConfig {
diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto
index ea977381..5ea1823e 100644
--- a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto
+++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto
@@ -15,10 +15,10 @@ message TouchScreenTaskDevicesConfig {
   reserved 1;
   reserved "robot_id";
 
-  optional string arm_id = 4;
-  optional string dexhand_id = 2;
   optional string camera_id = 3;
+  optional string arm_id = 4;
   optional string external_camera_id = 5;
+  optional string dexhand_id = 2;
 }
 
 message TouchScreenTaskInitializationConfig {
@@ -27,26 +27,56 @@ message TouchScreenTaskInitializationConfig {
   repeated TouchScreenInitJointPoint joint_positions = 3;
   optional double velocity = 4;
   optional double acceleration = 5;
+  // 当前关节位置误差小于该值时可跳过初始化 MoveJ，单位为弧度。
+  optional double skip_position_tolerance_rad = 6;
+  // 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+  optional double skip_velocity_tolerance_rad_s = 7;
 }
 
-message TouchScreenTaskExternalApriltagConfig {
-  optional double tag_size_m = 1;
-  optional int32 hand_tag_id = 2;
-  .cmvr.common.Mat4 t_h_p = 3;
+message TouchScreenTaskTagConfig {
+  optional int32 id = 1;
+  optional double size_m = 2;
+}
+
+message TouchScreenTaskTagsConfig {
+  TouchScreenTaskTagConfig screen = 1;
+  TouchScreenTaskTagConfig hand = 2;
+}
+
+message TouchScreenTaskHandCameraConfig {
+  reserved 1;
+  reserved "camera_id";
+  optional TouchScreenDepthPolicy depth_policy = 2;
+  optional TouchScreenTargetPointMethod target_point_method = 3;
 }
 
 message TouchScreenTaskPerceptionConfig {
-  TouchScreenApriltagConfig apriltag = 1;
-  TouchScreenTaskExternalApriltagConfig external_apriltag = 2;
+  reserved 1, 2, 6;
+  reserved "apriltag", "external_apriltag", "external_camera";
+
+  TouchScreenTaskTagsConfig tags = 4;
+  TouchScreenTaskHandCameraConfig hand_camera = 5;
 }
 
 message TouchScreenAlignmentTargetConfig {
-  .cmvr.common.Vec3 rotation_vector = 2;
+  reserved 2;
+  reserved "rotation_vector";
+
+  // Target orientation of Hand Tag H relative to Screen Tag G.
+  // rx, ry and rz are fixed-axis (extrinsic) rotations about G.X, G.Y and G.Z,
+  // applied in that order, in radians.
+  .cmvr.common.Euler hand_orientation_G = 6;
   optional TouchScreenAlignMode mode = 3;
   .cmvr.common.Vec3 position_offset_G = 4;
   .cmvr.common.Vec3 rotation_offset_G = 5;
 }
 
+message TouchScreenTaskAlignmentCalibrationConfig {
+  .cmvr.common.Mat4 hand_tag_to_tcp = 1;
+  reserved 2;
+  reserved "external_camera_to_base";
+}
+
 message TouchScreenTaskAlignmentConfig {
   reserved 1;
   reserved "kinematics";
@@ -56,6 +86,7 @@ message TouchScreenTaskAlignmentConfig {
   optional double timeout_s = 6;
   optional bool pause_when_reached = 7;
   TouchScreenTaskPbvsConfig pbvs = 8;
+  TouchScreenTaskAlignmentCalibrationConfig calibration = 9;
 }
 
 message TouchScreenTouchSpeedLConfig {
@@ -93,7 +124,10 @@ message TouchScreenTaskTouchConfig {
 message TouchScreenTaskRetractConfig {
   .cmvr.common.Vec6 twist_tool = 1;
   optional double acceleration = 2;
-  optional double duration_s = 3;
+  reserved 3;
+  reserved "duration_s";
+  // Distance traveled by the TCP before the retract motion stops, in meters.
+  optional double distance_m = 4;
 }
 
 message TouchScreenTaskConfig {
@@ -104,6 +138,10 @@ message TouchScreenTaskConfig {
   TouchScreenTaskTouchConfig touch = 5;
   TouchScreenTaskRetractConfig retract = 6;
   optional string id = 7;
+  // 是否在外部相机编码后的 gRPC 视频流中绘制坐标系，仅影响显示帧；未配置时默认开启。
+  optional bool debug_draw_coordinate_frames = 8;
+  // G/H 坐标轴长度，单位为米。
+  optional double debug_coordinate_axis_length_m = 9;
 }
 
 message TouchScreenTaskRootConfig {