Merge branch 'lgv_dev_touch_v2' into lgv_dev

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
+

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -12,7 +12,7 @@ add_subdirectory(arm_control)
 #)  其他动态库类似
 file(GLOB SRC
         ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pbvs/src/pbvs_controller.cpp
 
 )
 
@@ -58,3 +58,13 @@ target_link_libraries(controller PUBLIC
 add_library(cmvr_es::algorithms::controller ALIAS controller)
 
 install(TARGETS controller LIBRARY DESTINATION lib)
+
+add_executable(pbvs_controller_test
+        pbvs/src/pbvs_controller_test.cpp
+)
+
+target_link_libraries(pbvs_controller_test PRIVATE
+        cmvr_es::algorithms::controller
+        gtest
+        gtest_main
+)

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:

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();
 }

cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h

@@ -0,0 +1,453 @@
+#pragma once
+
+#ifndef CMVR_PBVS_CONTROLLER_H
+#define CMVR_PBVS_CONTROLLER_H
+
+#include <array>
+
+#include <Eigen/Dense>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr {
+
+/**
+ * @brief 基于相对 3D 位姿的 PBVS 控制器。
+ *
+ * 坐标系：
+ *
+ *   G : Screen Tag 坐标系，同时作为屏幕参考坐标系
+ *   P : TCP / 触控点坐标系
+ *
+ * 输入：
+ *
+ *   ^G T_P_des : TCP 相对于 Screen Tag 的目标位姿
+ *   ^G T_P_cur : TCP 相对于 Screen Tag 的当前位姿
+ *
+ * 输出：
+ *
+ *   ^G V_P =
+ *
+ *   [ vx ]
+ *   [ vy ]
+ *   [ vz ]
+ *   [ wx ]
+ *   [ wy ]
+ *   [ wz ]
+ *
+ * 即 TCP 在 Screen Tag 坐标系 G 下表达的 6D Cartesian Twist。
+ *
+ * 位置误差：
+ *
+ *   e_p = p_des - p_cur
+ *
+ * 姿态误差：
+ *
+ *   R_err = R_des * R_cur^T
+ *
+ *   e_R = Log(R_err)^vee
+ *
+ * 控制律：
+ *
+ *   v = Kp * e_p
+ *   w = Kr * e_R
+ *
+ * 本类只负责视觉伺服 6D Twist 计算，不负责：
+ *
+ * - AprilTag 检测
+ * - RobotArm
+ * - G -> Base 坐标转换
+ * - IK
+ * - speedL 下发
+ */
+class PbvsController {
+public:
+    enum class ComputeStatus {
+        OK = 0,
+        TARGET_NOT_SET,
+        INVALID_INPUT,
+        INVALID_DT,
+        INVALID_TARGET,
+        INVALID_CURRENT_POSE
+    };
+
+    struct Output {
+        // 当前位置误差，单位 m
+        Eigen::Vector3d position_error_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 当前姿态误差 rotation-vector，单位 rad
+        Eigen::Vector3d rotation_error_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // PBVS 计算得到的原始线速度，单位 m/s
+        Eigen::Vector3d raw_linear_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // PBVS 计算得到的原始角速度，单位 rad/s
+        Eigen::Vector3d raw_angular_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 经过限幅 / 加速度 / 滤波后的线速度
+        Eigen::Vector3d linear_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 经过限幅 / 加速度 / 滤波后的角速度
+        Eigen::Vector3d angular_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 可直接取出的 CartesianVelocity。
+        //
+        // 注意：
+        // 这里仍然是在 G / ScreenTag frame 下表达。
+        device::CartesianVelocity twist_G{};
+
+        bool position_reached{false};
+        bool orientation_reached{false};
+        bool reached{false};
+
+        bool valid{false};
+    };
+
+public:
+    PbvsController();
+
+    /**
+     * @brief 设置完整目标位姿。
+     *
+     * @param T_G_P_des TCP(P) 相对于 ScreenTag(G) 的目标位姿。
+     */
+    bool setTargetPose(
+        const Eigen::Matrix4d& T_G_P_des);
+
+    /**
+     * @brief 使用目标位置 + 目标姿态设置期望位姿。
+     */
+    bool setTargetPose(
+        const Eigen::Vector3d& position_G,
+        const Eigen::Matrix3d& rotation_G_P);
+
+    /**
+     * @brief 当前是否已经设置有效目标。
+     */
+    bool hasTarget() const {
+        return target_valid_;
+    }
+
+    /**
+     * @brief 获取目标位姿。
+     */
+    const Eigen::Matrix4d& targetPose() const {
+        return T_G_P_des_;
+    }
+
+    /**
+     * @brief 根据当前 TCP 位姿计算 PBVS 速度命令。
+     *
+     * @param T_G_P_cur 当前 TCP 相对于 Screen Tag 的位姿。
+     * @param dt        控制周期，单位 s。
+     * @param output    输出结果。
+     *
+     * @return 成功返回 true。
+     */
+    bool compute(
+        const Eigen::Matrix4d& T_G_P_cur,
+        double dt,
+        Output& output);
+
+    /**
+     * @brief 便捷接口，只输出 CartesianVelocity。
+     *
+     * 注意输出仍然是 G frame。
+     */
+    bool compute(
+        const Eigen::Matrix4d& T_G_P_cur,
+        double dt,
+        device::CartesianVelocity& twist_G_out);
+
+    /**
+     * @brief 设置位置比例增益。
+     *
+     * x/y/z 分别控制。
+     */
+    void setPositionGain(
+        const Eigen::Vector3d& kp);
+
+    /**
+     * @brief 设置姿态比例增益。
+     *
+     * rx/ry/rz 分别控制。
+     */
+    void setRotationGain(
+        const Eigen::Vector3d& kr);
+
+    /**
+     * @brief 设置六维速度上限。
+     *
+     * [vx vy vz wx wy wz]
+     *
+     * 前三维 m/s；
+     * 后三维 rad/s。
+     */
+    void setVelocityLimit6(
+        const std::array<double, 6>& vmax6);
+
+    /**
+     * @brief 设置六维加速度限制。
+     *
+     * 前三维 m/s^2；
+     * 后三维 rad/s^2。
+     *
+     * <= 0 表示对应维度不限制。
+     */
+    void setAccelerationLimit6(
+        const std::array<double, 6>& amax6);
+
+    /**
+     * @brief 设置六维误差阈值。
+     *
+     * [x y z rx ry rz]
+     *
+     * 前三维 m；
+     * 后三维 rad。
+     */
+    void setTolerance6(
+        const std::array<double, 6>& tolerance6);
+
+    /**
+     * @brief 设置一阶低通 alpha。
+     *
+     * alpha = 1：
+     * 不滤波。
+     *
+     * 0 < alpha < 1：
+     *
+     * cmd =
+     * alpha * current
+     * +
+     * (1-alpha) * previous
+     */
+    void setTwistFilterAlpha(double alpha);
+
+    /**
+     * @brief 是否启用每个轴。
+     *
+     * 默认 6DoF 全部开启。
+     *
+     * 例如以后如果不希望控制 yaw：
+     *
+     * enabled[5] = false;
+     */
+    void setAxisEnabled(
+        const std::array<bool, 6>& enabled);
+
+    /**
+     * @brief 清空速度历史，但保留目标。
+     */
+    void resetTwistCommandState();
+
+    /**
+     * @brief 清空整个 PBVS 状态和目标。
+     */
+    void reset();
+
+    ComputeStatus lastComputeStatus() const {
+        return last_compute_status_;
+    }
+
+    static const char* statusToString(
+        ComputeStatus status);
+
+    const Eigen::Vector3d& lastPositionError() const {
+        return last_position_error_G_;
+    }
+
+    const Eigen::Vector3d& lastRotationError() const {
+        return last_rotation_error_G_;
+    }
+
+    const Eigen::Vector3d& positionGain() const {
+        return kp_position_;
+    }
+
+    const Eigen::Vector3d& rotationGain() const {
+        return kp_rotation_;
+    }
+
+    const std::array<double, 6>& velocityLimit6() const {
+        return vmax6_;
+    }
+
+    const std::array<double, 6>& accelerationLimit6() const {
+        return amax6_;
+    }
+
+    const std::array<double, 6>& tolerance6() const {
+        return tolerance6_;
+    }
+
+    double twistFilterAlpha() const {
+        return twist_lpf_alpha_;
+    }
+
+    const Eigen::Matrix<double, 6, 1>&
+    lastTwistCommandG() const {
+        return last_twist_cmd_G_;
+    }
+
+    bool lastReached() const {
+        return last_reached_;
+    }
+
+private:
+    static bool isFiniteTransform(
+        const Eigen::Matrix4d& T);
+
+    static bool hasValidBottomRow(
+        const Eigen::Matrix4d& T,
+        double tolerance = 1e-6);
+
+    static bool isValidTransform(
+        const Eigen::Matrix4d& T);
+
+    /**
+     * @brief 将可能有微小数值误差的旋转矩阵投影到 SO(3)。
+     */
+    static Eigen::Matrix3d projectToSO3(
+        const Eigen::Matrix3d& R);
+
+    /**
+     * @brief 计算空间旋转误差，在 G frame 表达。
+     *
+     * R_err = R_des * R_cur^T
+     *
+     * e_R = Log(R_err)^vee
+     */
+    static Eigen::Vector3d rotationError(
+        const Eigen::Matrix3d& R_des,
+        const Eigen::Matrix3d& R_cur);
+
+    static double clampValue(
+        double value,
+        double lower,
+        double upper);
+
+    static device::CartesianVelocity
+    toCartesianVelocity(
+        const Eigen::Matrix<double, 6, 1>& twist);
+
+private:
+    // ---------------- target ----------------
+
+    Eigen::Matrix4d T_G_P_des_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    bool target_valid_{false};
+
+    // ---------------- gains ----------------
+
+    Eigen::Vector3d kp_position_{
+        2.0,
+        2.0,
+        1.5
+    };
+
+    Eigen::Vector3d kp_rotation_{
+        1.5,
+        1.5,
+        1.5
+    };
+
+    // ---------------- velocity limits ----------------
+
+    //
+    // [vx vy vz wx wy wz]
+    //
+    std::array<double, 6> vmax6_{{
+        0.10,
+        0.10,
+        0.05,
+        0.50,
+        0.50,
+        0.50
+    }};
+
+    // ---------------- acceleration limits ----------------
+
+    std::array<double, 6> amax6_{{
+        0.50,
+        0.50,
+        0.30,
+        2.0,
+        2.0,
+        2.0
+    }};
+
+    // ---------------- tolerance ----------------
+
+    std::array<double, 6> tolerance6_{{
+        0.0015,  // x  1.5 mm
+        0.0015,  // y  1.5 mm
+        0.0020,  // z  2.0 mm
+
+        0.05,    // rx ~2.9 deg
+        0.05,    // ry
+        0.05     // rz
+    }};
+
+    // ---------------- axis enable ----------------
+
+    std::array<bool, 6> axis_enabled_{{
+        true,
+        true,
+        true,
+        true,
+        true,
+        true
+    }};
+
+    // ---------------- LPF ----------------
+
+    double twist_lpf_alpha_{1.0};
+
+    // ---------------- command history ----------------
+
+    Eigen::Matrix<double, 6, 1>
+        previous_twist_cmd_G_{
+            Eigen::Matrix<double, 6, 1>::Zero()
+        };
+
+    bool has_previous_twist_{false};
+
+    // ---------------- last output ----------------
+
+    Eigen::Vector3d last_position_error_G_{
+        Eigen::Vector3d::Zero()
+    };
+
+    Eigen::Vector3d last_rotation_error_G_{
+        Eigen::Vector3d::Zero()
+    };
+
+    Eigen::Matrix<double, 6, 1>
+        last_twist_cmd_G_{
+            Eigen::Matrix<double, 6, 1>::Zero()
+        };
+
+    bool last_reached_{false};
+
+    ComputeStatus last_compute_status_{
+        ComputeStatus::TARGET_NOT_SET
+    };
+};
+
+} // namespace cmvr
+
+#endif // CMVR_PBVS_CONTROLLER_H

cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp

@@ -0,0 +1,774 @@
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+#include <Eigen/SVD>
+
+namespace cmvr {
+
+PbvsController::PbvsController()
+{
+    resetTwistCommandState();
+}
+
+bool PbvsController::setTargetPose(
+    const Eigen::Matrix4d& T_G_P_des)
+{
+    if (!isValidTransform(T_G_P_des)) {
+        target_valid_ = false;
+        last_compute_status_ =
+            ComputeStatus::INVALID_TARGET;
+        return false;
+    }
+
+    T_G_P_des_ = T_G_P_des;
+
+    //
+    // 视觉位姿可能有微小数值误差。
+    // 强制把 rotation 投影到 SO(3)。
+    //
+    T_G_P_des_.block<3, 3>(0, 0) =
+        projectToSO3(
+            T_G_P_des.block<3, 3>(0, 0));
+
+    target_valid_ = true;
+    last_compute_status_ =
+        ComputeStatus::OK;
+
+    return true;
+}
+
+bool PbvsController::setTargetPose(
+    const Eigen::Vector3d& position_G,
+    const Eigen::Matrix3d& rotation_G_P)
+{
+    if (!position_G.allFinite() ||
+        !rotation_G_P.allFinite()) {
+
+        target_valid_ = false;
+        last_compute_status_ =
+            ComputeStatus::INVALID_TARGET;
+
+        return false;
+    }
+
+    Eigen::Matrix4d T =
+        Eigen::Matrix4d::Identity();
+
+    T.block<3, 3>(0, 0) =
+        projectToSO3(rotation_G_P);
+
+    T.block<3, 1>(0, 3) =
+        position_G;
+
+    return setTargetPose(T);
+}
+
+bool PbvsController::compute(
+    const Eigen::Matrix4d& T_G_P_cur,
+    const double dt,
+    Output& output)
+{
+    output = Output{};
+
+    last_reached_ = false;
+    last_position_error_G_.setZero();
+    last_rotation_error_G_.setZero();
+    last_twist_cmd_G_.setZero();
+
+    if (!target_valid_) {
+        last_compute_status_ =
+            ComputeStatus::TARGET_NOT_SET;
+
+        return false;
+    }
+
+    if (!std::isfinite(dt) ||
+        dt <= 0.0) {
+
+        last_compute_status_ =
+            ComputeStatus::INVALID_DT;
+
+        return false;
+    }
+
+    if (!isValidTransform(T_G_P_cur)) {
+        last_compute_status_ =
+            ComputeStatus::INVALID_CURRENT_POSE;
+
+        return false;
+    }
+
+    // =====================================================
+    // 1. Current pose
+    // =====================================================
+
+    const Eigen::Vector3d p_cur_G =
+        T_G_P_cur.block<3, 1>(0, 3);
+
+    const Eigen::Matrix3d R_cur_G_P =
+        projectToSO3(
+            T_G_P_cur.block<3, 3>(0, 0));
+
+    // =====================================================
+    // 2. Desired pose
+    // =====================================================
+
+    const Eigen::Vector3d p_des_G =
+        T_G_P_des_.block<3, 1>(0, 3);
+
+    const Eigen::Matrix3d R_des_G_P =
+        T_G_P_des_.block<3, 3>(0, 0);
+
+    // =====================================================
+    // 3. Position error
+    //
+    // e_p = p_des - p_cur
+    //
+    // expressed in G frame.
+    // =====================================================
+
+    Eigen::Vector3d e_pos_G =
+        p_des_G -
+        p_cur_G;
+
+    // =====================================================
+    // 4. Rotation error
+    //
+    // R_err =
+    //     R_des * R_cur^T
+    //
+    // e_R =
+    //     Log(R_err)^vee
+    //
+    // e_R is also expressed in G frame.
+    // =====================================================
+
+    Eigen::Vector3d e_rot_G =
+        rotationError(
+            R_des_G_P,
+            R_cur_G_P);
+
+    if (!e_pos_G.allFinite() ||
+        !e_rot_G.allFinite()) {
+
+        last_compute_status_ =
+            ComputeStatus::INVALID_INPUT;
+
+        return false;
+    }
+
+    // =====================================================
+    // 5. Disabled axes
+    // =====================================================
+
+    for (int i = 0; i < 3; ++i) {
+        if (!axis_enabled_[i]) {
+            e_pos_G[i] = 0.0;
+        }
+
+        if (!axis_enabled_[i + 3]) {
+            e_rot_G[i] = 0.0;
+        }
+    }
+
+    last_position_error_G_ =
+        e_pos_G;
+
+    last_rotation_error_G_ =
+        e_rot_G;
+
+    output.position_error_G =
+        e_pos_G;
+
+    output.rotation_error_G =
+        e_rot_G;
+
+    // =====================================================
+    // 6. Reached check
+    // =====================================================
+
+    bool position_reached = true;
+    bool orientation_reached = true;
+
+    for (int i = 0; i < 3; ++i) {
+
+        if (axis_enabled_[i] &&
+            std::abs(e_pos_G[i]) >
+                tolerance6_[i]) {
+
+            position_reached = false;
+        }
+
+        if (axis_enabled_[i + 3] &&
+            std::abs(e_rot_G[i]) >
+                tolerance6_[i + 3]) {
+
+            orientation_reached = false;
+        }
+    }
+
+    const bool reached =
+        position_reached &&
+        orientation_reached;
+
+    output.position_reached =
+        position_reached;
+
+    output.orientation_reached =
+        orientation_reached;
+
+    output.reached =
+        reached;
+
+    last_reached_ =
+        reached;
+
+    // =====================================================
+    // 7. PBVS P control
+    //
+    // v = Kp * e_pos
+    //
+    // w = Kr * e_rot
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_raw_G =
+            Eigen::Matrix<double, 6, 1>::Zero();
+
+    twist_raw_G.head<3>() =
+        kp_position_.cwiseProduct(
+            e_pos_G);
+
+    twist_raw_G.tail<3>() =
+        kp_rotation_.cwiseProduct(
+            e_rot_G);
+
+    // =====================================================
+    // 8. Dead zone
+    //
+    // 某个轴已经进入误差阈值，则这个轴不再主动运动。
+    // =====================================================
+
+    for (int i = 0; i < 6; ++i) {
+
+        if (!axis_enabled_[i]) {
+            twist_raw_G[i] = 0.0;
+            continue;
+        }
+
+        const double error_value =
+            i < 3
+                ? e_pos_G[i]
+                : e_rot_G[i - 3];
+
+        if (std::abs(error_value) <=
+            tolerance6_[i]) {
+
+            twist_raw_G[i] = 0.0;
+        }
+    }
+
+    output.raw_linear_velocity_G =
+        twist_raw_G.head<3>();
+
+    output.raw_angular_velocity_G =
+        twist_raw_G.tail<3>();
+
+    // =====================================================
+    // 9. Velocity limits
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_vel_limited =
+            twist_raw_G;
+
+    for (int i = 0; i < 6; ++i) {
+
+        const double vmax =
+            vmax6_[i];
+
+        if (!std::isfinite(vmax) ||
+            vmax <= 0.0) {
+
+            twist_vel_limited[i] = 0.0;
+            continue;
+        }
+
+        twist_vel_limited[i] =
+            clampValue(
+                twist_vel_limited[i],
+                -vmax,
+                vmax);
+    }
+
+    // =====================================================
+    // 10. Reached:
+    //
+    // 进入完整目标阈值后直接输出 0。
+    //
+    // 上层可以随后调用 RobotArm::stopL()。
+    // =====================================================
+
+    if (reached) {
+
+        previous_twist_cmd_G_.setZero();
+        has_previous_twist_ = true;
+
+        last_twist_cmd_G_.setZero();
+
+        output.linear_velocity_G.setZero();
+        output.angular_velocity_G.setZero();
+
+        output.twist_G =
+            device::CartesianVelocity{};
+
+        output.valid = true;
+
+        last_compute_status_ =
+            ComputeStatus::OK;
+
+        return true;
+    }
+
+    // =====================================================
+    // 11. Acceleration limits
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_acc_limited;
+
+    if (!has_previous_twist_) {
+
+        previous_twist_cmd_G_.setZero();
+        has_previous_twist_ = true;
+    }
+
+    twist_acc_limited =
+        previous_twist_cmd_G_;
+
+    for (int i = 0; i < 6; ++i) {
+
+        if (!axis_enabled_[i]) {
+            twist_acc_limited[i] = 0.0;
+            continue;
+        }
+
+        const double amax =
+            amax6_[i];
+
+        // <=0：不做加速度限制
+        if (!std::isfinite(amax) ||
+            amax <= 0.0) {
+
+            twist_acc_limited[i] =
+                twist_vel_limited[i];
+
+            continue;
+        }
+
+        const double dv_max =
+            amax * dt;
+
+        const double dv_des =
+            twist_vel_limited[i]
+            -
+            previous_twist_cmd_G_[i];
+
+        const double dv =
+            clampValue(
+                dv_des,
+                -dv_max,
+                dv_max);
+
+        twist_acc_limited[i] =
+            previous_twist_cmd_G_[i]
+            +
+            dv;
+    }
+
+    // =====================================================
+    // 12. First-order LPF
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_filtered =
+            twist_acc_limited;
+
+    const double alpha =
+        std::clamp(
+            twist_lpf_alpha_,
+            0.0,
+            1.0);
+
+    if (alpha > 0.0 &&
+        alpha < 1.0) {
+
+        twist_filtered =
+            alpha *
+            twist_acc_limited
+            +
+            (1.0 - alpha) *
+            previous_twist_cmd_G_;
+    }
+
+    // =====================================================
+    // 13. Make sure disabled axis is zero
+    // =====================================================
+
+    for (int i = 0; i < 6; ++i) {
+        if (!axis_enabled_[i]) {
+            twist_filtered[i] = 0.0;
+        }
+    }
+
+    // =====================================================
+    // 14. Save state
+    // =====================================================
+
+    previous_twist_cmd_G_ =
+        twist_filtered;
+
+    last_twist_cmd_G_ =
+        twist_filtered;
+
+    // =====================================================
+    // 15. Output
+    // =====================================================
+
+    output.linear_velocity_G =
+        twist_filtered.head<3>();
+
+    output.angular_velocity_G =
+        twist_filtered.tail<3>();
+
+    output.twist_G =
+        toCartesianVelocity(
+            twist_filtered);
+
+    output.valid = true;
+
+    last_compute_status_ =
+        ComputeStatus::OK;
+
+    return true;
+}
+
+bool PbvsController::compute(
+    const Eigen::Matrix4d& T_G_P_cur,
+    const double dt,
+    device::CartesianVelocity& twist_G_out)
+{
+    Output output;
+
+    if (!compute(
+            T_G_P_cur,
+            dt,
+            output)) {
+
+        twist_G_out =
+            device::CartesianVelocity{};
+
+        return false;
+    }
+
+    twist_G_out =
+        output.twist_G;
+
+    return true;
+}
+
+void PbvsController::setPositionGain(
+    const Eigen::Vector3d& kp)
+{
+    for (int i = 0; i < 3; ++i) {
+        if (std::isfinite(kp[i]) &&
+            kp[i] >= 0.0) {
+
+            kp_position_[i] =
+                kp[i];
+        }
+    }
+}
+
+void PbvsController::setRotationGain(
+    const Eigen::Vector3d& kr)
+{
+    for (int i = 0; i < 3; ++i) {
+        if (std::isfinite(kr[i]) &&
+            kr[i] >= 0.0) {
+
+            kp_rotation_[i] =
+                kr[i];
+        }
+    }
+}
+
+void PbvsController::setVelocityLimit6(
+    const std::array<double, 6>& vmax6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(vmax6[i]) &&
+            vmax6[i] >= 0.0) {
+
+            vmax6_[i] =
+                vmax6[i];
+        }
+    }
+}
+
+void PbvsController::setAccelerationLimit6(
+    const std::array<double, 6>& amax6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(amax6[i])) {
+            amax6_[i] =
+                amax6[i];
+        }
+    }
+}
+
+void PbvsController::setTolerance6(
+    const std::array<double, 6>& tolerance6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(tolerance6[i]) &&
+            tolerance6[i] >= 0.0) {
+
+            tolerance6_[i] =
+                tolerance6[i];
+        }
+    }
+}
+
+void PbvsController::setTwistFilterAlpha(
+    const double alpha)
+{
+    if (!std::isfinite(alpha)) {
+        return;
+    }
+
+    twist_lpf_alpha_ =
+        std::clamp(
+            alpha,
+            0.0,
+            1.0);
+}
+
+void PbvsController::setAxisEnabled(
+    const std::array<bool, 6>& enabled)
+{
+    axis_enabled_ =
+        enabled;
+}
+
+void PbvsController::resetTwistCommandState()
+{
+    previous_twist_cmd_G_.setZero();
+    last_twist_cmd_G_.setZero();
+
+    has_previous_twist_ = false;
+}
+
+void PbvsController::reset()
+{
+    T_G_P_des_.setIdentity();
+
+    target_valid_ = false;
+
+    last_position_error_G_.setZero();
+    last_rotation_error_G_.setZero();
+
+    last_reached_ = false;
+
+    resetTwistCommandState();
+
+    last_compute_status_ =
+        ComputeStatus::TARGET_NOT_SET;
+}
+
+const char* PbvsController::statusToString(
+    const ComputeStatus status)
+{
+    switch (status) {
+
+        case ComputeStatus::OK:
+            return "ok";
+
+        case ComputeStatus::TARGET_NOT_SET:
+            return "target_not_set";
+
+        case ComputeStatus::INVALID_INPUT:
+            return "invalid_input";
+
+        case ComputeStatus::INVALID_DT:
+            return "invalid_dt";
+
+        case ComputeStatus::INVALID_TARGET:
+            return "invalid_target";
+
+        case ComputeStatus::INVALID_CURRENT_POSE:
+            return "invalid_current_pose";
+
+        default:
+            return "unknown";
+    }
+}
+
+bool PbvsController::isFiniteTransform(
+    const Eigen::Matrix4d& T)
+{
+    return T.allFinite();
+}
+
+bool PbvsController::hasValidBottomRow(
+    const Eigen::Matrix4d& T,
+    const double tolerance)
+{
+    return
+        std::abs(T(3, 0)) <= tolerance &&
+        std::abs(T(3, 1)) <= tolerance &&
+        std::abs(T(3, 2)) <= tolerance &&
+        std::abs(T(3, 3) - 1.0) <= tolerance;
+}
+
+bool PbvsController::isValidTransform(
+    const Eigen::Matrix4d& T)
+{
+    if (!isFiniteTransform(T)) {
+        return false;
+    }
+
+    if (!hasValidBottomRow(T)) {
+        return false;
+    }
+
+    return true;
+}
+
+Eigen::Matrix3d PbvsController::projectToSO3(
+    const Eigen::Matrix3d& R)
+{
+    if (!R.allFinite()) {
+        return Eigen::Matrix3d::Identity();
+    }
+
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(
+        R,
+        Eigen::ComputeFullU |
+        Eigen::ComputeFullV);
+
+    Eigen::Matrix3d U =
+        svd.matrixU();
+
+    const Eigen::Matrix3d V =
+        svd.matrixV();
+
+    Eigen::Matrix3d R_projected =
+        U * V.transpose();
+
+    //
+    // 确保 det = +1，而不是 reflection。
+    //
+    if (R_projected.determinant() < 0.0) {
+
+        U.col(2) *= -1.0;
+
+        R_projected =
+            U * V.transpose();
+    }
+
+    return R_projected;
+}
+
+Eigen::Vector3d PbvsController::rotationError(
+    const Eigen::Matrix3d& R_des,
+    const Eigen::Matrix3d& R_cur)
+{
+    const Eigen::Matrix3d R_d =
+        projectToSO3(R_des);
+
+    const Eigen::Matrix3d R_c =
+        projectToSO3(R_cur);
+
+    //
+    // 空间旋转误差，在 G frame 表达。
+    //
+    // 当前为 I，目标绕 +Z 旋转 theta：
+    //
+    // R_err = R_des
+    //
+    // 得到 +theta Z，
+    // 因此角速度方向正确。
+    //
+    const Eigen::Matrix3d R_err =
+        R_d *
+        R_c.transpose();
+
+    Eigen::AngleAxisd aa(
+        R_err);
+
+    const double angle =
+        aa.angle();
+
+    if (!std::isfinite(angle) ||
+        std::abs(angle) <= 1e-12) {
+
+        return Eigen::Vector3d::Zero();
+    }
+
+    const Eigen::Vector3d axis =
+        aa.axis();
+
+    if (!axis.allFinite()) {
+        return Eigen::Vector3d::Zero();
+    }
+
+    return axis * angle;
+}
+
+double PbvsController::clampValue(
+    const double value,
+    const double lower,
+    const double upper)
+{
+    return std::max(
+        lower,
+        std::min(
+            value,
+            upper));
+}
+
+device::CartesianVelocity
+PbvsController::toCartesianVelocity(
+    const Eigen::Matrix<double, 6, 1>& twist)
+{
+    device::CartesianVelocity velocity;
+
+    velocity.vx =
+        twist[0];
+
+    velocity.vy =
+        twist[1];
+
+    velocity.vz =
+        twist[2];
+
+    velocity.wx =
+        twist[3];
+
+    velocity.wy =
+        twist[4];
+
+    velocity.wz =
+        twist[5];
+
+    return velocity;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp

@@ -0,0 +1,61 @@
+#include "gtest/gtest.h"
+
+#include <array>
+
+#include <Eigen/Geometry>
+
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+
+namespace {
+
+TEST(PbvsControllerTest, AppliesConfigurationSetters) {
+    cmvr::PbvsController controller;
+
+    const Eigen::Vector3d position_gain(1.0, 2.0, 3.0);
+    const Eigen::Vector3d rotation_gain(4.0, 5.0, 6.0);
+    const std::array<double, 6> vmax{{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}};
+    const std::array<double, 6> amax{{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}};
+    const std::array<double, 6> tolerance{{0.001, 0.002, 0.003, 0.01, 0.02, 0.03}};
+
+    controller.setPositionGain(position_gain);
+    controller.setRotationGain(rotation_gain);
+    controller.setVelocityLimit6(vmax);
+    controller.setAccelerationLimit6(amax);
+    controller.setTolerance6(tolerance);
+    controller.setTwistFilterAlpha(0.75);
+
+    EXPECT_TRUE(controller.positionGain().isApprox(position_gain));
+    EXPECT_TRUE(controller.rotationGain().isApprox(rotation_gain));
+    EXPECT_EQ(controller.velocityLimit6(), vmax);
+    EXPECT_EQ(controller.accelerationLimit6(), amax);
+    EXPECT_EQ(controller.tolerance6(), tolerance);
+    EXPECT_DOUBLE_EQ(controller.twistFilterAlpha(), 0.75);
+}
+
+TEST(PbvsControllerTest, CommandsTowardPositionAndRotationError) {
+    cmvr::PbvsController controller;
+    controller.setPositionGain(Eigen::Vector3d::Ones());
+    controller.setRotationGain(Eigen::Vector3d::Ones());
+    controller.setVelocityLimit6({{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}});
+    controller.setAccelerationLimit6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    controller.setTolerance6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    controller.setTwistFilterAlpha(1.0);
+
+    Eigen::Matrix4d target = Eigen::Matrix4d::Identity();
+    target.block<3, 3>(0, 0) =
+        Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    target.block<3, 1>(0, 3) = Eigen::Vector3d(0.1, -0.2, 0.3);
+    ASSERT_TRUE(controller.setTargetPose(target));
+
+    cmvr::PbvsController::Output output;
+    ASSERT_TRUE(controller.compute(Eigen::Matrix4d::Identity(), 0.01, output));
+    ASSERT_TRUE(output.valid);
+    EXPECT_GT(output.linear_velocity_G.x(), 0.0);
+    EXPECT_LT(output.linear_velocity_G.y(), 0.0);
+    EXPECT_GT(output.linear_velocity_G.z(), 0.0);
+    EXPECT_NEAR(output.angular_velocity_G.x(), 0.0, 1e-12);
+    EXPECT_NEAR(output.angular_velocity_G.y(), 0.0, 1e-12);
+    EXPECT_GT(output.angular_velocity_G.z(), 0.0);
+}
+
+} // namespace

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)
+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
+)

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);

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) {

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;
     }

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

cmvr-es/algorithms/perception/CMakeLists.txt

@@ -4,6 +4,7 @@ find_package(OpenCV REQUIRED)
 add_library(perception SHARED
         apriltag/src/tag_relative_target_3d.cpp
         apriltag/src/apriltag_perception.cpp
+        apriltag/src/tag_relative_tcp_pose.cpp
 )
 
 target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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 {

cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h

@@ -0,0 +1,289 @@
+#pragma once
+
+#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
+#define CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
+
+namespace cmvr::perception {
+
+/**
+ * @brief 根据同一台相机同时观测到的 Screen Tag 和 Hand Tag，
+ *        计算 TCP 相对于 Screen Tag 的位姿。
+ *
+ * 坐标系：
+ *
+ *   C : 固定外部相机坐标系
+ *   G : Screen Tag 坐标系，同时作为屏幕参考坐标系
+ *   H : Hand Tag 坐标系
+ *   P : TCP / 触控点坐标系
+ *
+ * 已知：
+ *
+ *   T_C_G : Screen Tag -> Camera
+ *   T_C_H : Hand Tag   -> Camera
+ *   T_H_P : TCP        -> Hand Tag
+ *
+ * 其中 T_H_P 是外部传入的一次标定结果。
+ *
+ * 计算：
+ *
+ *   T_G_H = inverse(T_C_G) * T_C_H
+ *
+ *   T_G_P = T_G_H * T_H_P
+ *
+ * 即：
+ *
+ *   T_G_P = inverse(T_C_G) * T_C_H * T_H_P
+ *
+ * 最终：
+ *
+ *   p_G_P = T_G_P.block<3, 1>(0, 3)
+ *
+ * 得到 TCP 原点在 Screen Tag 坐标系下的位置。
+ *
+ * 注意：
+ * - 本类不主动抓相机图像；
+ * - 本类不主动调用 AprilTagPerception::update()；
+ * - 上层应保证当前 perception 缓存来自同一帧；
+ * - Screen Tag 和 Hand Tag 必须同时在当前帧可见。
+ */
+class TagRelativeTcpPose {
+public:
+    enum class Status {
+        OK = 0,
+
+        NO_PERCEPTION,
+
+        INVALID_SCREEN_TAG_ID,
+
+        INVALID_HAND_TAG_ID,
+
+        SAME_TAG_ID,
+
+        SCREEN_TAG_NOT_FOUND,
+
+        HAND_TAG_NOT_FOUND,
+
+        INVALID_T_C_G,
+
+        INVALID_T_C_H,
+
+        INVALID_T_H_P,
+
+        INVALID_T_G_H,
+
+        INVALID_T_G_P,
+
+        INVALID_TCP_POSITION
+    };
+
+public:
+    explicit TagRelativeTcpPose(
+        const std::shared_ptr<AprilTagPerception>& perception = nullptr);
+
+    /**
+     * @brief 设置 AprilTag 感知前端。
+     *
+     * 本类只读取感知缓存，不主动 update。
+     */
+    void setPerception(
+        const std::shared_ptr<AprilTagPerception>& perception);
+
+    const std::shared_ptr<AprilTagPerception>& perception() const {
+        return perception_;
+    }
+
+    /**
+     * @brief 设置 Screen Tag ID。
+     */
+    void setScreenTagId(int id);
+
+    /**
+     * @brief 设置 Hand Tag ID。
+     */
+    void setHandTagId(int id);
+
+    int screenTagId() const {
+        return screen_tag_id_;
+    }
+
+    int handTagId() const {
+        return hand_tag_id_;
+    }
+
+    /**
+     * @brief 使用当前 AprilTagPerception 缓存计算 TCP 相对 Screen Tag 的位姿。
+     *
+     * 核心公式：
+     *
+     *   T_G_P =
+     *       inverse(T_C_G)
+     *       * T_C_H
+     *       * T_H_P
+     *
+     * @param T_H_P
+     *        TCP(P) 相对于 Hand Tag(H) 的固定齐次变换。
+     *
+     *        坐标变换语义：
+     *
+     *          p_H = T_H_P * p_P
+     *
+     *        即：
+     *
+     *          ^H T_P
+     *
+     * @return 成功返回 true。
+     */
+    bool update(
+        const Eigen::Matrix4d& T_H_P);
+
+    /**
+     * @brief 当前结果是否有效。
+     */
+    bool valid() const {
+        return valid_;
+    }
+
+    /**
+     * @brief 最近一次 update() 的状态。
+     */
+    Status lastStatus() const {
+        return last_status_;
+    }
+
+    static const char* statusToString(Status status);
+
+    /**
+     * @brief 当前 Screen Tag 在 Camera 中的位姿。
+     *
+     * ^C T_G
+     */
+    const Eigen::Matrix4d& T_C_G() const {
+        return T_C_G_;
+    }
+
+    /**
+     * @brief 当前 Hand Tag 在 Camera 中的位姿。
+     *
+     * ^C T_H
+     */
+    const Eigen::Matrix4d& T_C_H() const {
+        return T_C_H_;
+    }
+
+    /**
+     * @brief Hand Tag 相对于 Screen Tag 的位姿。
+     *
+     * ^G T_H
+     */
+    const Eigen::Matrix4d& T_G_H() const {
+        return T_G_H_;
+    }
+
+    /**
+     * @brief TCP 相对于 Screen Tag 的完整 6DoF 位姿。
+     *
+     * ^G T_P
+     */
+    const Eigen::Matrix4d& T_G_P() const {
+        return T_G_P_;
+    }
+
+    /**
+     * @brief TCP 原点在 Screen Tag 坐标系中的位置。
+     *
+     * P_P^G =
+     *
+     * [ x_P ]
+     * [ y_P ]
+     * [ z_P ]
+     */
+    const Eigen::Vector3d& tcpPositionInScreenTag() const {
+        return p_G_P_;
+    }
+
+    /**
+     * @brief TCP 相对于 Screen Tag 的旋转矩阵。
+     *
+     * R_G_P
+     */
+    Eigen::Matrix3d tcpRotationInScreenTag() const {
+        return T_G_P_.block<3, 3>(0, 0);
+    }
+
+    /**
+     * @brief 清空当前结果。
+     */
+    void clear();
+
+private:
+    /**
+     * @brief 检查 4x4 矩阵元素是否全部有限。
+     */
+    static bool isFiniteTransform(
+        const Eigen::Matrix4d& T);
+
+    /**
+     * @brief 基础检查齐次矩阵最后一行。
+     */
+    static bool hasValidHomogeneousBottomRow(
+        const Eigen::Matrix4d& T,
+        double tolerance = 1e-6);
+
+    /**
+     * @brief 判断一个矩阵是否可以作为基本齐次变换使用。
+     *
+     * 当前只检查：
+     * - 所有元素 finite
+     * - 最后一行约等于 [0 0 0 1]
+     *
+     * 暂时不强制检查 rotation orthonormal，
+     * 避免视觉估计中的微小数值误差导致误判。
+     */
+    static bool isValidTransform(
+        const Eigen::Matrix4d& T);
+
+private:
+    std::shared_ptr<AprilTagPerception> perception_{nullptr};
+
+    int screen_tag_id_{-1};
+    int hand_tag_id_{-1};
+
+    // 当前外部相机观测
+    Eigen::Matrix4d T_C_G_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    Eigen::Matrix4d T_C_H_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    // 相对变换
+    Eigen::Matrix4d T_G_H_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    Eigen::Matrix4d T_G_P_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    // TCP 原点在 Screen Tag 坐标系的位置
+    Eigen::Vector3d p_G_P_{
+        Eigen::Vector3d::Zero()
+    };
+
+    bool valid_{false};
+
+    Status last_status_{
+        Status::NO_PERCEPTION
+    };
+};
+
+} // namespace cmvr::perception
+
+#endif // CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H

cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp

@@ -0,0 +1,315 @@
+#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
+
+#include <cmath>
+
+namespace cmvr::perception {
+
+TagRelativeTcpPose::TagRelativeTcpPose(
+    const std::shared_ptr<AprilTagPerception>& perception)
+    : perception_(perception)
+{
+}
+
+void TagRelativeTcpPose::setPerception(
+    const std::shared_ptr<AprilTagPerception>& perception)
+{
+    perception_ = perception;
+    clear();
+
+    if (!perception_) {
+        last_status_ = Status::NO_PERCEPTION;
+    }
+}
+
+void TagRelativeTcpPose::setScreenTagId(
+    const int id)
+{
+    screen_tag_id_ = id;
+    valid_ = false;
+}
+
+void TagRelativeTcpPose::setHandTagId(
+    const int id)
+{
+    hand_tag_id_ = id;
+    valid_ = false;
+}
+
+bool TagRelativeTcpPose::update(
+    const Eigen::Matrix4d& T_H_P)
+{
+    valid_ = false;
+
+    /*
+     * 1. 检查 perception
+     */
+    if (!perception_) {
+        last_status_ = Status::NO_PERCEPTION;
+        return false;
+    }
+
+    /*
+     * 2. 检查 Tag ID
+     */
+    if (screen_tag_id_ < 0) {
+        last_status_ = Status::INVALID_SCREEN_TAG_ID;
+        return false;
+    }
+
+    if (hand_tag_id_ < 0) {
+        last_status_ = Status::INVALID_HAND_TAG_ID;
+        return false;
+    }
+
+    if (screen_tag_id_ == hand_tag_id_) {
+        last_status_ = Status::SAME_TAG_ID;
+        return false;
+    }
+
+    /*
+     * 3. 检查外部传入的：
+     *
+     *      ^H T_P
+     *
+     * Hand Tag -> TCP 固定标定矩阵。
+     */
+    if (!isValidTransform(T_H_P)) {
+        last_status_ = Status::INVALID_T_H_P;
+        return false;
+    }
+
+    /*
+     * 4. 从 AprilTagPerception 当前缓存取 Screen Tag。
+     *
+     * AprilTagPerception 已经通过 ViSP / PnP 得到：
+     *
+     *      ^C T_G
+     */
+    const auto* screen_tag =
+        perception_->findTag(screen_tag_id_);
+
+    if (!screen_tag) {
+        last_status_ =
+            Status::SCREEN_TAG_NOT_FOUND;
+        return false;
+    }
+
+    /*
+     * 5. 取 Hand Tag：
+     *
+     *      ^C T_H
+     */
+    const auto* hand_tag =
+        perception_->findTag(hand_tag_id_);
+
+    if (!hand_tag) {
+        last_status_ =
+            Status::HAND_TAG_NOT_FOUND;
+        return false;
+    }
+
+    /*
+     * 6. 保存当前帧两个原始视觉变换。
+     *
+     * AprilTagPerception::Tag::T_c_t
+     *
+     * 定义是：
+     *
+     *      Tag -> Camera
+     *
+     * 因此：
+     *
+     * screen tag:
+     *
+     *      ^C T_G
+     *
+     * hand tag:
+     *
+     *      ^C T_H
+     */
+    T_C_G_ = screen_tag->T_c_t;
+    T_C_H_ = hand_tag->T_c_t;
+
+    if (!isValidTransform(T_C_G_)) {
+        last_status_ = Status::INVALID_T_C_G;
+        return false;
+    }
+
+    if (!isValidTransform(T_C_H_)) {
+        last_status_ = Status::INVALID_T_C_H;
+        return false;
+    }
+
+    /*
+     * 7. 求 Hand Tag 相对于 Screen Tag 的位姿。
+     *
+     * 已知：
+     *
+     *      ^C T_G
+     *      ^C T_H
+     *
+     * 因此：
+     *
+     *      ^G T_H
+     *
+     *    = (^C T_G)^-1 * ^C T_H
+     */
+    T_G_H_ =
+        T_C_G_.inverse() *
+        T_C_H_;
+
+    if (!isValidTransform(T_G_H_)) {
+        last_status_ = Status::INVALID_T_G_H;
+        return false;
+    }
+
+    /*
+     * 8. 求 TCP 相对于 Screen Tag 的位姿。
+     *
+     * 已知：
+     *
+     *      ^G T_H
+     *      ^H T_P
+     *
+     * 因此：
+     *
+     *      ^G T_P
+     *
+     *    = ^G T_H * ^H T_P
+     *
+     *    = (^C T_G)^-1
+     *      * ^C T_H
+     *      * ^H T_P
+     */
+    T_G_P_ =
+        T_G_H_ *
+        T_H_P;
+
+    if (!isValidTransform(T_G_P_)) {
+        last_status_ = Status::INVALID_T_G_P;
+        return false;
+    }
+
+    /*
+     * 9. 提取 TCP 原点在 Screen Tag
+     *    坐标系 G 下的位置。
+     *
+     *      P_P^G =
+     *
+     *      [ x_P ]
+     *      [ y_P ]
+     *      [ z_P ]
+     *
+     */
+    p_G_P_ =
+        T_G_P_.block<3, 1>(0, 3);
+
+    if (!p_G_P_.allFinite()) {
+        last_status_ =
+            Status::INVALID_TCP_POSITION;
+        return false;
+    }
+
+    /*
+     * 10. 当前帧结果有效。
+     */
+    valid_ = true;
+    last_status_ = Status::OK;
+
+    return true;
+}
+
+void TagRelativeTcpPose::clear()
+{
+    T_C_G_.setIdentity();
+    T_C_H_.setIdentity();
+
+    T_G_H_.setIdentity();
+    T_G_P_.setIdentity();
+
+    p_G_P_.setZero();
+
+    valid_ = false;
+}
+
+const char* TagRelativeTcpPose::statusToString(
+    const Status status)
+{
+    switch (status) {
+
+        case Status::OK:
+            return "ok";
+
+        case Status::NO_PERCEPTION:
+            return "no_perception";
+
+        case Status::INVALID_SCREEN_TAG_ID:
+            return "invalid_screen_tag_id";
+
+        case Status::INVALID_HAND_TAG_ID:
+            return "invalid_hand_tag_id";
+
+        case Status::SAME_TAG_ID:
+            return "same_tag_id";
+
+        case Status::SCREEN_TAG_NOT_FOUND:
+            return "screen_tag_not_found";
+
+        case Status::HAND_TAG_NOT_FOUND:
+            return "hand_tag_not_found";
+
+        case Status::INVALID_T_C_G:
+            return "invalid_T_C_G";
+
+        case Status::INVALID_T_C_H:
+            return "invalid_T_C_H";
+
+        case Status::INVALID_T_H_P:
+            return "invalid_T_H_P";
+
+        case Status::INVALID_T_G_H:
+            return "invalid_T_G_H";
+
+        case Status::INVALID_T_G_P:
+            return "invalid_T_G_P";
+
+        case Status::INVALID_TCP_POSITION:
+            return "invalid_tcp_position";
+
+        default:
+            return "unknown";
+    }
+}
+
+bool TagRelativeTcpPose::isFiniteTransform(
+    const Eigen::Matrix4d& T)
+{
+    return T.allFinite();
+}
+
+bool TagRelativeTcpPose::hasValidHomogeneousBottomRow(
+    const Eigen::Matrix4d& T,
+    const double tolerance)
+{
+    return
+        std::abs(T(3, 0)) <= tolerance &&
+        std::abs(T(3, 1)) <= tolerance &&
+        std::abs(T(3, 2)) <= tolerance &&
+        std::abs(T(3, 3) - 1.0) <= tolerance;
+}
+
+bool TagRelativeTcpPose::isValidTransform(
+    const Eigen::Matrix4d& T)
+{
+    if (!isFiniteTransform(T)) {
+        return false;
+    }
+
+    if (!hasValidHomogeneousBottomRow(T)) {
+        return false;
+    }
+
+    return true;
+}
+
+} // namespace cmvr::perception

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();

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
           }
         }
       }

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
           }
         }
       }

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
           }
         }
       }

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
           }
         }
       }

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
           }
         }
       }

cmvr-es/config/devices/camera/camera.pb.txt

@@ -92,7 +92,7 @@ camera {
       }
       consume_new_frame_only: false
       viewer_pip {
-        enable: true
+        enable: false
         left: -10
         bottom: 10
         width: 320
@@ -101,6 +101,36 @@ camera {
     }
   }
 
+  cameras {
+    id: "mujoco_external_touch_cam"
+    mujoco {
+      world_id: "mujoco_world"
+      camera_name: "external_touch_cam"
+      render {
+        width: 1280
+        height: 720
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 480
+        height: 320
+        fps: 30
+        codec: "H264"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
+      consume_new_frame_only: false
+      viewer_pip {
+        enable: false
+        left: 10
+        bottom: 10
+        width: 320
+        height: 180
+      }
+    }
+  }
+
   cameras {
     id: "left_eye_cam"
     uvc {

cmvr-es/config/devices/mujoco/right_arm_eye_to_hand_world.pb.txt

@@ -0,0 +1,7 @@
+worlds {
+  id: "mujoco_world"
+  model_path: "model/xiaoyan_description/right_arm_eye_to_hand.xml"
+  timestep_s: 0.001
+  realtime_factor: 1.0
+  require_actuator: true
+}

cmvr-es/config/manager/device_manager.pb.txt

@@ -5,36 +5,43 @@ device_manager {
   devices {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD
-    config_file: "devices/mujoco/mujoco_world.pb.txt"
-    enable: false
+    config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
+    enable: true
   }
 
   devices {
     id: "right_arm_mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm_mujoco_qp.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_viewer"
     type: DEVICE_TYPE_MUJOCO_VIEWER
     config_file: "devices/mujoco/mujoco_viewer.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: false
+    enable: true
+  }
+
+  devices {
+    id: "mujoco_external_touch_cam"
+    type: DEVICE_TYPE_CAMERA
+    config_file: "devices/camera/camera.pb.txt"
+    enable: true
   }
 
   devices {
@@ -53,19 +60,12 @@ device_manager {
 
 
   devices {
-    id: "hand1"
+    id: "hand2"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
+    enable: false
   }
 
-    devices {
-      id: "hand2"
-      type: DEVICE_TYPE_DEXHAND
-      config_file: "devices/dexhand/dexhand.pb.txt"
-      enable: true
-    }
-
   devices {
     id: "paxini_tip_1"
     type: DEVICE_TYPE_DEXHAND
@@ -77,7 +77,7 @@ device_manager {
     id: "mujoco_zero_touch_dexhand"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
@@ -91,7 +91,7 @@ device_manager {
     id: "right_arm_can_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -119,16 +119,9 @@ device_manager {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm.pb.txt"
-    enable: true
+    enable: false
   }
 
-    devices {
-      id: "left_arm"
-      type: DEVICE_TYPE_ROBOT_ARM
-      config_file: "devices/arm/arm.pb.txt"
-      enable: false
-    }
-
   devices {
     id: "aubo_arm"
     type: DEVICE_TYPE_ROBOT_ARM

cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt

@@ -1,10 +1,16 @@
 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"
   }
 
   initialization {
@@ -19,46 +25,56 @@ 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
     }
   }
 
   alignment {
-    ibvs {
-      camera_link: "R_CAM"
-      lambda: 0.4
-      mu: 0.1
-      qdot_max: 1.0
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
+    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 }
+      vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 }
+      amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 }
       twist_filter_alpha: 1.0
-      r_camera_to_visp {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: 1.0
-      }
-      r_camera_to_urdf {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: 1.0
-      }
-      control_joint_names: "R_SHOULDER_P"
-      control_joint_names: "R_SHOULDER_R"
-      control_joint_names: "R_SHOULDER_Y"
-      control_joint_names: "R_ELBOW_R"
-      control_joint_names: "R_WRIST_P"
-      control_joint_names: "R_WRIST_Y"
-      control_joint_names: "R_WRIST_R"
     }
     target {
-      position_in_camera { x: -0.001 y: 0.08 z: 0.15 }
-      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 {
@@ -92,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
   }
 }

cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt

@@ -1,10 +1,16 @@
 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"
   }
 
   initialization {
@@ -17,54 +23,73 @@ 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.12
+    # 屏幕 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
     }
   }
 
   alignment {
-    ibvs {
-      camera_link: "R_CAM"
-      lambda: 0.4
-      mu: 0.1
-      qdot_max: 0.8
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
+    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 }
+      vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 }
+      amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 }
       twist_filter_alpha: 1.0
-      r_camera_to_visp {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: -1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: -1.0
-      }
-      r_camera_to_urdf {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: -1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: -1.0
-      }
-      control_joint_names: "R_SHOULDER_P"
-      control_joint_names: "R_SHOULDER_R"
-      control_joint_names: "R_SHOULDER_Y"
-      control_joint_names: "R_ELBOW_R"
-      control_joint_names: "R_WRIST_P"
-      control_joint_names: "R_WRIST_Y"
-      control_joint_names: "R_WRIST_R"
     }
     target {
-      position_in_camera { x: 0.0 y: 0.0 z: 0.30 }
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
-      mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
+      # 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 {
       x: 0.005
       y: 0.005
-      z: 0.010
+      z: 0.005
       rx: 0.08726646259971647
       ry: 0.08726646259971647
       rz: 0.08726646259971647
@@ -78,7 +103,7 @@ touch_screen_task {
     speed_l {
       twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
       acceleration: 6.0
-      max_distance_m: 0.12
+      max_distance_m: 0.02
     }
     tactile {
       finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
@@ -91,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
   }
 }

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};

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;
 }
 

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();

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,

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

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

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -19,6 +19,13 @@ constexpr int kDefaultWidth = 640;
 constexpr int kDefaultHeight = 480;
 constexpr int kMaxGeom = 100000;
 
+// GLFW keeps process-global initialization state. MuJoCo cameras render on
+// independent threads, so serialize the one-time init/window creation path.
+std::mutex& glfwInitMutex() {
+    static std::mutex mutex;
+    return mutex;
+}
+
 int positiveOrDefault(const int value, const int fallback)
 {
     return value > 0 ? value : fallback;
@@ -338,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;
     }
@@ -353,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;
@@ -453,6 +464,7 @@ bool MujocoCamera::initOffscreen_()
         return true;
     }
 
+    std::lock_guard<std::mutex> glfw_lock(glfwInitMutex());
     if (!glfwInit()) {
         setError_("[MujocoCamera] glfwInit failed");
         return false;

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);

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_;

cmvr-es/manager/device_manager/src/device_manager.cpp

@@ -308,12 +308,14 @@ void DeviceManager::configure_mujoco_viewer_pip_()
 
             auto viewer = getDevice<cmvr::MujocoViewerDevice>(viewer_id);
             if (viewer && viewer->setPiPCameraConfig(camera_config)) {
-                camera->setFetchRgbdFn([viewer](std::vector<unsigned char>& rgb,
-                                                std::vector<float>& depth,
-                                                int& width,
-                                                int& height,
-                                                uint64_t& frame_id) {
-                    return viewer->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+                const std::string camera_name = camera_config.camera_name();
+                camera->setFetchRgbdFn([viewer, camera_name](std::vector<unsigned char>& rgb,
+                                                             std::vector<float>& depth,
+                                                             int& width,
+                                                             int& height,
+                                                             uint64_t& frame_id) {
+                    return viewer->getPiPCameraRGBD(
+                        camera_name, rgb, depth, width, height, frame_id);
                 });
                 break;
             }

cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt

@@ -35,4 +35,9 @@ target_link_libraries(mujoco_viewer_test
         gtest_main
         cmvr_es::mujoco_viewer
         cmvr_es::mujoco_world
+        cmvr_es::device_manager
+        cmvr_es::device::camera
+        cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device::motor_robot_arm
 )

cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h

@@ -5,8 +5,11 @@
 #pragma once
 
 #include <atomic>
+#include <cstdint>
+#include <deque>
 #include <memory>
 #include <mutex>
+#include <string>
 #include <thread>
 #include <vector>
 
@@ -52,6 +55,14 @@ namespace cmvr {
                              int display_height,
                              int render_width,
                              int render_height);
+        // Append another fixed camera view to the same viewer window.
+        void addPiPCamera(const char *camera_name,
+                          int left,
+                          int bottom,
+                          int display_width,
+                          int display_height,
+                          int render_width,
+                          int render_height);
         void disablePiPCamera();
         // 获取 PiP 相机 RGB+Depth（Depth 已线性化为米）
         // depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
@@ -60,9 +71,16 @@ namespace cmvr {
                               int &width,
                               int &height,
                               uint64_t &frame_id) const;
+        bool getPiPCameraRGBD(const std::string &camera_name,
+                              std::vector<unsigned char> &rgb,
+                              std::vector<float> &depth,
+                              int &width,
+                              int &height,
+                              uint64_t &frame_id) const;
 
         // 只拿 frame_id，便于 physics 线程判断是否新帧
         uint64_t getPiPCameraFrameId() const;
+        uint64_t getPiPCameraFrameId(const std::string &camera_name) const;
 
     public:
         void setupCamera(double distance = 3.0,
@@ -79,9 +97,34 @@ namespace cmvr {
         void printCameraState() const;
 
     private:
+        struct PiPCameraState {
+            std::string name;
+            int camera_id = -1;
+            int width = 320;
+            int height = 240;
+            int render_width = 320;
+            int render_height = 240;
+            bool render_size_warning_logged = false;
+            int margin = 10;
+            bool custom_pos = false;
+            int left = 0;
+            int bottom = 0;
+            mjvCamera camera{};
+            mjvScene scene{};
+            bool scene_inited = false;
+            mjModel *scene_model = nullptr;
+            std::vector<unsigned char> rgb;
+            std::vector<float> depth;
+            int rgb_width = 0;
+            int rgb_height = 0;
+            bool rgb_valid = false;
+            uint64_t frame_id = 0;
+        };
+
         void renderPiP();
         void initSim();
         void syncThreadFunc();
+        void clearPiPCameras();
 
     private:
         std::shared_ptr<simulate::MujocoWorld> world_;
@@ -93,31 +136,10 @@ namespace cmvr {
         std::unique_ptr<mujoco::Simulate> sim_;
         std::thread sync_thread_;
 
-        bool pip_enabled_ = false;
-        std::string pip_camera_name_;
-        int pip_camera_id_ = -1;
-        int pip_width_ = 320;
-        int pip_height_ = 240;
-        int pip_render_width_ = 320;
-        int pip_render_height_ = 240;
-        bool pip_render_size_warning_logged_ = false;
-        int pip_margin_ = 10;
-        bool pip_custom_pos_ = false;
-        int pip_left_ = 0;
-        int pip_bottom_ = 0;
-        mjvCamera pip_cam_;
-        mjvScene pip_scene_;
-        bool pip_scene_inited_ = false;
-        mjModel *pip_scene_model_ = nullptr;
+        std::deque<PiPCameraState> pip_cameras_;
         mjData *pip_render_data_ = nullptr;
         mjModel *pip_render_data_model_ = nullptr;
         mutable std::mutex pip_rgb_mtx_;
-        std::vector<unsigned char> pip_rgb_;
-        std::vector<float> pip_depth_;     // 新增：z-buffer
-        int pip_rgb_width_ = 0;
-        int pip_rgb_height_ = 0;
-        bool pip_rgb_valid_ = false;
-        uint64_t pip_frame_id_ = 0;        // 新增：帧序号
 
     };
 
@@ -139,15 +161,20 @@ namespace cmvr {
                               int& width,
                               int& height,
                               uint64_t& frame_id) const;
+        bool getPiPCameraRGBD(const std::string& camera_name,
+                              std::vector<unsigned char>& rgb,
+                              std::vector<float>& depth,
+                              int& width,
+                              int& height,
+                              uint64_t& frame_id) const;
 
     private:
         config::MujocoViewerConfig config_;
-        config::MujocoCameraConfig pip_camera_config_;
+        std::vector<config::MujocoCameraConfig> pip_camera_configs_;
         std::shared_ptr<simulate::MujocoWorld> world_;
         std::unique_ptr<MuJocoViewer> viewer_;
         std::thread viewer_thread_;
         mutable std::mutex mtx_;
-        bool has_pip_camera_config_ = false;
         bool running_ = false;
         bool stop_requested_ = false;
     };

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp

@@ -53,8 +53,6 @@ namespace cmvr {
         mjv_defaultCamera(&cam_);
         mjv_defaultOption(&opt_);
         mjv_defaultPerturb(&pert_);
-        mjv_defaultCamera(&pip_cam_);
-        mjv_defaultScene(&pip_scene_);
 
         auto platform_ui = std::make_unique<PiPGlfwAdapter>(this);
         sim_ = std::make_unique<mj::Simulate>(
@@ -72,10 +70,7 @@ namespace cmvr {
             sync_thread_.join();
         }
 
-        if (pip_scene_inited_) {
-            mjv_freeScene(&pip_scene_);
-            pip_scene_inited_ = false;
-        }
+        clearPiPCameras();
         if (pip_render_data_ != nullptr) {
             mj_deleteData(pip_render_data_);
             pip_render_data_ = nullptr;
@@ -120,14 +115,12 @@ namespace cmvr {
     }
 
     void MuJocoViewer::enablePiPCamera(const char *camera_name) {
-        pip_enabled_ = true;
-        pip_camera_name_ = camera_name ? camera_name : "";
-        pip_camera_id_ = -1;
-        pip_width_ = 320;
-        pip_height_ = 240;
-        pip_render_width_ = pip_width_;
-        pip_render_height_ = pip_height_;
-        pip_custom_pos_ = false;
+        clearPiPCameras();
+        PiPCameraState state;
+        state.name = camera_name ? camera_name : "";
+        mjv_defaultCamera(&state.camera);
+        mjv_defaultScene(&state.scene);
+        pip_cameras_.push_back(std::move(state));
     }
 
     void MuJocoViewer::enablePiPCamera(const char *camera_name,
@@ -145,77 +138,66 @@ namespace cmvr {
                                        int display_height,
                                        int render_width,
                                        int render_height) {
-        pip_enabled_ = true;
-        pip_camera_name_ = camera_name ? camera_name : "";
-        pip_camera_id_ = -1;
-        pip_left_ = left;
-        pip_bottom_ = bottom;
-        pip_width_ = display_width > 0 ? display_width : 320;
-        pip_height_ = display_height > 0 ? display_height : 240;
-        pip_render_width_ = render_width > 0 ? render_width : pip_width_;
-        pip_render_height_ = render_height > 0 ? render_height : pip_height_;
-        pip_custom_pos_ = true;
+        clearPiPCameras();
+        addPiPCamera(camera_name,
+                     left,
+                     bottom,
+                     display_width,
+                     display_height,
+                     render_width,
+                     render_height);
+    }
+
+    void MuJocoViewer::addPiPCamera(const char *camera_name,
+                                    int left,
+                                    int bottom,
+                                    int display_width,
+                                    int display_height,
+                                    int render_width,
+                                    int render_height) {
+        PiPCameraState state;
+        state.name = camera_name ? camera_name : "";
+        state.left = left;
+        state.bottom = bottom;
+        state.width = display_width > 0 ? display_width : 320;
+        state.height = display_height > 0 ? display_height : 240;
+        state.render_width = render_width > 0 ? render_width : state.width;
+        state.render_height = render_height > 0 ? render_height : state.height;
+        state.custom_pos = true;
+        mjv_defaultCamera(&state.camera);
+        mjv_defaultScene(&state.scene);
+        pip_cameras_.push_back(std::move(state));
     }
 
     void MuJocoViewer::disablePiPCamera() {
-        pip_enabled_ = false;
+        clearPiPCameras();
+    }
+
+    void MuJocoViewer::clearPiPCameras() {
+        for (auto &pip : pip_cameras_) {
+            if (pip.scene_inited) {
+                mjv_freeScene(&pip.scene);
+                pip.scene_inited = false;
+                pip.scene_model = nullptr;
+            }
+        }
+        pip_cameras_.clear();
     }
 
 
 
     void MuJocoViewer::renderPiP() {
-        if (!pip_enabled_ || !sim_) return;
-        if (pip_camera_name_.empty()) return;
+        if (!sim_ || pip_cameras_.empty()) return;
 
         mjModel* render_model = sim_->m_passive_ ? sim_->m_passive_ : sim_->m_;
         mjData* render_data = sim_->d_passive_ ? sim_->d_passive_ : sim_->d_;
         if (!render_model || !render_data) return;
 
-        if (pip_camera_id_ < 0) {
-            pip_camera_id_ = mj_name2id(render_model, mjOBJ_CAMERA, pip_camera_name_.c_str());
-            if (pip_camera_id_ < 0) {
-                return;
-            }
-        }
-
         auto [fb_width, fb_height] = sim_->platform_ui->GetFramebufferSize();
         if (fb_width <= 0 || fb_height <= 0) return;
 
-        int left = 0;
-        int bottom = 0;
-        int width = 0;
-        int height = 0;
-
-        if (pip_custom_pos_) {
-            width = std::min(pip_width_, fb_width);
-            height = std::min(pip_height_, fb_height);
-            const int max_left = fb_width - width;
-            const int max_bottom = fb_height - height;
-            left = pip_left_ >= 0
-                       ? std::max(0, std::min(pip_left_, max_left))
-                       : std::max(0, std::min(fb_width - width + pip_left_, max_left));
-            bottom = pip_bottom_ >= 0
-                         ? std::max(0, std::min(pip_bottom_, max_bottom))
-                         : std::max(0, std::min(fb_height - height + pip_bottom_, max_bottom));
-        } else {
-            width = std::min(pip_width_, fb_width - 2 * pip_margin_);
-            height = std::min(pip_height_, fb_height - 2 * pip_margin_);
-            left = fb_width - pip_margin_ - width;
-            bottom = pip_margin_;
-        }
-
-        if (width <= 0 || height <= 0) return;
-
-        mjrRect display_rect;
-        display_rect.width = width;
-        display_rect.height = height;
-        display_rect.left = left;
-        display_rect.bottom = bottom;
-
-        // Copy only the visualization state while synchronized with Simulate.
-        // Keep scene update, GPU rendering and pixel readback out of this lock
-        // so the world sync thread cannot hold the simulation mutex while
-        // waiting for the viewer.
+        // Copy one visualization snapshot for all PiP cameras. Keep the
+        // simulation lock out of scene updates, GPU rendering, and readback.
         {
             std::unique_lock<std::recursive_mutex> lock(sim_->mtx, std::try_to_lock);
             if (lock.owns_lock()) {
@@ -233,121 +215,164 @@ namespace cmvr {
             }
         }
 
-        // The simulation lock is intentionally non-blocking.  If the sync
-        // thread owns it, use the last complete snapshot so every back buffer
-        // still gets the PiP overlay; skip only until the first snapshot exists.
+        // If the sync thread owns the lock, use the last complete snapshot.
         if (pip_render_data_ == nullptr || pip_render_data_model_ != render_model) {
             return;
         }
 
-        if (!pip_scene_inited_ || pip_scene_model_ != render_model) {
-            if (pip_scene_inited_) {
-                mjv_freeScene(&pip_scene_);
-            }
-            mjv_makeScene(render_model, &pip_scene_, kPiPMaxGeom);
-            pip_scene_inited_ = true;
-            pip_scene_model_ = render_model;
-        }
-
-        pip_cam_.type = mjCAMERA_FIXED;
-        pip_cam_.fixedcamid = pip_camera_id_;
-        pip_cam_.trackbodyid = -1;
-
-        mjv_updateScene(render_model, pip_render_data_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
-
         auto& context = sim_->platform_ui->mjr_context();
-        const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
-        const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
-        const int render_width = std::min(std::max(1, pip_render_width_), offscreen_width);
-        const int render_height = std::min(std::max(1, pip_render_height_), offscreen_height);
-        if (!pip_render_size_warning_logged_ &&
-            (render_width != pip_render_width_ || render_height != pip_render_height_)) {
-            CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
-                              << ", requested=" << pip_render_width_ << "x" << pip_render_height_
-                              << ", actual=" << render_width << "x" << render_height
-                              << ", offscreen=" << offscreen_width << "x" << offscreen_height;
-            pip_render_size_warning_logged_ = true;
-        }
-        const bool use_offscreen = render_width != display_rect.width || render_height != display_rect.height;
+        for (size_t pip_index = 0; pip_index < pip_cameras_.size(); ++pip_index) {
+            auto& pip = pip_cameras_[pip_index];
+            if (pip.name.empty()) continue;
 
-        mjrRect render_rect;
-        render_rect.left = 0;
-        render_rect.bottom = 0;
-        render_rect.width = render_width;
-        render_rect.height = render_height;
+            if (pip.camera_id < 0) {
+                pip.camera_id = mj_name2id(render_model, mjOBJ_CAMERA, pip.name.c_str());
+                if (pip.camera_id < 0) {
+                    CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera not found: " << pip.name;
+                    continue;
+                }
+            }
 
-        bool rendered_offscreen = false;
-        if (use_offscreen) {
-            mjr_setBuffer(mjFB_OFFSCREEN, &context);
-            if (context.currentBuffer == mjFB_OFFSCREEN) {
-                mjr_render(render_rect, &pip_scene_, &context);
-                rendered_offscreen = true;
+            int left = 0;
+            int bottom = 0;
+            int width = 0;
+            int height = 0;
+            if (pip.custom_pos) {
+                width = std::min(pip.width, fb_width);
+                height = std::min(pip.height, fb_height);
+                const int max_left = fb_width - width;
+                const int max_bottom = fb_height - height;
+                left = pip.left >= 0
+                           ? std::max(0, std::min(pip.left, max_left))
+                           : std::max(0, std::min(fb_width - width + pip.left, max_left));
+                bottom = pip.bottom >= 0
+                             ? std::max(0, std::min(pip.bottom, max_bottom))
+                             : std::max(0, std::min(fb_height - height + pip.bottom, max_bottom));
             } else {
-                mjr_render(display_rect, &pip_scene_, &context);
+                width = std::min(pip.width, fb_width - 2 * pip.margin);
+                height = std::min(pip.height, fb_height - 2 * pip.margin);
+                left = fb_width - pip.margin - width;
+                bottom = pip.margin;
+            }
+            if (width <= 0 || height <= 0) continue;
+
+            mjrRect display_rect;
+            display_rect.width = width;
+            display_rect.height = height;
+            display_rect.left = left;
+            display_rect.bottom = bottom;
+
+            if (!pip.scene_inited || pip.scene_model != render_model) {
+                if (pip.scene_inited) {
+                    mjv_freeScene(&pip.scene);
+                }
+                mjv_makeScene(render_model, &pip.scene, kPiPMaxGeom);
+                pip.scene_inited = true;
+                pip.scene_model = render_model;
+            }
+
+            pip.camera.type = mjCAMERA_FIXED;
+            pip.camera.fixedcamid = pip.camera_id;
+            pip.camera.trackbodyid = -1;
+            mjv_updateScene(render_model,
+                            pip_render_data_,
+                            &opt_,
+                            &pert_,
+                            &pip.camera,
+                            mjCAT_ALL,
+                            &pip.scene);
+
+            const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
+            const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
+            const int render_width = std::min(std::max(1, pip.render_width), offscreen_width);
+            const int render_height = std::min(std::max(1, pip.render_height), offscreen_height);
+            if (!pip.render_size_warning_logged &&
+                (render_width != pip.render_width || render_height != pip.render_height)) {
+                CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
+                                  << ", camera=" << pip.name
+                                  << ", requested=" << pip.render_width << "x" << pip.render_height
+                                  << ", actual=" << render_width << "x" << render_height
+                                  << ", offscreen=" << offscreen_width << "x" << offscreen_height;
+                pip.render_size_warning_logged = true;
+            }
+            const bool use_offscreen =
+                render_width != display_rect.width || render_height != display_rect.height;
+
+            mjrRect render_rect;
+            render_rect.left = 0;
+            render_rect.bottom = 0;
+            render_rect.width = render_width;
+            render_rect.height = render_height;
+
+            bool rendered_offscreen = false;
+            if (use_offscreen) {
+                mjr_setBuffer(mjFB_OFFSCREEN, &context);
+                if (context.currentBuffer == mjFB_OFFSCREEN) {
+                    mjr_render(render_rect, &pip.scene, &context);
+                    rendered_offscreen = true;
+                } else {
+                    mjr_render(display_rect, &pip.scene, &context);
+                    render_rect = display_rect;
+                }
+            } else {
+                mjr_render(display_rect, &pip.scene, &context);
                 render_rect = display_rect;
             }
-        } else {
-            mjr_render(display_rect, &pip_scene_, &context);
-            render_rect = display_rect;
-        }
 
-        {
-            std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-            const int w = render_rect.width;
-            const int h = render_rect.height;
-            if (w > 0 && h > 0) {
-                pip_rgb_.resize(static_cast<size_t>(3 * w * h));
-                pip_depth_.resize(static_cast<size_t>(w * h));
+            {
+                std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+                const int w = render_rect.width;
+                const int h = render_rect.height;
+                if (w > 0 && h > 0) {
+                    pip.rgb.resize(static_cast<size_t>(3 * w * h));
+                    pip.depth.resize(static_cast<size_t>(w * h));
+                    mjr_readPixels(pip.rgb.data(), pip.depth.data(), render_rect, &context);
 
-                // 同时读 RGB 和 depth(z-buffer 0..1)
-                mjr_readPixels(pip_rgb_.data(), pip_depth_.data(),
-                               render_rect, &context);
-                if (rendered_offscreen) {
-                    mjr_setBuffer(mjFB_WINDOW, &context);
-                    mjr_render(display_rect, &pip_scene_, &context);
-                }
+                    // OpenGL's pixel origin is bottom-left; expose top-left images.
+                    for (int r = 0; r < h / 2; ++r) {
+                        unsigned char *top_row = pip.rgb.data() + 3 * w * r;
+                        unsigned char *bottom_row = pip.rgb.data() + 3 * w * (h - 1 - r);
+                        std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
 
-                // OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth
-                for (int r = 0; r < h / 2; ++r) {
-                    // flip rgb row
-                    unsigned char *top_row = pip_rgb_.data() + 3 * w * r;
-                    unsigned char *bottom_row = pip_rgb_.data() + 3 * w * (h - 1 - r);
-                    std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
-
-                    // flip depth row
-                    float *top_d = pip_depth_.data() + w * r;
-                    float *bot_d = pip_depth_.data() + w * (h - 1 - r);
-                    std::swap_ranges(top_d, top_d + w, bot_d);
-                }
-
-                // 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。
-                const double znear = static_cast<double>(render_model->vis.map.znear) *
-                                     static_cast<double>(render_model->stat.extent);
-                const double zfar  = static_cast<double>(render_model->vis.map.zfar) *
-                                     static_cast<double>(render_model->stat.extent);
-                if (znear > 0.0 && zfar > znear) {
-                    const double two_nf = 2.0 * znear * zfar;
-                    const double f_plus_n = zfar + znear;
-                    const double f_minus_n = zfar - znear;
-                    for (float &d : pip_depth_) {
-                        if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
-                            d = std::numeric_limits<float>::infinity();
-                            continue;
-                        }
-                        const double z_ndc = 2.0 * static_cast<double>(d) - 1.0; // [-1,1]
-                        const double denom = f_plus_n - z_ndc * f_minus_n;
-                        if (denom <= 1e-12) {
-                            d = std::numeric_limits<float>::infinity();
-                            continue;
-                        }
-                        d = static_cast<float>(two_nf / denom);
+                        float *top_d = pip.depth.data() + w * r;
+                        float *bot_d = pip.depth.data() + w * (h - 1 - r);
+                        std::swap_ranges(top_d, top_d + w, bot_d);
                     }
-                }
 
-                pip_rgb_width_ = w;
-                pip_rgb_height_ = h;
-                pip_rgb_valid_ = true;
-                ++pip_frame_id_;   // 新帧
+                    // Linearize the OpenGL depth buffer to camera-forward meters.
+                    const double znear = static_cast<double>(render_model->vis.map.znear) *
+                                         static_cast<double>(render_model->stat.extent);
+                    const double zfar = static_cast<double>(render_model->vis.map.zfar) *
+                                        static_cast<double>(render_model->stat.extent);
+                    if (znear > 0.0 && zfar > znear) {
+                        const double two_nf = 2.0 * znear * zfar;
+                        const double f_plus_n = zfar + znear;
+                        const double f_minus_n = zfar - znear;
+                        for (float &d : pip.depth) {
+                            if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
+                                d = std::numeric_limits<float>::infinity();
+                                continue;
+                            }
+                            const double z_ndc = 2.0 * static_cast<double>(d) - 1.0;
+                            const double denom = f_plus_n - z_ndc * f_minus_n;
+                            if (denom <= 1e-12) {
+                                d = std::numeric_limits<float>::infinity();
+                                continue;
+                            }
+                            d = static_cast<float>(two_nf / denom);
+                        }
+                    }
+
+                    pip.rgb_width = w;
+                    pip.rgb_height = h;
+                    pip.rgb_valid = true;
+                    ++pip.frame_id;
+                }
+            }
+
+            if (rendered_offscreen) {
+                mjr_setBuffer(mjFB_WINDOW, &context);
+                mjr_render(display_rect, &pip.scene, &context);
             }
         }
     }
@@ -362,12 +387,18 @@ namespace cmvr {
             if (!world_->model() || !world_->data()) {
                 mju_error("MuJocoViewer world has null model/data");
             }
-            if (pip_enabled_ && pip_render_width_ > 0 && pip_render_height_ > 0) {
+            int max_pip_render_width = 0;
+            int max_pip_render_height = 0;
+            for (const auto &pip : pip_cameras_) {
+                max_pip_render_width = std::max(max_pip_render_width, pip.render_width);
+                max_pip_render_height = std::max(max_pip_render_height, pip.render_height);
+            }
+            if (!pip_cameras_.empty() && max_pip_render_width > 0 && max_pip_render_height > 0) {
                 mjModel* model = world_->model();
                 const int old_width = model->vis.global.offwidth;
                 const int old_height = model->vis.global.offheight;
-                model->vis.global.offwidth = std::max(model->vis.global.offwidth, pip_render_width_);
-                model->vis.global.offheight = std::max(model->vis.global.offheight, pip_render_height_);
+                model->vis.global.offwidth = std::max(model->vis.global.offwidth, max_pip_render_width);
+                model->vis.global.offheight = std::max(model->vis.global.offheight, max_pip_render_height);
                 if (model->vis.global.offwidth != old_width || model->vis.global.offheight != old_height) {
                     CMVR_LOG(INFO) << "[MuJocoViewer] resize offscreen buffer before context creation"
                                    << ", old=" << old_width << "x" << old_height
@@ -417,7 +448,15 @@ namespace cmvr {
 
     uint64_t MuJocoViewer::getPiPCameraFrameId() const {
         std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-        return pip_frame_id_;
+        return pip_cameras_.empty() ? 0 : pip_cameras_.front().frame_id;
+    }
+
+    uint64_t MuJocoViewer::getPiPCameraFrameId(const std::string &camera_name) const {
+        std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+        const auto it = std::find_if(
+            pip_cameras_.begin(), pip_cameras_.end(),
+            [&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; });
+        return it == pip_cameras_.end() ? 0 : it->frame_id;
     }
 
     bool MuJocoViewer::getPiPCameraRGBD(std::vector<unsigned char> &rgb,
@@ -426,13 +465,35 @@ namespace cmvr {
                                         int &height,
                                         uint64_t &frame_id) const {
         std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-        if (!pip_rgb_valid_ || pip_rgb_.empty()) return false;
+        if (pip_cameras_.empty()) return false;
+        const auto &pip = pip_cameras_.front();
+        if (!pip.rgb_valid || pip.rgb.empty()) return false;
 
-        rgb = pip_rgb_;
-        depth = pip_depth_;
-        width = pip_rgb_width_;
-        height = pip_rgb_height_;
-        frame_id = pip_frame_id_;
+        rgb = pip.rgb;
+        depth = pip.depth;
+        width = pip.rgb_width;
+        height = pip.rgb_height;
+        frame_id = pip.frame_id;
+        return true;
+    }
+
+    bool MuJocoViewer::getPiPCameraRGBD(const std::string &camera_name,
+                                        std::vector<unsigned char> &rgb,
+                                        std::vector<float> &depth,
+                                        int &width,
+                                        int &height,
+                                        uint64_t &frame_id) const {
+        std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+        const auto it = std::find_if(
+            pip_cameras_.begin(), pip_cameras_.end(),
+            [&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; });
+        if (it == pip_cameras_.end() || !it->rgb_valid || it->rgb.empty()) return false;
+
+        rgb = it->rgb;
+        depth = it->depth;
+        width = it->rgb_width;
+        height = it->rgb_height;
+        frame_id = it->frame_id;
         return true;
     }
 
@@ -487,6 +548,7 @@ namespace cmvr {
         }
 
         std::shared_ptr<simulate::MujocoWorld> world;
+        std::vector<config::MujocoCameraConfig> pip_camera_configs;
         {
             std::lock_guard<std::mutex> lock(mtx_);
             if (running_) {
@@ -497,6 +559,7 @@ namespace cmvr {
                 return false;
             }
             world = world_;
+            pip_camera_configs = pip_camera_configs_;
             stop_requested_ = false;
             running_ = true;
         }
@@ -517,19 +580,30 @@ namespace cmvr {
                             config_.camera_azimuth(),
                             config_.camera_elevation());
 
-        if (has_pip_camera_config_ && !pip_camera_config_.camera_name().empty()) {
-            const auto& pip = pip_camera_config_.viewer_pip();
-            const auto& render = pip_camera_config_.render();
-            if (pip.width() > 0 && pip.height() > 0) {
-                viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str(),
+        bool first_pip_camera = true;
+        for (const auto& camera_config : pip_camera_configs) {
+            if (camera_config.camera_name().empty()) {
+                continue;
+            }
+            const auto& pip = camera_config.viewer_pip();
+            const auto& render = camera_config.render();
+            if (first_pip_camera) {
+                viewer->enablePiPCamera(camera_config.camera_name().c_str(),
                                         pip.left(),
                                         pip.bottom(),
                                         pip.width(),
                                         pip.height(),
                                         render.width(),
                                         render.height());
+                first_pip_camera = false;
             } else {
-                viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str());
+                viewer->addPiPCamera(camera_config.camera_name().c_str(),
+                                     pip.left(),
+                                     pip.bottom(),
+                                     pip.width(),
+                                     pip.height(),
+                                     render.width(),
+                                     render.height());
             }
         }
 
@@ -556,19 +630,31 @@ namespace cmvr {
         if (camera_config.world_id() != config_.world_id()) {
             return false;
         }
-
-        std::lock_guard<std::mutex> lock(mtx_);
-        if (has_pip_camera_config_) {
-            CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first"
-                              << ", viewer_id=" << id_
-                              << ", current_camera=" << pip_camera_config_.camera_name()
-                              << ", ignored_camera=" << camera_config.camera_name();
+        if (camera_config.camera_name().empty()) {
             return false;
         }
 
-        pip_camera_config_ = camera_config;
-        has_pip_camera_config_ = true;
-        CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera"
+        std::lock_guard<std::mutex> lock(mtx_);
+        if (running_) {
+            CMVR_LOG(WARNING) << "[MujocoViewerDevice] cannot add PiP camera while viewer is running"
+                              << ", viewer_id=" << id_
+                              << ", camera=" << camera_config.camera_name();
+            return false;
+        }
+        const auto duplicate = std::find_if(
+            pip_camera_configs_.begin(), pip_camera_configs_.end(),
+            [&camera_config](const config::MujocoCameraConfig& configured) {
+                return configured.camera_name() == camera_config.camera_name();
+            });
+        if (duplicate != pip_camera_configs_.end()) {
+            CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured"
+                              << ", viewer_id=" << id_
+                              << ", camera=" << camera_config.camera_name();
+            return false;
+        }
+
+        pip_camera_configs_.push_back(camera_config);
+        CMVR_LOG(INFO) << "[MujocoViewerDevice] add PiP camera"
                        << ", viewer_id=" << id_
                        << ", camera=" << camera_config.camera_name()
                        << ", world_id=" << camera_config.world_id();
@@ -587,6 +673,20 @@ namespace cmvr {
         return viewer_->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
     }
 
+    bool MujocoViewerDevice::getPiPCameraRGBD(const std::string& camera_name,
+                                              std::vector<unsigned char>& rgb,
+                                              std::vector<float>& depth,
+                                              int& width,
+                                              int& height,
+                                              uint64_t& frame_id) const {
+        std::lock_guard<std::mutex> lock(mtx_);
+        if (!viewer_) {
+            return false;
+        }
+        return viewer_->getPiPCameraRGBD(
+            camera_name, rgb, depth, width, height, frame_id);
+    }
+
     bool MujocoViewerDevice::stop() {
         std::thread thread_to_join;
         {

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp

@@ -1,11 +1,21 @@
+#include <algorithm>
+#include <array>
 #include <cstdlib>
 #include <filesystem>
 #include <iostream>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <gtest/gtest.h>
 
+#include "common/config/config_files.h"
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "devices/arm/robot_arm.h"
+#include "devices/camera/abstract_camera.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"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 #include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
@@ -37,6 +47,63 @@ std::string defaultModelPath()
     return (root / "model/xiaoyan_description/dual_arm.xml").string();
 }
 
+cmvr::device::DeviceManager& createEyeToHandDeviceManager(
+    const std::filesystem::path& project_root)
+{
+    cmvr::device::DeviceManager::destroyInstance();
+    cmvr::ConfigHelper::setConfigRootFromFile(
+        (project_root / "cmvr-es/config/cmvr_es.pb.txt").string());
+
+    cmvr::config::DeviceManagerConfig config;
+    config.set_name("mujoco_viewer_eye_to_hand_test");
+    config.set_version("test");
+
+    auto* world = config.add_devices();
+    world->set_id("mujoco_world");
+    world->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD);
+    world->set_config_file("devices/mujoco/right_arm_eye_to_hand_world.pb.txt");
+    world->set_enable(true);
+
+    auto* motors = config.add_devices();
+    motors->set_id("right_arm_mujoco_motors");
+    motors->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
+    motors->set_config_file("devices/motor/mujoco_motors.pb.txt");
+    motors->set_enable(true);
+
+    auto* arm = config.add_devices();
+    arm->set_id("mujoco_right_arm");
+    arm->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM);
+    arm->set_config_file("devices/arm/arm_mujoco.pb.txt");
+    arm->set_enable(true);
+
+    for (const auto* camera_id : {"mujoco_hand_cam", "mujoco_external_touch_cam"}) {
+        auto* camera = config.add_devices();
+        camera->set_id(camera_id);
+        camera->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA);
+        camera->set_config_file("devices/camera/camera.pb.txt");
+        camera->set_enable(true);
+    }
+
+    return cmvr::device::DeviceManager::getInstance(config);
+}
+
+cmvr::device::Result moveArmToInitialization(cmvr::device::RobotArm& arm)
+{
+    const std::vector<double> positions = {
+        -0.2423,
+        1.2929,
+        1.61,
+        1.58,
+        -2.8792,
+        0.1150,
+        -0.08,
+    };
+    cmvr::device::MotionOptions options;
+    options.velocity = 2.8;
+    options.acceleration = 20.0;
+    return arm.moveJ(cmvr::device::JointPositionCommand{positions}, options);
+}
+
 } // namespace
 
 TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
@@ -63,3 +130,67 @@ TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
 
     world->stop();
 }
+
+TEST(MujocoViewerTest, ShowsRightArmEyeToHandCamera)
+{
+    const auto project_root = findProjectRoot();
+    ASSERT_FALSE(project_root.empty());
+    const auto model_path = project_root / "model/xiaoyan_description/right_arm_eye_to_hand.xml";
+
+    auto& device_manager = createEyeToHandDeviceManager(project_root);
+    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
+    ASSERT_NE(arm, nullptr);
+
+    auto hand_camera_base =
+        device_manager.getDevice<cmvr::device::AbstractCamera>("mujoco_hand_cam");
+    auto external_camera_base =
+        device_manager.getDevice<cmvr::device::AbstractCamera>("mujoco_external_touch_cam");
+    auto hand_camera = std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(hand_camera_base);
+    auto external_camera =
+        std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(external_camera_base);
+    ASSERT_NE(hand_camera, nullptr);
+    ASSERT_NE(external_camera, nullptr);
+
+    auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
+    ASSERT_NE(world, nullptr);
+    ASSERT_TRUE(world->isLoaded());
+    ASSERT_TRUE(world->isRunning());
+
+    const auto move_result = moveArmToInitialization(*arm);
+    ASSERT_TRUE(move_result.ok()) << move_result.message;
+
+    cmvr::MuJocoViewer viewer(world);
+    ASSERT_NE(viewer.model(), nullptr);
+    ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0)
+        << "right_arm_eye_to_hand.xml does not contain hand_cam";
+    ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0)
+        << "right_arm_eye_to_hand.xml does not contain external_touch_cam";
+
+    viewer.setupCamera(2.5, -160.0, -25.0);
+    const auto& external_config = external_camera->config();
+    const auto& hand_config = hand_camera->config();
+    ASSERT_TRUE(external_config.viewer_pip().enable());
+    ASSERT_TRUE(hand_config.viewer_pip().enable());
+    viewer.enablePiPCamera(external_config.camera_name().c_str(),
+                           external_config.viewer_pip().left(),
+                           external_config.viewer_pip().bottom(),
+                           external_config.viewer_pip().width(),
+                           external_config.viewer_pip().height(),
+                           external_config.encoder().width(),
+                           external_config.encoder().height());
+    viewer.addPiPCamera(hand_config.camera_name().c_str(),
+                        hand_config.viewer_pip().left(),
+                        hand_config.viewer_pip().bottom(),
+                        hand_config.viewer_pip().width(),
+                        hand_config.viewer_pip().height(),
+                        hand_config.encoder().width(),
+                        hand_config.encoder().height());
+
+    std::cout << "MujocoWorld loaded: " << model_path.string() << std::endl;
+    std::cout << "Showing hand_cam and external_touch_cam in PiP. "
+                 "Close the MuJoCo window to exit."
+              << std::endl;
+    viewer.run();
+
+    world->stop();
+}

cmvr-es/task/touch_screen_task/include/touch_screen_task.h

@@ -13,13 +13,14 @@
 #include <Eigen/Dense>
 
 #include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
-#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
+#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
+#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
 #include "devices/camera/abstract_camera.h"
 #include "devices/dexhand/abstract_dexhand.h"
 #include "devices/arm/robot_arm.h"
 #include "task/task.h"
-#include "algorithms/perception/apriltag/include/apriltag_perception.h"
-#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
 
 namespace cmvr::task {
 
@@ -27,7 +28,7 @@ class TouchScreenTask : public Task {
 public:
     enum class Phase {
         IDLE = 0,       // 空闲，尚未开始任务。
-        ALIGNING,       // 视觉对准阶段：持续 IBVS 对齐目标点。
+        ALIGNING,       // 视觉对准阶段：持续 PBVS 对齐目标点。
         ALIGN_REACHED,  // 视觉对准已达到阈值，等待进入下一阶段。
         TOUCHING,       // 前进触控阶段：沿设定方向向屏幕推进。
         DWELLING,       // 已检测到接触，保持当前位置短暂停留。
@@ -40,11 +41,10 @@ public:
         IDLE = 0,                 // 空闲状态。
         NOT_INITIALIZED,          // 尚未调用 init() 完成初始化。
         INVALID_CONFIG,           // 配置非法，无法启动或应用参数。
-        CONTROL_JOINT_MISMATCH,   // 控制关节顺序与 IK 链不一致。
         ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。
         ALIGN_WAITING_TRACK,      // 对准阶段等待目标点跟踪恢复成功。
-        ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。
-        ALIGN_COMPUTE_FAILED,     // 对准阶段 IBVS 或 IK 计算失败。
+        ALIGN_TARGET_SETUP_FAILED,// PBVS 目标位姿设置失败。
+        ALIGN_COMPUTE_FAILED,     // 对准阶段 PBVS 计算失败。
         ALIGN_TIMEOUT,            // 对准阶段超时仍未收敛。
         ALIGNING,                 // 正在执行视觉对准。
         ALIGN_REACHED,            // 视觉对准完成。
@@ -67,6 +67,10 @@ public:
     bool init(const std::shared_ptr<device::RobotArm>& arm,
               const std::shared_ptr<device::AbstractDexHand>& dexhand,
               const std::shared_ptr<device::AbstractCamera>& camera);
+    bool init(const std::shared_ptr<device::RobotArm>& arm,
+              const std::shared_ptr<device::AbstractDexHand>& dexhand,
+              const std::shared_ptr<device::AbstractCamera>& camera,
+              const std::shared_ptr<device::AbstractCamera>& external_camera);
 
     const std::string& id() const override { return id_; }
 
@@ -92,11 +96,15 @@ public:
     double lastTouchPressureSum() const;
     int lastTouchNonzeroCount() const;
     int lastActiveTagId() const;
-    Eigen::Vector3d lastAlignErrorCamera() const;
+    Eigen::Vector3d lastAlignErrorScreenTag() const;
 
     const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
+    const std::shared_ptr<perception::AprilTagPerception>& externalPerception() const {
+        return external_perception_;
+    }
     const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
-    const IbvsController& ibvs() const { return ibvs_; }
+    const perception::TagRelativeTcpPose& tcpPoseTracker() const { return tcp_pose_tracker_; }
+    const PbvsController& pbvs() const { return pbvs_; }
 
 private:
     using Clock = std::chrono::steady_clock;
@@ -106,18 +114,14 @@ private:
     bool startFromPixelUnlocked(int u, int v);
     void stopUnlocked();
     bool applyConfig();
-    bool validateControlJointNames() const;
     bool stepAligning(double dt);
     bool stepTouching();
     bool stepDwelling();
     bool stepRetracting();
 
-    bool readControlledJointPositions(std::vector<double>& q_out) const;
-    bool sendJointVelocity(const std::vector<double>& qdot) const;
-    bool sendZeroJointVelocity() const;
-    void hardStopIbvsMotion();
-    bool holdCurrentControlledPosition() const;
+    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;
@@ -129,6 +133,8 @@ private:
     void enterFailed(Status status);
 
     bool updateTouchPressure();
+    void publishCoordinateOverlay();
+    void refreshCoordinateOverlay();
 
 private:
     mutable std::mutex mutex_;
@@ -136,10 +142,13 @@ private:
     std::shared_ptr<device::RobotArm> arm_{nullptr};
     std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
     std::shared_ptr<device::AbstractCamera> camera_{nullptr};
+    std::shared_ptr<device::AbstractCamera> external_camera_{nullptr};
 
     std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
+    std::shared_ptr<perception::AprilTagPerception> external_perception_{nullptr};
     perception::TagRelativeTarget3D tracker_;
-    IbvsController ibvs_;
+    perception::TagRelativeTcpPose tcp_pose_tracker_;
+    PbvsController pbvs_;
     cmvr::config::TouchScreenTaskConfig config_{};
     bool config_valid_{false};
 
@@ -149,7 +158,7 @@ private:
 
     bool initialized_{false};
     bool target_locked_{false};
-    bool ibvs_target_initialized_{false};
+    bool pbvs_target_initialized_{false};
     bool touch_command_started_{false};
     bool retract_command_started_{false};
 
@@ -157,19 +166,31 @@ private:
     int target_v_{-1};
     int align_stable_count_{0};
     int align_debug_count_{0};
+    int pbvs_debug_count_{0};
     int last_active_tag_id_{-1};
 
     double last_touch_pressure_sum_{0.0};
     int last_touch_nonzero_count_{0};
-    Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
+    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};
     Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
     bool retract_start_position_valid_{false};
     Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()};
+    bool have_last_T_B_G_{false};
+    Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()};
+    double max_T_B_G_translation_delta_m_{0.0};
+    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};
 };

cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp

@@ -1,6 +1,7 @@
 #include "gtest/gtest.h"
 
 #include <algorithm>
+#include <atomic>
 #include <array>
 #include <chrono>
 #include <cmath>
@@ -22,9 +23,11 @@
 #include "common/vision/image_projection.h"
 #include "common/io/proto_file_io.h"
 #include "cmvr/config/arm_config/arm_config.pb.h"
+#include "cmvr/config/logger_config/logger_config.pb.h"
 #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"
@@ -74,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)
@@ -103,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(
@@ -208,9 +215,9 @@ void run_touch_once(int u, int v) {
                   << p_c_target.z() << "]"
                   << ", nonzero_count=" << task->lastTouchNonzeroCount()
                   << ", pressure_sum=" << task->lastTouchPressureSum()
-                  << ", err_c=[" << task->lastAlignErrorCamera().x() << ", "
-                  << task->lastAlignErrorCamera().y() << ", "
-                  << task->lastAlignErrorCamera().z() << "]\n";
+                  << ", err_G=[" << task->lastAlignErrorScreenTag().x() << ", "
+                  << task->lastAlignErrorScreenTag().y() << ", "
+                  << task->lastAlignErrorScreenTag().z() << "]\n";
 
         if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::ALIGN_REACHED) {
             std::cout << "align reached, target_c=[" << p_c_target.x() << ", "
@@ -264,17 +271,384 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
         const auto& config = root_config.touch_screen_task();
         EXPECT_TRUE(config.has_initialization());
         EXPECT_TRUE(config.has_perception());
+        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());
-        EXPECT_TRUE(config.alignment().has_ibvs());
-        EXPECT_TRUE(config.alignment().ibvs().has_camera_link());
+        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);
+        EXPECT_DOUBLE_EQ(pbvs.rotation_gain().z(), 1.5);
+        EXPECT_DOUBLE_EQ(pbvs.vmax6().z(), 0.05);
+        EXPECT_DOUBLE_EQ(pbvs.amax6().rz(), 2.0);
+        EXPECT_DOUBLE_EQ(pbvs.twist_filter_alpha(), 1.0);
         EXPECT_TRUE(config.has_touch());
         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, RunTouchOnceInMujoco) {
+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());
+
+    cmvr::config::LoggerRootConfig logger_root;
+    const auto logger_config_path = project_root / "cmvr-es/config/logger/logger.pb.txt";
+    ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+        logger_config_path.string(), &logger_root))
+        << logger_config_path;
+    ASSERT_TRUE(cmvr::logging::initLogging(
+        logger_root.logger(), "touch_screen_task_test", project_root));
+
+    cmvr::ConfigHelper::setConfigRootFromFile(
+        (project_root / "cmvr-es/config/cmvr_es.pb.txt").string());
+
+    cmvr::task::TaskManager::destroyInstance();
+    cmvr::device::DeviceManager::destroyInstance();
+
+    // This test intentionally builds only the devices required by the
+    // MuJoCo Eye-to-Hand task. The task itself is still loaded from
+    // touch_screen_task_mujoco.pb.txt below.
+    cmvr::config::DeviceManagerConfig device_manager_config;
+    device_manager_config.set_name("touch_screen_eye_to_hand_mujoco_test");
+    device_manager_config.set_version("test");
+
+    auto add_device = [&](const char* id,
+                          cmvr::config::DeviceConfigEntry::DeviceType type,
+                          const char* config_file) {
+        auto* entry = device_manager_config.add_devices();
+        entry->set_id(id);
+        entry->set_type(type);
+        entry->set_config_file(config_file);
+        entry->set_enable(true);
+    };
+
+    add_device("mujoco_world",
+               cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD,
+               "devices/mujoco/right_arm_eye_to_hand_world.pb.txt");
+    add_device("right_arm_mujoco_motors",
+               cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
+               "devices/motor/mujoco_motors.pb.txt");
+    add_device("mujoco_right_arm",
+               cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM,
+               "devices/arm/arm_mujoco.pb.txt");
+    add_device("mujoco_zero_touch_dexhand",
+               cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND,
+               "devices/dexhand/dexhand.pb.txt");
+    add_device("mujoco_hand_cam",
+               cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA,
+               "devices/camera/camera.pb.txt");
+    add_device("mujoco_external_touch_cam",
+               cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA,
+               "devices/camera/camera.pb.txt");
+
+    auto& device_manager =
+        cmvr::device::DeviceManager::getInstance(device_manager_config);
+    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
+    auto hand_camera = device_manager.getDevice<cmvr::device::AbstractCamera>(
+        "mujoco_hand_cam");
+    auto external_camera = device_manager.getDevice<cmvr::device::AbstractCamera>(
+        "mujoco_external_touch_cam");
+    ASSERT_NE(arm, nullptr);
+    ASSERT_NE(hand_camera, nullptr);
+    ASSERT_NE(external_camera, nullptr);
+
+    const auto hand_mujoco_camera =
+        std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(hand_camera);
+    const auto external_mujoco_camera =
+        std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(external_camera);
+    ASSERT_NE(hand_mujoco_camera, nullptr);
+    ASSERT_NE(external_mujoco_camera, nullptr);
+
+    device_manager.start();
+
+    cv::Mat hand_camera_frame;
+    cmvr::device::Rs2Intrinsics hand_camera_intrinsics{};
+    hand_camera->getRGBImage(hand_camera_frame, hand_camera_intrinsics);
+    ASSERT_FALSE(hand_camera_frame.empty())
+        << "mujoco_hand_cam did not produce a frame after DeviceManager::start()";
+
+    cv::Mat external_camera_frame;
+    cmvr::device::Rs2Intrinsics external_camera_intrinsics{};
+    external_camera->getRGBImage(external_camera_frame, external_camera_intrinsics);
+    ASSERT_FALSE(external_camera_frame.empty())
+        << "mujoco_external_touch_cam did not produce a frame after DeviceManager::start()";
+
+    auto world = cmvr::device::MotorManager::mujocoWorldFor(
+        "right_arm_mujoco_motors");
+    ASSERT_NE(world, nullptr);
+    ASSERT_TRUE(world->isLoaded());
+    ASSERT_TRUE(world->isRunning());
+
+    cmvr::MuJocoViewer viewer(world);
+    ASSERT_NE(viewer.model(), nullptr);
+    ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0);
+    ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0);
+    viewer.setupCamera(2.5, -160.0, -25.0);
+
+    // PiP is display-only. Its visibility and layout follow camera.pb.txt;
+    // camera acquisition still uses each MujocoCamera's offscreen thread.
+    bool first_pip_camera = true;
+    const auto add_configured_pip = [&](const cmvr::device::MujocoCamera& camera) {
+        const auto& camera_config = camera.config();
+        const auto& pip = camera_config.viewer_pip();
+        if (!pip.enable()) {
+            return;
+        }
+
+        const auto& render = camera_config.render();
+        if (first_pip_camera) {
+            viewer.enablePiPCamera(camera_config.camera_name().c_str(),
+                                   pip.left(),
+                                   pip.bottom(),
+                                   pip.width(),
+                                   pip.height(),
+                                   render.width(),
+                                   render.height());
+            first_pip_camera = false;
+        } else {
+            viewer.addPiPCamera(camera_config.camera_name().c_str(),
+                                pip.left(),
+                                pip.bottom(),
+                                pip.width(),
+                                pip.height(),
+                                render.width(),
+                                render.height());
+        }
+    };
+    add_configured_pip(*external_mujoco_camera);
+    add_configured_pip(*hand_mujoco_camera);
+
+    struct Outcome {
+        bool init_ok{false};
+        bool touch_ok{false};
+        bool align_reached{false};
+        bool retracting_seen{false};
+        bool finished{false};
+        cmvr::task::TouchScreenTask::Phase final_phase{
+            cmvr::task::TouchScreenTask::Phase::IDLE};
+        cmvr::task::TouchScreenTask::Status final_status{
+            cmvr::task::TouchScreenTask::Status::IDLE};
+        double max_pressure{0.0};
+        double speedl_command_norm{0.0};
+        int active_tag_id{-1};
+        std::string error;
+    } outcome;
+
+    // The viewer owns the lifetime of this test. Closing its window asks the
+    // worker to stop; a completed touch cycle must not close the viewer.
+    std::atomic<bool> stop_requested{false};
+    std::thread scenario([&] {
+        try {
+            const auto touch_config = loadMujocoTouchConfig(project_root);
+            if (touch_config.devices().arm_id() != "mujoco_right_arm" ||
+                touch_config.devices().camera_id() != "mujoco_hand_cam" ||
+                touch_config.devices().external_camera_id() !=
+                    "mujoco_external_touch_cam") {
+                throw std::runtime_error(
+                    "touch_screen_task_mujoco.pb.txt has unexpected device IDs");
+            }
+
+            cmvr::config::TaskManagerConfig task_manager_config;
+            auto* task_entry = task_manager_config.add_tasks();
+            task_entry->set_id(touch_config.id());
+            task_entry->set_type(
+                cmvr::config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN);
+            task_entry->set_run_mode(
+                cmvr::config::TaskConfigEntry::TASK_RUN_MODE_PERIODIC_STEP);
+            task_entry->set_control_period_s(0.001);
+            task_entry->set_config_file(
+                "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt");
+            task_entry->set_enable(true);
+
+            auto& task_manager =
+                cmvr::task::TaskManager::getInstance(task_manager_config);
+            auto task = task_manager.getTouchScreenTask(touch_config.id());
+            if (!task) {
+                throw std::runtime_error("TouchScreenTask not found: " +
+                                         touch_config.id());
+            }
+            outcome.init_ok =
+                task->lastStatus() !=
+                cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED;
+            if (!outcome.init_ok) {
+                throw std::runtime_error("TouchScreenTask initialization failed");
+            }
+
+            task_manager.startRunTask();
+
+            const auto& render = hand_mujoco_camera->config().render();
+            const int target_u = render.width() > 0 ? render.width() / 2 : 640;
+            const int target_v = render.height() > 0 ? render.height() / 2 : 360;
+            std::cout << "[TouchScreenTaskMujocoTest] touch target pixel: u="
+                      << target_u << ", v=" << target_v << std::endl;
+
+            const bool touch_started = task->touch(target_u, target_v);
+            if (!touch_started) {
+                throw std::runtime_error(
+                    "task->touch failed, status=" +
+                    std::string(cmvr::task::TouchScreenTask::statusToString(
+                        task->lastStatus())));
+            }
+
+            outcome.touch_ok = true;
+            std::cout << "[TouchScreenTaskMujocoTest] touch started once"
+                      << std::endl;
+
+            const auto deadline =
+                std::chrono::steady_clock::now() + std::chrono::seconds(35);
+            int step_count = 0;
+            bool paused_at_alignment = false;
+            while (!stop_requested.load(std::memory_order_acquire) &&
+                   task->isBusy() &&
+                   std::chrono::steady_clock::now() < deadline) {
+                outcome.final_phase = task->phase();
+                outcome.final_status = task->lastStatus();
+                outcome.active_tag_id = task->lastActiveTagId();
+                outcome.max_pressure = std::max(
+                    outcome.max_pressure, task->lastTouchPressureSum());
+
+                const auto speedl = arm->getSpeedLCommandTwistBase();
+                outcome.speedl_command_norm = std::max(
+                    outcome.speedl_command_norm,
+                    std::sqrt(speedl.vx * speedl.vx + speedl.vy * speedl.vy +
+                              speedl.vz * speedl.vz));
+                const auto phase = task->phase();
+                if (task->lastStatus() ==
+                        cmvr::task::TouchScreenTask::Status::ALIGN_REACHED ||
+                    phase == cmvr::task::TouchScreenTask::Phase::TOUCHING ||
+                    phase == cmvr::task::TouchScreenTask::Phase::DWELLING ||
+                    phase == cmvr::task::TouchScreenTask::Phase::RETRACTING ||
+                    phase == cmvr::task::TouchScreenTask::Phase::DONE) {
+                    outcome.align_reached = true;
+                }
+                if (phase == cmvr::task::TouchScreenTask::Phase::RETRACTING) {
+                    outcome.retracting_seen = true;
+                }
+                if (phase == cmvr::task::TouchScreenTask::Phase::ALIGN_REACHED &&
+                    touch_config.alignment().pause_when_reached()) {
+                    paused_at_alignment = true;
+                    break;
+                }
+                if ((step_count++ % 20) == 0) {
+                    std::cout << "[TouchScreenTaskMujocoTest] phase="
+                              << cmvr::task::TouchScreenTask::phaseToString(
+                                     task->phase())
+                              << ", status="
+                              << cmvr::task::TouchScreenTask::statusToString(
+                                     task->lastStatus())
+                              << ", active_tag=" << task->lastActiveTagId()
+                              << ", tcp_pose="
+                              << cmvr::perception::TagRelativeTcpPose::statusToString(
+                                     task->tcpPoseTracker().lastStatus())
+                              << ", pressure=" << task->lastTouchPressureSum()
+                              << std::endl;
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            }
+
+            if (stop_requested.load(std::memory_order_acquire)) {
+                task->stop();
+            } else if (task->isBusy() && !paused_at_alignment) {
+                task->stop();
+                throw std::runtime_error("touch scenario exceeded 35 seconds");
+            }
+
+            outcome.finished = task->isFinished();
+            outcome.final_phase = task->phase();
+            outcome.final_status = task->lastStatus();
+            std::cout << "[TouchScreenTaskMujocoTest] touch finished once, phase="
+                      << cmvr::task::TouchScreenTask::phaseToString(
+                             outcome.final_phase)
+                      << "; viewer remains running" << std::endl;
+
+            // Keep the viewer and its camera frames alive after the task has
+            // run once. The main thread sets stop_requested when the window
+            // is closed.
+            while (!stop_requested.load(std::memory_order_acquire)) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            }
+
+            task->stop();
+            task_manager.stopRunTask();
+            std::cout << "[TouchScreenTaskMujocoTest] viewer is still running; "
+                         "close the MuJoCo window to finish gtest." << std::endl;
+        } catch (const std::exception& error) {
+            try {
+                cmvr::task::TaskManager::getInstance().stopRunTask();
+            } catch (...) {
+            }
+            outcome.error = error.what();
+            stop_requested.store(true, std::memory_order_release);
+            std::cerr << "[TouchScreenTaskMujocoTest] scenario error: "
+                      << outcome.error
+                      << "; viewer remains open, close it manually to finish gtest."
+                      << std::endl;
+        }
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    stop_requested.store(true, std::memory_order_release);
+    scenario.join();
+
+    cmvr::task::TaskManager::destroyInstance();
+    device_manager.stop();
+    cmvr::device::DeviceManager::destroyInstance();
+
+    EXPECT_TRUE(outcome.error.empty()) << outcome.error;
+    EXPECT_TRUE(outcome.init_ok);
+    EXPECT_TRUE(outcome.touch_ok);
+    EXPECT_TRUE(outcome.align_reached);
+    EXPECT_GT(outcome.speedl_command_norm, 0.005);
+}
+
+TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
     const auto project_root = findProjectRoot();
     ASSERT_FALSE(project_root.empty());
     cmvr::ConfigHelper::setConfigRootFromFile(
@@ -373,7 +747,8 @@ TEST(TouchScreenTaskTest, 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();
@@ -395,7 +770,8 @@ TEST(TouchScreenTaskTest, 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");

model/xiaoyan_description/right_arm_eye_to_hand.xml

@@ -0,0 +1,809 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<mujoco model="right_arm_eye_to_hand">
+
+    <!-- =========================================================
+         Compiler
+         ========================================================= -->
+    <compiler angle="radian"
+              meshdir="meshes/"/>
+
+    <option timestep="0.001"
+            gravity="0 0 -9.81"
+            integrator="implicitfast"/>
+
+    <default>
+        <joint damping="10"
+               armature="0.001"/>
+    </default>
+
+
+    <!-- =========================================================
+         Visual
+         ========================================================= -->
+    <visual>
+        <global offwidth="1280"
+                offheight="720"/>
+
+        <map znear="0.02"
+             zfar="5"/>
+    </visual>
+
+
+    <!-- =========================================================
+         Assets
+         ========================================================= -->
+    <asset>
+
+        <!-- ================= Right arm meshes ================= -->
+
+        <mesh name="PELVIS_S"
+              file="PELVIS_S.STL"/>
+
+        <mesh name="R_SHOULDER_P_S"
+              file="R_SHOULDER_P_S.STL"/>
+
+        <mesh name="R_SHOULDER_R_S"
+              file="R_SHOULDER_R_S.STL"/>
+
+        <mesh name="R_SHOULDER_Y_S"
+              file="R_SHOULDER_Y_S.STL"/>
+
+        <mesh name="R_ELBOW_R_S"
+              file="R_ELBOW_R_S.STL"/>
+
+        <mesh name="R_WRIST_P_S"
+              file="R_WRIST_P_S.STL"/>
+
+        <mesh name="R_WRIST_Y_S"
+              file="R_WRIST_Y_S.STL"/>
+
+        <mesh name="R_WRIST_R_S"
+              file="R_WRIST_R_S.STL"/>
+
+
+        <!-- ================= Skybox ================= -->
+
+        <texture name="skybox"
+                 type="skybox"
+                 builtin="gradient"
+                 rgb1="0.4 0.6 0.8"
+                 rgb2="0 0 0"
+                 width="512"
+                 height="512"/>
+
+
+        <!-- ================= Floor ================= -->
+
+        <texture name="grid"
+                 type="2d"
+                 builtin="checker"
+                 width="512"
+                 height="512"
+                 rgb1="0.2 0.3 0.4"
+                 rgb2="0.1 0.2 0.3"
+                 mark="cross"
+                 markrgb="0.8 0.8 0.8"/>
+
+        <material name="grid_floor"
+                  texture="grid"
+                  texrepeat="5 5"
+                  rgba="1 1 1 1"
+                  emission="0.9"
+                  specular="0.5"
+                  shininess="1"
+                  reflectance="0.3"/>
+
+
+        <!-- ================= AprilTag =================
+             tag36h11 family
+
+             Hand Tag H:
+                 ID = 0
+                 texture = tag36_11.png
+
+             Screen Tag G:
+                 ID = 1
+                 texture = tag36_11_00001.png
+
+             Both texture planes are 37.5 mm x 37.5 mm. The PNG quiet zone
+             leaves a 30 mm x 30 mm detectable black tag region.
+             =========================================== -->
+
+        <!-- Hand Tag H : tag36h11 ID 0 -->
+        <texture name="hand_apriltag_tex"
+                 type="2d"
+                 file="../april_tag/tag36_11.png"/>
+
+        <!-- The +Z face is the visible face for external_touch_cam. -->
+        <material name="hand_apriltag_mat"
+                  texture="hand_apriltag_tex"
+                  texrepeat="1 1"
+                  rgba="1 1 1 1"
+                  emission="1"
+                  specular="0"
+                  shininess="0"
+                  reflectance="0"/>
+
+        <!-- Screen Tag G : tag36h11 ID 1 -->
+        <texture name="screen_apriltag_tex"
+                 type="2d"
+                 file="../april_tag/tag36_11_00001.png"/>
+
+        <material name="screen_apriltag_mat"
+                  texture="screen_apriltag_tex"
+                  texrepeat="-1 -1"
+                  rgba="1 1 1 1"
+                  emission="1"
+                  specular="0"
+                  shininess="0"
+                  reflectance="0"/>
+
+
+        <!-- ================= Screen material ================= -->
+
+        <material name="screen_material"
+                  rgba="0.04 0.05 0.06 1"
+                  specular="0.25"
+                  shininess="0.4"/>
+
+        <material name="screen_frame_material"
+                  rgba="0.15 0.15 0.15 1"
+                  specular="0.2"
+                  shininess="0.3"/>
+
+    </asset>
+
+
+    <!-- =========================================================
+         World
+         ========================================================= -->
+
+    <worldbody>
+
+
+        <!-- =====================================================
+             Ground
+             ===================================================== -->
+
+        <geom name="floor"
+              type="plane"
+              pos="0 0 0"
+              size="0 0 0.05"
+              material="grid_floor"
+              condim="3"
+              friction="1 0.005 0.0001"/>
+
+
+        <!-- =====================================================
+             Robot support
+             ===================================================== -->
+
+        <geom name="robot_stand_visual"
+              size="0.05 0.6"
+              pos="0 0 0.6"
+              type="cylinder"
+              contype="0"
+              conaffinity="0"
+              group="1"
+              density="0"
+              rgba="0.25 0.25 0.28 1"/>
+
+
+        <!-- =====================================================
+             Fixed touch screen
+
+             screen center:
+                 world = [0, -1.05, 1.25]
+
+             screen size:
+                 width  = 500 mm
+                 height = 300 mm
+
+             Front surface points toward +Y,
+             i.e. toward robot.
+
+             Screen plane for Eye-to-Hand:
+                 approximately y = -1.045
+
+             normal toward robot:
+                 n_world = [0, 1, 0]
+             ===================================================== -->
+
+        <body name="touch_screen"
+              pos="0.62 -0.2 0.95"
+              euler="0 0.1 1.57">
+
+            <!-- Back/frame -->
+            <geom name="screen_frame"
+                  type="box"
+                  size="0.265 0.012 0.165"
+                  material="screen_frame_material"
+                  contype="1"
+                  conaffinity="1"/>
+
+            <!-- Actual screen glass -->
+            <geom name="screen_surface"
+                  type="box"
+                  pos="0 0.013 0"
+                  size="0.25 0.002 0.15"
+                  material="screen_material"
+                  contype="1"
+                  conaffinity="1"
+                  friction="0.5 0.005 0.0001"/>
+
+            <!--
+                Touch surface center.
+
+                This is the point that should be used later
+                when defining the screen plane.
+            -->
+            <site name="screen_center"
+                  pos="0 0.015 0"
+                  size="0.006"
+                  rgba="1 0 0 1"/>
+
+            <!-- Four visual reference points -->
+            <site name="screen_top_left"
+                  pos="-0.25 0.015 0.15"
+                  size="0.004"
+                  rgba="1 1 0 1"/>
+
+            <site name="screen_top_right"
+                  pos="0.25 0.015 0.15"
+                  size="0.004"
+                  rgba="1 1 0 0"/>
+
+            <site name="screen_bottom_left"
+                  pos="-0.25 0.015 -0.15"
+                  size="0.004"
+                  rgba="1 1 0 1"/>
+
+            <site name="screen_bottom_right"
+                  pos="0.25 0.015 -0.15"
+                  size="0.004"
+                  rgba="1 1 0 1"/>
+
+
+            <!-- =================================================
+                 Screen AprilTag G : tag36h11 ID 1
+
+                 Screen local frame:
+                   - screen plane axes are local X and Z
+                   - screen front normal is local +Y
+
+                 The screen front surface is at local y = 0.015 m.
+                 The tag is placed at the upper-right corner and
+                 lifted by 0.1 mm to avoid z-fighting.
+
+                 Texture plane size:
+                     37.5 mm x 37.5 mm
+
+                 Detectable black tag size:
+                     30 mm x 30 mm
+                 ================================================= -->
+
+            <body name="SCREEN_APRILTAG"
+                  pos="0.235 0.0151 0.135"
+                  euler="-1.57079632679 0 0">
+
+                <geom name="SCREEN_APRILTAG_GEOM"
+                      type="box"
+                      size="0.01875 0.01875 0.0001"
+                      material="screen_apriltag_mat"
+                      rgba="1 1 1 1"
+                      contype="0"
+                      conaffinity="0"
+                      group="2"/>
+
+                <!-- Non-rendered Screen Tag center Ground Truth -->
+                <site name="SCREEN_APRILTAG_SITE"
+                      pos="0 0 0"
+                      size="0.002"
+                      rgba="0 1 0 0"/>
+
+            </body>
+
+        </body>
+
+
+        <!-- =====================================================
+             Fixed external Eye-to-Hand camera
+
+             IMPORTANT:
+             - The camera is placed slightly to the left of the robot and
+               aimed at the screen center, like a human viewing from the
+               left-front side.
+             - Screen front normal is local +Y.
+             - MuJoCo camera looks along its local -Z.
+
+             The optical axis is deliberately yawed toward the screen
+             center instead of being exactly fronto-parallel.  This keeps
+             the Hand Tag visible while the arm moves in front of the
+             screen.
+             ===================================================== -->
+
+        <body name="external_camera_mount"
+              pos="0.7 -0.2 0.9"
+              euler="0 0.4 1.57">
+
+            <geom name="external_camera_body"
+                  type="box"
+                  pos="0.18 0.55 0"
+                  size="0.025 0.035 0.018"
+                  rgba="0.1 0.1 0.1 1"
+                  contype="0"
+                  conaffinity="0"/>
+
+            <site name="external_touch_cam_site"
+                  pos="0.18 0.55 0"
+                  size="0.008"
+                  rgba="1 0.2 0.2 1"/>
+
+            <camera name="external_touch_cam"
+                    pos="0.18 0.55 0"
+                    xyaxes="
+              -0.950  0.311  0
+               0      0      1
+            "
+                    fovy="50"/>
+
+        </body>
+
+
+        <!-- =====================================================
+             Robot
+             ===================================================== -->
+
+        <body name="PELVIS_S"
+              pos="0 0 1.2">
+
+            <!-- Pelvis visual -->
+            <geom pos="0 0 0"
+                  quat="1 0 0 0"
+                  type="mesh"
+                  contype="0"
+                  conaffinity="0"
+                  group="1"
+                  density="0"
+                  rgba="0.698039 0.698039 0.698039 1"
+                  mesh="PELVIS_S"/>
+
+            <!-- Pelvis collision -->
+            <geom pos="0 0 0"
+                  quat="1 0 0 0"
+                  type="mesh"
+                  rgba="0.698039 0.698039 0.698039 1"
+                  mesh="PELVIS_S"/>
+
+
+            <!-- =================================================
+                 R_SHOULDER_P
+                 ================================================= -->
+
+            <body name="R_SHOULDER_P_S"
+                  pos="0 -0.0945 0.042">
+
+                <inertial
+                        pos="-0.00982259 -0.0704593 -1.1507e-06"
+                        quat="0.706163 0.705933 0.0386962 0.0386741"
+                        mass="0.880738"
+                        diaginertia="0.000584874 0.000465648 0.000443849"/>
+
+                <joint name="R_SHOULDER_P"
+                       pos="0 0 0"
+                       axis="0 -1 0"
+                       range="-3.14 3.14"
+                       actuatorfrcrange="-120 120"/>
+
+                <geom type="mesh"
+                      contype="0"
+                      conaffinity="0"
+                      group="1"
+                      density="0"
+                      rgba="0.890196 0.890196 0.913725 1"
+                      mesh="R_SHOULDER_P_S"/>
+
+                <geom type="mesh"
+                      rgba="0.890196 0.890196 0.913725 1"
+                      mesh="R_SHOULDER_P_S"/>
+
+
+                <!-- ===============================================
+                     R_SHOULDER_R
+                     =============================================== -->
+
+                <body name="R_SHOULDER_R_S"
+                      pos="0.035 -0.0765 0">
+
+                    <inertial
+                            pos="-0.0346025 -0.0917393 1.86281e-08"
+                            quat="0.609323 0.358823 -0.609303 0.358778"
+                            mass="0.594788"
+                            diaginertia="0.000414771 0.000407636 0.000294296"/>
+
+                    <joint name="R_SHOULDER_R"
+                           pos="0 0 0"
+                           axis="1 0 0"
+                           range="-0.78 1.57"
+                           actuatorfrcrange="-120 120"/>
+
+                    <geom type="mesh"
+                          contype="0"
+                          conaffinity="0"
+                          group="1"
+                          density="0"
+                          rgba="0.890196 0.890196 0.913725 1"
+                          mesh="R_SHOULDER_R_S"/>
+
+                    <geom type="mesh"
+                          rgba="0.890196 0.890196 0.913725 1"
+                          mesh="R_SHOULDER_R_S"/>
+
+
+                    <!-- =============================================
+                         R_SHOULDER_Y
+                         ============================================= -->
+
+                    <body name="R_SHOULDER_Y_S"
+                          pos="-0.035 -0.1475 0">
+
+                        <inertial
+                                pos="-0.00440977 -0.086362 -7.58792e-09"
+                                quat="0.705001 0.704998 0.054559 0.0545441"
+                                mass="0.563406"
+                                diaginertia="0.000329815 0.000297341 0.000211019"/>
+
+                        <joint name="R_SHOULDER_Y"
+                               pos="0 0 0"
+                               axis="0 -1 0"
+                               range="-3.14 3.14"
+                               actuatorfrcrange="-80 80"/>
+
+                        <geom type="mesh"
+                              contype="0"
+                              conaffinity="0"
+                              group="1"
+                              density="0"
+                              rgba="0.890196 0.890196 0.913725 1"
+                              mesh="R_SHOULDER_Y_S"/>
+
+                        <geom type="mesh"
+                              rgba="0.890196 0.890196 0.913725 1"
+                              mesh="R_SHOULDER_Y_S"/>
+
+
+                        <!-- ===========================================
+                             R_ELBOW_R
+                             =========================================== -->
+
+                        <body name="R_ELBOW_R_S"
+                              pos="0.034 -0.1025 0">
+
+                            <inertial
+                                    pos="-0.0335624 -0.06032 -2.97736e-07"
+                                    quat="0.674756 0.674714 -0.211333 -0.211667"
+                                    mass="0.393572"
+                                    diaginertia="0.000189078 0.000181042 0.000139034"/>
+
+                            <joint name="R_ELBOW_R"
+                                   pos="0 0 0"
+                                   axis="1 0 0"
+                                   range="0 2.05"
+                                   actuatorfrcrange="-80 80"/>
+
+                            <geom type="mesh"
+                                  contype="0"
+                                  conaffinity="0"
+                                  group="1"
+                                  density="0"
+                                  rgba="0.890196 0.890196 0.913725 1"
+                                  mesh="R_ELBOW_R_S"/>
+
+                            <geom type="mesh"
+                                  rgba="0.890196 0.890196 0.913725 1"
+                                  mesh="R_ELBOW_R_S"/>
+
+
+                            <!-- =========================================
+                                 R_WRIST_P
+                                 ========================================= -->
+
+                            <body name="R_WRIST_P_S"
+                                  pos="-0.034 -0.0965 0">
+
+                                <inertial
+                                        pos="-1.39657e-10 -0.0675973 0.0192006"
+                                        quat="0.530481 0.467537 -0.467537 0.530481"
+                                        mass="0.442332"
+                                        diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+
+                                <joint name="R_WRIST_P"
+                                       pos="0 0 0"
+                                       axis="0 -1 0"
+                                       range="-3.14 3.14"
+                                       actuatorfrcrange="-50 50"/>
+
+                                <geom type="mesh"
+                                      contype="0"
+                                      conaffinity="0"
+                                      group="1"
+                                      density="0"
+                                      rgba="0.647059 0.619608 0.588235 1"
+                                      mesh="R_WRIST_P_S"/>
+
+                                <geom type="mesh"
+                                      rgba="0.647059 0.619608 0.588235 1"
+                                      mesh="R_WRIST_P_S"/>
+
+
+                                <!-- =======================================
+                                     R_WRIST_Y
+                                     ======================================= -->
+
+                                <body name="R_WRIST_Y_S"
+                                      pos="0 -0.1525 0.039">
+
+                                    <inertial
+                                            pos="-0.00464136 -5.06426e-10 -0.0341254"
+                                            quat="0.298107 0.641196 0.641196 0.298107"
+                                            mass="0.235738"
+                                            diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+
+                                    <joint name="R_WRIST_Y"
+                                           pos="0 0 0"
+                                           axis="0 0 1"
+                                           range="-0.78 0.78"
+                                           actuatorfrcrange="-50 50"/>
+
+                                    <geom type="mesh"
+                                          contype="0"
+                                          conaffinity="0"
+                                          group="1"
+                                          density="0"
+                                          rgba="0.647059 0.619608 0.588235 1"
+                                          mesh="R_WRIST_Y_S"/>
+
+                                    <geom type="mesh"
+                                          rgba="0.647059 0.619608 0.588235 1"
+                                          mesh="R_WRIST_Y_S"/>
+
+
+                                    <!-- =====================================
+                                         R_WRIST_R / hand
+                                         ===================================== -->
+
+                                    <body name="R_WRIST_R_S"
+                                          pos="0.03 0 -0.039">
+
+                                        <inertial
+                                                pos="-0.0201642 -0.11075 -0.00598955"
+                                                quat="0.483404 0.529786 -0.463203 0.520663"
+                                                mass="0.504366"
+                                                diaginertia="0.00027189 0.000186086 0.000130629"/>
+
+                                        <joint name="R_WRIST_R"
+                                               pos="0 0 0"
+                                               axis="1 0 0"
+                                               range="-0.26 1.57"
+                                               actuatorfrcrange="-50 50"/>
+
+
+                                        <!-- ===============================
+                                             Wrist / hand mesh
+                                             =============================== -->
+
+                                        <geom type="mesh"
+                                              contype="0"
+                                              conaffinity="0"
+                                              group="1"
+                                              density="0"
+                                              rgba="0.890196 0.890196 0.913725 1"
+                                              mesh="R_WRIST_R_S"/>
+
+                                        <geom type="mesh"
+                                              rgba="0.890196 0.890196 0.913725 1"
+                                              mesh="R_WRIST_R_S"/>
+
+
+                                        <!-- =================================================
+                                             Robot TCP
+
+                                             Same location as original:
+                                             R_FINGER_TIP_FIXED
+
+                                             ================================================= -->
+
+                                        <site name="R_FINGER_TIP_SITE"
+                                              pos="0.00684256 -0.284077 0.00801525"
+                                              size="0.006"
+                                              rgba="0 1 1 1"/>
+
+                                        <!-- Hand-mounted camera copied from dual_arm.xml. -->
+                                        <camera name="hand_cam"
+                                                pos="-0.01212 -0.17655 0.07506"
+                                                quat="3.17467e-11 -3.17467e-11 0.707107 0.707107"
+                                                fovy="60"/>
+
+
+                                        <!-- fingertip visual -->
+                                        <geom name="R_FINGER_TIP_VISUAL"
+                                              type="sphere"
+                                              size="0.004"
+                                              pos="0.00684256 -0.284077 0.00801525"
+                                              contype="0"
+                                              conaffinity="0"
+                                              rgba="0 1 1 1"/>
+
+
+                                        <!-- fingertip collision
+                                             for later physical screen contact
+                                        -->
+                                        <geom name="R_FINGER_TIP_COLLISION"
+                                              type="sphere"
+                                              size="0.004"
+                                              pos="0.00684256 -0.284077 0.00801525"
+                                              contype="1"
+                                              conaffinity="1"
+                                              rgba="0 1 1 0.25"/>
+
+
+                                        <!-- =================================================
+                                             Hand AprilTag H : tag36h11 ID 0
+                                             3 cm AprilTag mounted on BACK OF HAND
+
+                                             Texture plane full width:
+                                                 0.0375 m
+
+                                             Detectable black tag width:
+                                                 0.030 m
+
+                                             MuJoCo box size is HALF-size:
+                                                 0.01875 x 0.01875
+
+                                             thickness:
+                                                 0.0006 m
+
+                                             half thickness:
+                                                 0.0003
+
+                                             It is fixed rigidly to R_WRIST_R_S.
+
+                                             Current position is on upper/back side of hand,
+                                             slightly downstream from wrist camera area.
+
+                                             The tag front surface normal is local +Z.
+                                             ================================================= -->
+
+                                        <body name="R_HAND_APRILTAG"
+                                              pos="0.00684256 -0.254077 0.00801525"
+                                              euler="-1.57079632679 0 0">
+
+                                            <geom name="R_HAND_APRILTAG_GEOM"
+                                                  type="box"
+                                                  size="0.01875 0.01875 0.0003"
+                                                  material="hand_apriltag_mat"
+                                                  rgba="1 1 1 1"
+                                                  contype="0"
+                                                  conaffinity="0"
+                                                  group="2"/>
+
+                                            <!-- Non-rendered Hand Tag center Ground Truth -->
+                                            <site name="R_HAND_APRILTAG_SITE"
+                                                  pos="0 0 0"
+                                                  size="0.002"
+                                                  rgba="1 0 1 0"/>
+
+                                        </body>
+
+
+                                    </body>
+                                </body>
+                            </body>
+                        </body>
+                    </body>
+                </body>
+            </body>
+        </body>
+
+
+        <!-- =====================================================
+             Lighting
+             ===================================================== -->
+
+        <light name="top_light"
+               mode="fixed"
+               directional="true"
+               diffuse="0.7 0.7 0.7"
+               specular="0.3 0.3 0.3"
+               pos="0 0 3"
+               dir="0 0 -1"/>
+
+        <!-- Add some front lighting for AprilTag -->
+        <light name="front_light"
+               mode="fixed"
+               directional="false"
+               diffuse="0.5 0.5 0.5"
+               specular="0.1 0.1 0.1"
+               pos="0.4 -0.5 2.0"
+               dir="-0.3 -0.5 -0.4"/>
+
+    </worldbody>
+
+
+    <!-- =========================================================
+         Actuators
+         Keep original RIGHT ARM actuator names.
+
+         This is important because existing:
+             mujoco_motors
+             mujoco_right_arm
+
+         uses these right-arm joints.
+         ========================================================= -->
+
+    <actuator>
+
+        <position name="R_SHOULDER_P_pos"
+                  joint="R_SHOULDER_P"
+                  kp="2000"
+                  ctrlrange="-3.14 3.14"/>
+
+        <position name="R_SHOULDER_R_pos"
+                  joint="R_SHOULDER_R"
+                  kp="2000"
+                  ctrlrange="-0.78 1.57"/>
+
+        <position name="R_SHOULDER_Y_pos"
+                  joint="R_SHOULDER_Y"
+                  kp="2000"
+                  ctrlrange="-3.14 3.14"/>
+
+        <position name="R_ELBOW_R_pos"
+                  joint="R_ELBOW_R"
+                  kp="1500"
+                  ctrlrange="0 2.05"/>
+
+        <position name="R_WRIST_P_pos"
+                  joint="R_WRIST_P"
+                  kp="800"
+                  ctrlrange="-3.14 3.14"/>
+
+        <position name="R_WRIST_Y_pos"
+                  joint="R_WRIST_Y"
+                  kp="800"
+                  ctrlrange="-0.78 0.78"/>
+
+        <position name="R_WRIST_R_pos"
+                  joint="R_WRIST_R"
+                  kp="800"
+                  ctrlrange="-0.26 1.57"/>
+
+    </actuator>
+
+
+    <!-- =========================================================
+         Optional initial pose
+
+         This pose keeps the right arm roughly facing the screen.
+         We can adjust this later after actually running Viewer.
+
+         ========================================================= -->
+
+    <keyframe>
+
+        <key name="home"
+             qpos="
+           0
+           0
+           0
+           0
+           0
+           0
+           0
+         "/>
+
+    </keyframe>
+
+
+</mujoco>

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 {

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 {

protos/cmvr/config/touch_screen_algorithm_config.proto

@@ -11,17 +11,10 @@ message TouchScreenApriltagConfig {
   optional TouchScreenTargetPointMethod target_point_method = 3;
 }
 
-message TouchScreenIbvsConfig {
-  optional string camera_link = 1;
-  optional double lambda = 2;
-  optional double mu = 3;
-  optional double qdot_max = 4;
-  .cmvr.common.Vec6 vmax6 = 5;
-  .cmvr.common.Vec6 amax6 = 6;
-  optional double twist_filter_alpha = 7;
-
-  .cmvr.common.Mat3 r_camera_to_visp = 12;
-  .cmvr.common.Mat3 r_camera_to_urdf = 13;
-
-  repeated string control_joint_names = 14;
+message TouchScreenTaskPbvsConfig {
+  .cmvr.common.Vec3 position_gain = 1;
+  .cmvr.common.Vec3 rotation_gain = 2;
+  .cmvr.common.Vec6 vmax6 = 3;
+  .cmvr.common.Vec6 amax6 = 4;
+  optional double twist_filter_alpha = 5;
 }

protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto

@@ -15,9 +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 {
@@ -26,27 +27,66 @@ 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 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;
+  reserved 1, 2, 6;
+  reserved "apriltag", "external_apriltag", "external_camera";
+
+  TouchScreenTaskTagsConfig tags = 4;
+  TouchScreenTaskHandCameraConfig hand_camera = 5;
 }
 
 message TouchScreenAlignmentTargetConfig {
-  .cmvr.common.Vec3 position_in_camera = 1;
-  .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";
-  TouchScreenIbvsConfig ibvs = 2;
   TouchScreenAlignmentTargetConfig target = 3;
   .cmvr.common.Vec6 error_threshold = 4;
   optional int32 stable_frames = 5;
   optional double timeout_s = 6;
   optional bool pause_when_reached = 7;
+  TouchScreenTaskPbvsConfig pbvs = 8;
+  TouchScreenTaskAlignmentCalibrationConfig calibration = 9;
 }
 
 message TouchScreenTouchSpeedLConfig {
@@ -84,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 {
@@ -95,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 {