feat(touch): add eye-to-hand PBVS MuJoCo support

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/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/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/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/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,7 +5,7 @@ device_manager {
   devices {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD
-    config_file: "devices/mujoco/mujoco_world.pb.txt"
+    config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
     enable: true
   }
 
@@ -37,6 +37,13 @@ device_manager {
     enable: true
   }
 
+  devices {
+    id: "mujoco_external_touch_cam"
+    type: DEVICE_TYPE_CAMERA
+    config_file: "devices/camera/camera.pb.txt"
+    enable: true
+  }
+
   devices {
     id: "right_hand_cam"
     type: DEVICE_TYPE_CAMERA

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

@@ -5,6 +5,7 @@ touch_screen_task {
     arm_id: "right_arm"
     dexhand_id: "paxini_tip_1"
     camera_id: "right_hand_cam"
+    external_camera_id: "cam5"
   }
 
   initialization {
@@ -27,37 +28,27 @@ touch_screen_task {
       depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
       target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
     }
+    external_apriltag {
+      tag_size_m: 0.012
+      hand_tag_id: 1
+      t_h_p {
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
+        m10: 0.0 m11: 1.0 m12: 0.0 m13: -0.03
+        m20: 0.0 m21: 0.0 m22: 1.0 m23: 0.0
+        m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
+      }
+    }
   }
 
   alignment {
-    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 }
+    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 }
       mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
     }

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

@@ -5,6 +5,7 @@ touch_screen_task {
     arm_id: "mujoco_right_arm"
     dexhand_id: "mujoco_zero_touch_dexhand"
     camera_id: "mujoco_hand_cam"
+    external_camera_id: "mujoco_external_touch_cam"
   }
 
   initialization {
@@ -23,42 +24,33 @@ touch_screen_task {
 
   perception {
     apriltag {
-      tag_size_m: 0.12
+      tag_size_m: 0.03
       depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
       target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
     }
+    external_apriltag {
+      tag_size_m: 0.03
+      hand_tag_id: 0
+      # Hand Tag H -> TCP P after flipping the tag to face external_touch_cam.
+      t_h_p {
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
+        m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
+        m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
+        m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
+      }
+    }
   }
 
   alignment {
-    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 }
+    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 }
+      rotation_vector { x: 1.57 y: 0.0 z: 0.0 }
       mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
     }
     error_threshold {
@@ -78,7 +70,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

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;
@@ -453,6 +460,7 @@ bool MujocoCamera::initOffscreen_()
         return true;
     }
 
+    std::lock_guard<std::mutex> glfw_lock(glfwInitMutex());
     if (!glfwInit()) {
         setError_("[MujocoCamera] glfwInit failed");
         return false;

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,17 +114,12 @@ 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 moveToInitPositionBeforeStartIfEnabled();
     bool moveToInitPositionIfEnabled() const;
@@ -136,10 +139,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 +155,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,17 +163,24 @@ 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};
     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_retract_log_time_{};

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,6 +23,7 @@
 #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"
@@ -208,9 +210,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,9 +266,15 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
         const auto& config = root_config.touch_screen_task();
         EXPECT_TRUE(config.has_initialization());
         EXPECT_TRUE(config.has_perception());
+        EXPECT_TRUE(config.perception().has_external_apriltag());
         EXPECT_TRUE(config.has_alignment());
-        EXPECT_TRUE(config.alignment().has_ibvs());
-        EXPECT_TRUE(config.alignment().ibvs().has_camera_link());
+        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);
@@ -274,7 +282,322 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
     }
 }
 
-TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
+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(

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

@@ -18,6 +18,7 @@ message TouchScreenTaskDevicesConfig {
   optional string arm_id = 4;
   optional string dexhand_id = 2;
   optional string camera_id = 3;
+  optional string external_camera_id = 5;
 }
 
 message TouchScreenTaskInitializationConfig {
@@ -28,12 +29,18 @@ message TouchScreenTaskInitializationConfig {
   optional double acceleration = 5;
 }
 
+message TouchScreenTaskExternalApriltagConfig {
+  optional double tag_size_m = 1;
+  optional int32 hand_tag_id = 2;
+  .cmvr.common.Mat4 t_h_p = 3;
+}
+
 message TouchScreenTaskPerceptionConfig {
   TouchScreenApriltagConfig apriltag = 1;
+  TouchScreenTaskExternalApriltagConfig external_apriltag = 2;
 }
 
 message TouchScreenAlignmentTargetConfig {
-  .cmvr.common.Vec3 position_in_camera = 1;
   .cmvr.common.Vec3 rotation_vector = 2;
   optional TouchScreenAlignMode mode = 3;
 }
@@ -41,12 +48,12 @@ message TouchScreenAlignmentTargetConfig {
 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;
 }
 
 message TouchScreenTouchSpeedLConfig {