feat: add touch app

.vscode/extensions.json

@@ -0,0 +1,6 @@
+{
+  "recommendations": [
+    "ms-vscode.cpptools",
+    "ms-vscode.cmake-tools"
+  ]
+}

.vscode/launch.json

@@ -0,0 +1,32 @@
+{
+    "configurations": [
+        
+        {
+            "name": "C/C++: g++ 构建和调试活动文件",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${fileDirname}/${fileBasenameNoExtension}",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "${fileDirname}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "gdb",
+            "setupCommands": [
+                {
+                    "description": "为 gdb 启用整齐打印",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "将反汇编风格设置为 Intel",
+                    "text": "-gdb-set disassembly-flavor intel",
+                    "ignoreFailures": true
+                }
+            ],
+            "preLaunchTask": "C/C++: g++ 生成活动文件",
+            "miDebuggerPath": "/usr/bin/gdb"
+        }
+    ],
+    "version": "2.0.0"
+}

.vscode/settings.json

@@ -0,0 +1,12 @@
+{
+  "cmake.configureOnOpen": true,
+  "cmake.buildDirectory": "${workspaceFolder}/cmake-build-debug",
+  "cmake.copyCompileCommands": "${workspaceFolder}/compile_commands.json",
+  "cmake.configureSettings": {
+    "CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
+  },
+  "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
+  "C_Cpp.default.compileCommands": "${workspaceFolder}/compile_commands.json",
+  "C_Cpp.default.cppStandard": "c++17",
+  "C_Cpp.default.intelliSenseMode": "linux-gcc-x64"
+}

.vscode/tasks.json

@@ -0,0 +1,25 @@
+{
+    "tasks": [
+        {
+            "type": "cppbuild",
+            "label": "C/C++: g++ 生成活动文件",
+            "command": "/usr/bin/g++",
+            "args": [
+                "-fdiagnostics-color=always",
+                "-g",
+                "${file}",
+                "-o",
+                "${fileDirname}/${fileBasenameNoExtension}"
+            ],
+            "options": {
+                "cwd": "${fileDirname}"
+            },
+            "problemMatcher": [
+                "$gcc"
+            ],
+            "group": "build",
+            "detail": "调试器生成的任务。"
+        }
+    ],
+    "version": "2.0.0"
+}

cmvr-es/applications/CMakeLists.txt

@@ -4,8 +4,12 @@ add_library(applications
 
 target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(applications PUBLIC
+target_link_libraries(applications
+        PUBLIC
         cmvr_es::controller
+        cmvr_es::common
+        PRIVATE
+        cmvr_es::device_manager
 )
 
 add_library(cmvr_es::applications ALIAS applications)

cmvr-es/applications/include/touch_screen_app.h

@@ -11,6 +11,7 @@
 
 #include <Eigen/Dense>
 
+#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
 #include "controller/include/ibvs_controller.h"
 #include "devices/camera/abstract_camera.h"
 #include "devices/dexhand/abstract_dexhand.h"
@@ -23,36 +24,37 @@ namespace cmvr::app {
 class TouchScreenApp {
 public:
     enum class Phase {
-        IDLE = 0,
+        IDLE = 0,       // 空闲，尚未开始任务。
-        ALIGNING,
+        ALIGNING,       // 视觉对准阶段：持续 IBVS 对齐目标点。
-        ALIGN_REACHED,
+        ALIGN_REACHED,  // 视觉对准已达到阈值，等待进入下一阶段。
-        TOUCHING,
+        TOUCHING,       // 前进触控阶段：沿设定方向向屏幕推进。
-        DWELLING,
+        DWELLING,       // 已检测到接触，保持当前位置短暂停留。
-        RETRACTING,
+        RETRACTING,     // 回退阶段：沿设定回退方向离开屏幕。
-        DONE,
+        DONE,           // 整个流程成功完成。
-        FAILED
+        FAILED          // 流程失败并已停止。
     };
 
     enum class Status {
-        IDLE = 0,
+        IDLE = 0,                 // 空闲状态。
-        NOT_INITIALIZED,
+        NOT_INITIALIZED,          // 尚未调用 init() 完成初始化。
-        INVALID_CONFIG,
+        INVALID_CONFIG,           // 配置非法，无法启动或应用参数。
-        CONTROL_JOINT_MISMATCH,
+        CONTROL_JOINT_MISMATCH,   // 控制关节顺序与 IK 链不一致。
-        ALIGN_WAITING_PERCEPTION,
+        ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。
-        ALIGN_WAITING_TRACK,
+        ALIGN_WAITING_TRACK,      // 对准阶段等待目标点跟踪恢复成功。
-        ALIGN_TARGET_SETUP_FAILED,
+        ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。
-        ALIGN_COMPUTE_FAILED,
+        ALIGN_COMPUTE_FAILED,     // 对准阶段 IBVS 或 IK 计算失败。
-        ALIGN_TIMEOUT,
+        ALIGN_TIMEOUT,            // 对准阶段超时仍未收敛。
-        ALIGNING,
+        ALIGNING,                 // 正在执行视觉对准。
-        ALIGN_REACHED,
+        ALIGN_REACHED,            // 视觉对准完成。
-        TOUCHING,
+        TOUCHING,                 // 正在向前触控。
-        TOUCH_TRIGGERED,
+        TACTILE_UNAVAILABLE,      // 触觉数据不可用。
-        TOUCH_TIMEOUT,
+        TOUCH_TRIGGERED,          // 已检测到接触触发。
-        RETRACTING,
+        TOUCH_FORWARD_TIMEOUT,    // 前进触控时间到，但未触发接触。
-        DONE,
+        RETRACTING,               // 正在回退离开屏幕。
-        STOPPED,
+        DONE,                     // 流程成功完成。
-        ROBOT_STATE_FAILED,
+        STOPPED,                  // 被外部 stop() 主动停止。
-        ROBOT_COMMAND_FAILED
+        ROBOT_STATE_FAILED,       // 读取机器人状态失败。
+        ROBOT_COMMAND_FAILED      // 向机器人下发控制命令失败。
     };
 
     enum class TactileRegion {
@@ -64,80 +66,141 @@ public:
     };
 
     struct Options {
+        // 是否在触控流程开始前先回到指定初始关节位姿。
+        bool move_to_init_position_before_start{false};
+        // 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。
+        bool move_to_init_position{false};
+        // 初始关节位姿目标，在前置回位或结束后回位开启时使用。
+        std::vector<device::JointPoint> init_joint_positions{};
+        // 回到初始位姿时的 moveJ 主导速度，单位 rad/s。
+        double init_movej_vel{1.0};
+        // 回到初始位姿时的 moveJ 主导加速度，单位 rad/s^2。
+        double init_movej_acc{2.0};
+
         // IBVS / IK 初始化参数。
+        // URDF 文件路径，用于初始化 IbvsController 内部 IK 求解器。
         std::string urdf_path;
+        // IK 链基座 link 名称。
         std::string base_link{"PELVIS_S"};
+        // IK 链末端法兰 link 名称。
         std::string flange_link{"R_WRIST_R_S"};
+        // URDF 中相机 link 名称。
         std::string camera_link;
 
         // 视觉感知参数。
+        // AprilTag 实际边长，单位米。
         double tag_size_m{0.12};
+        // 感知更新时如何使用深度图：不用 / 尽量用 / 必须用。
         perception::AprilTagPerception::DepthPolicy depth_policy{
             perception::AprilTagPerception::DepthPolicy::NONE};
+        // 从像素恢复目标点时采用 tag 平面求交，还是深度图反投影。
         perception::TagRelativeTarget3D::TargetPointMethod target_point_method{
             perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE};
 
         // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。
-        Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.12};
+        // 目标点在相机坐标系中的期望位置，单位米。
+        Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.40};
+        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
         double target_rx{3.14159265358979323846};
+        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
         double target_ry{0.0};
+        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
         double target_rz{0.0};
 
         // IBVS 参数。
+        // 视觉伺服增益 lambda。
         double ibvs_lambda{0.6};
+        // DLS IK 阻尼系数 mu。
         double ibvs_mu{0.1};
-        double ibvs_qdot_max{0.5};
+        // 单关节最大速度，单位 rad/s。
-        std::array<double, 6> ibvs_vmax6{{0.04, 0.04, 0.04, 0.04, 0.04, 0.04}};
+        double ibvs_qdot_max{0.15};
+        // 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。
+        std::array<double, 6> ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
+        // 是否启用关节限位回避。
         bool enable_joint_limit_avoidance{true};
+        // 关节限位回避增益。
         double joint_limit_avoidance_gain{0.2};
-        double joint_limit_avoidance_margin_ratio{0.05};
+        // 距离关节限位多近时开始回避，按关节范围比例计算。
+        double joint_limit_avoidance_margin_ratio{0.15};
+        // 单关节限位回避最大推回速度。
         double joint_limit_avoidance_max_push{0.25};
 
+        // `AbstractCamera` 相机坐标系到 ViSP 相机坐标系的旋转矩阵。
         Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()};
+        // `AbstractCamera` 相机坐标系到 URDF 相机坐标系的旋转矩阵。
         Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()};
 
         // 关节控制链，默认右臂 7 轴。
+        // 顺序必须与 IbvsController 内部 IK 链顺序一致。
         std::vector<std::string> control_joint_names{
             "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
             "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
 
         // 视觉对准收敛判据。
+        // 目标点在相机坐标系 x/y 方向的允许误差，单位米。
         double align_xy_threshold_m{0.003};
+        // 目标点在相机坐标系 z 方向的允许误差，单位米。
         double align_z_threshold_m{0.010};
+        // tag 当前姿态与目标姿态的允许夹角误差，单位弧度。
+        double align_rot_threshold_rad{0.08726646259971647};
+        // 连续多少帧都满足阈值，才认为对准完成。
         int align_stable_frames{5};
+        // 对准阶段超时时间，单位秒。
         double align_timeout_s{10.0};
+        // 为 true 时对准完成后暂停，不自动进入触控阶段。
         bool pause_after_align_reached{false};
 
-        // 触控阶段：speedL 目标 twist（base_link 系）。
+        // 触控阶段：speedL 目标 twist。当前按末端 Tool 坐标系解释，字段名保留兼容。
+        // 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。
+        // 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。
         Eigen::Matrix<double, 6, 1> touch_twist_base{
-            (Eigen::Matrix<double, 6, 1>() << 0.0, 0.0, -0.02, 0.0, 0.0, 0.0).finished()};
+            (Eigen::Matrix<double, 6, 1>() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()};
-        double touch_acceleration{0.6};
+        // 触控阶段 speedL 的加速度参数。
-        double touch_timeout_s{2.0};
+        double touch_acceleration{3.0};
+        // 前进触控阶段的最大持续时间，单位秒。
+        // 该时间由 TouchScreenApp 状态机自行计时，不直接传给 robot->speedL(time)。
+        // 大于 0 时，达到该时间后无论压力是否达阈值，都会立即进入回退阶段。
+        // 小于等于 0 时，表示不启用这条限制。
+        double touch_forward_duration_s{1.0};
 
-        // 接触后停留与回退。
+        // 接触后停留与回退。当前按末端 Tool 坐标系解释。
+        // 检测到接触后在当前位置停留的时间，单位秒。
+        // 当该值小于 0 时，表示不做停留，直接把 speedL 切换为回退。
         double dwell_time_s{0.05};
+        // 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。
+        // 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。
         Eigen::Matrix<double, 6, 1> retract_twist_base{
-            (Eigen::Matrix<double, 6, 1>() << 0.0, 0.0, 0.03, 0.0, 0.0, 0.0).finished()};
+            (Eigen::Matrix<double, 6, 1>() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()};
-        double retract_acceleration{0.8};
+        // 回退阶段 speedL 的加速度参数。
-        double retract_duration_s{0.20};
+        double retract_acceleration{3.0};
+        // 回退阶段持续时间，单位秒。
+        double retract_duration_s{0.8};
 
         // 指尖触觉判据。
+        // 使用哪根手指的触觉阵列判断是否接触。
         device::FingerType tactile_finger{device::FingerType::INDEX};
-        TactileRegion tactile_region{TactileRegion::FINGER};
+        // 使用该手指的哪个触觉区域。
-        double tactile_pressure_sum_threshold{3000.0};
+        TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER};
+        // 触觉压力和阈值；总和超过该值认为已经接触。
+        double tactile_pressure_sum_threshold{100.0};
+        // 触觉峰值阈值；为 0 时表示不使用峰值判据。
         double tactile_pressure_peak_threshold{0.0};
     };
 
     TouchScreenApp();
     ~TouchScreenApp() = default;
 
+    bool init();
+    bool init(const std::shared_ptr<device::AbstractRobot>& robot,
+              const std::shared_ptr<device::AbstractDexHand>& dexhand,
+              const std::shared_ptr<device::AbstractCamera>& camera);
     bool init(const std::shared_ptr<device::AbstractRobot>& robot,
               const std::shared_ptr<device::AbstractDexHand>& dexhand,
               const std::shared_ptr<device::AbstractCamera>& camera,
-              const Options& options);
+              const cmvr::config::TouchScreenAppConfig& config);
 
-    void setOptions(const Options& options);
+    bool setOptionsFromConfig();
-    const Options& options() const { return options_; }
+    bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config);
 
     bool startFromPixel(int u, int v);
     bool step();
@@ -168,6 +231,9 @@ public:
 private:
     using Clock = std::chrono::steady_clock;
 
+    static bool optionsFromConfig(const cmvr::config::TouchScreenAppConfig& config,
+                                  Options& options_out);
+    void setOptions(const Options& options);
     bool applyOptions();
     bool validateControlJointNames() const;
     bool stepAligning();
@@ -179,6 +245,7 @@ private:
     bool sendJointVelocity(const std::vector<double>& qdot) const;
     bool sendZeroJointVelocity() const;
     bool holdCurrentControlledPosition() const;
+    bool moveToInitPositionIfEnabled() const;
 
     bool startTouchPhase();
     bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);

cmvr-es/applications/src/touch_screen_app.cpp

@@ -3,8 +3,13 @@
 #include <algorithm>
 #include <cmath>
 #include <exception>
+#include <limits>
 #include <unordered_map>
 
+#include "common/utils/config_helper/include/config_helper.h"
+#include "device_manager/include/device_manager.h"
+#include <visp3/core/vpRotationMatrix.h>
+
 namespace cmvr::app {
 
 namespace {
@@ -29,15 +34,250 @@ void accumulateMatrixStats(const std::vector<std::vector<device::TactilePoint>>&
     }
 }
 
+bool requiresReinitForOptionChange(const TouchScreenApp::Options& current,
+                                   const TouchScreenApp::Options& requested) {
+    const auto init_joints_differ = [&]() {
+        if (current.init_joint_positions.size() != requested.init_joint_positions.size()) {
+            return true;
+        }
+        for (size_t i = 0; i < current.init_joint_positions.size(); ++i) {
+            if (current.init_joint_positions[i].joint_name != requested.init_joint_positions[i].joint_name ||
+                current.init_joint_positions[i].rad != requested.init_joint_positions[i].rad) {
+                return true;
+            }
+        }
+        return false;
+    };
+    return current.move_to_init_position_before_start != requested.move_to_init_position_before_start ||
+           current.move_to_init_position != requested.move_to_init_position ||
+           init_joints_differ() ||
+           current.init_movej_vel != requested.init_movej_vel ||
+           current.init_movej_acc != requested.init_movej_acc ||
+           current.urdf_path != requested.urdf_path ||
+           current.base_link != requested.base_link ||
+           current.flange_link != requested.flange_link ||
+           current.camera_link != requested.camera_link ||
+           current.control_joint_names != requested.control_joint_names;
+}
+
+Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) {
+    vpRotationMatrix R_visp;
+    R_visp.buildFrom(rx, ry, rz);
+
+    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
+    for (int r = 0; r < 3; ++r) {
+        for (int c = 0; c < 3; ++c) {
+            R(r, c) = R_visp[r][c];
+        }
+    }
+    return R;
+}
+
+double rotationErrorRad(const Eigen::Matrix3d& R_current,
+                        const Eigen::Matrix3d& R_target) {
+    if (!R_current.allFinite() || !R_target.allFinite()) {
+        return std::numeric_limits<double>::infinity();
+    }
+
+    const Eigen::Matrix3d R_err = R_current * R_target.transpose();
+    const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0);
+    return std::acos(cos_angle);
+}
+
+device::TactileRegion toDeviceTactileRegion(const TouchScreenApp::TactileRegion region) {
+    switch (region) {
+        case TouchScreenApp::TactileRegion::TIP: return device::TactileRegion::TIP;
+        case TouchScreenApp::TactileRegion::FINGER: return device::TactileRegion::FINGER;
+        case TouchScreenApp::TactileRegion::PAD: return device::TactileRegion::PAD;
+        case TouchScreenApp::TactileRegion::TIP_AND_FINGER:
+            return device::TactileRegion::TIP_AND_FINGER;
+        case TouchScreenApp::TactileRegion::THUMB_MIDDLE:
+            return device::TactileRegion::THUMB_MIDDLE;
+    }
+    return device::TactileRegion::TIP;
+}
+
+perception::AprilTagPerception::DepthPolicy toDepthPolicy(
+    const cmvr::config::TouchScreenDepthPolicy policy) {
+    switch (policy) {
+        case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_NONE:
+            return perception::AprilTagPerception::DepthPolicy::NONE;
+        case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_PREFER:
+            return perception::AprilTagPerception::DepthPolicy::PREFER;
+        case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_REQUIRE:
+            return perception::AprilTagPerception::DepthPolicy::REQUIRE;
+    }
+    return perception::AprilTagPerception::DepthPolicy::NONE;
+}
+
+perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod(
+    const cmvr::config::TouchScreenTargetPointMethod method) {
+    switch (method) {
+        case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE:
+            return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
+        case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE:
+            return perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE;
+    }
+    return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
+}
+
+device::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) {
+    switch (finger) {
+        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY:
+            return device::FingerType::PINKY;
+        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING:
+            return device::FingerType::RING;
+        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE:
+            return device::FingerType::MIDDLE;
+        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX:
+            return device::FingerType::INDEX;
+        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB:
+            return device::FingerType::THUMB;
+    }
+    return device::FingerType::INDEX;
+}
+
+TouchScreenApp::TactileRegion toTactileRegion(
+    const cmvr::config::TouchScreenTactileRegion region) {
+    switch (region) {
+        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP:
+            return TouchScreenApp::TactileRegion::TIP;
+        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER:
+            return TouchScreenApp::TactileRegion::FINGER;
+        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD:
+            return TouchScreenApp::TactileRegion::PAD;
+        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER:
+            return TouchScreenApp::TactileRegion::TIP_AND_FINGER;
+        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE:
+            return TouchScreenApp::TactileRegion::THUMB_MIDDLE;
+    }
+    return TouchScreenApp::TactileRegion::TIP;
+}
+
+void applyVec3FromConfig(const cmvr::config::TouchScreenVec3& src,
+                         Eigen::Vector3d& dst) {
+    if (src.has_x()) {
+        dst.x() = src.x();
+    }
+    if (src.has_y()) {
+        dst.y() = src.y();
+    }
+    if (src.has_z()) {
+        dst.z() = src.z();
+    }
+}
+
+void applyTwist6FromConfig(const cmvr::config::TouchScreenTwist6& src,
+                           Eigen::Matrix<double, 6, 1>& dst) {
+    if (src.has_vx()) {
+        dst[0] = src.vx();
+    }
+    if (src.has_vy()) {
+        dst[1] = src.vy();
+    }
+    if (src.has_vz()) {
+        dst[2] = src.vz();
+    }
+    if (src.has_wx()) {
+        dst[3] = src.wx();
+    }
+    if (src.has_wy()) {
+        dst[4] = src.wy();
+    }
+    if (src.has_wz()) {
+        dst[5] = src.wz();
+    }
+}
+
+void applyMatrix3dFromConfig(const cmvr::config::TouchScreenMatrix3d& src,
+                             Eigen::Matrix3d& dst) {
+    if (src.has_m00()) {
+        dst(0, 0) = src.m00();
+    }
+    if (src.has_m01()) {
+        dst(0, 1) = src.m01();
+    }
+    if (src.has_m02()) {
+        dst(0, 2) = src.m02();
+    }
+    if (src.has_m10()) {
+        dst(1, 0) = src.m10();
+    }
+    if (src.has_m11()) {
+        dst(1, 1) = src.m11();
+    }
+    if (src.has_m12()) {
+        dst(1, 2) = src.m12();
+    }
+    if (src.has_m20()) {
+        dst(2, 0) = src.m20();
+    }
+    if (src.has_m21()) {
+        dst(2, 1) = src.m21();
+    }
+    if (src.has_m22()) {
+        dst(2, 2) = src.m22();
+    }
+}
+
 } // namespace
 
 TouchScreenApp::TouchScreenApp()
     : tracker_(nullptr) {}
 
+bool TouchScreenApp::init() {
+    cmvr::config::TouchScreenAppConfig config;
+    if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    if (!config.has_robot_id() || config.robot_id().empty() ||
+        !config.has_camera_id() || config.camera_id().empty() ||
+        !config.has_dexhand_id() || config.dexhand_id().empty()) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    try {
+        auto& dm = device::DeviceManager::getInstance();
+        auto robot = dm.getDevice<device::AbstractRobot>(config.robot_id());
+        auto dexhand = dm.getDevice<device::AbstractDexHand>(config.dexhand_id());
+        auto camera = dm.getDevice<device::AbstractCamera>(config.camera_id());
+        camera->start();
+        return init(robot, dexhand, camera, config);
+    } catch (...) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+}
+
+bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
+                          const std::shared_ptr<device::AbstractDexHand>& dexhand,
+                          const std::shared_ptr<device::AbstractCamera>& camera) {
+    cmvr::config::TouchScreenAppConfig config;
+    if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+    return init(robot, dexhand, camera, config);
+}
+
 bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
                           const std::shared_ptr<device::AbstractDexHand>& dexhand,
                           const std::shared_ptr<device::AbstractCamera>& camera,
-                          const Options& options) {
+                          const cmvr::config::TouchScreenAppConfig& config) {
+    Options options;
+    if (!optionsFromConfig(config, options)) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
     robot_ = robot;
     dexhand_ = dexhand;
     camera_ = camera;
@@ -49,6 +289,24 @@ bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
         return false;
     }
 
+    if ((options_.move_to_init_position_before_start || options_.move_to_init_position) &&
+        options_.init_joint_positions.empty()) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    if (options_.move_to_init_position_before_start) {
+        try {
+            auto init_cmd = options_.init_joint_positions;
+            robot_->moveJ(init_cmd, options_.init_movej_vel, options_.init_movej_acc);
+        } catch (...) {
+            initialized_ = false;
+            last_status_ = Status::ROBOT_COMMAND_FAILED;
+            return false;
+        }
+    }
+
     if (options_.urdf_path.empty() || options_.camera_link.empty() || options_.control_joint_names.empty()) {
         initialized_ = false;
         last_status_ = Status::INVALID_CONFIG;
@@ -102,10 +360,53 @@ bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
 }
 
 void TouchScreenApp::setOptions(const Options& options) {
+    if (!initialized_) {
         options_ = options;
-    if (initialized_) {
+        return;
-        applyOptions();
     }
+
+    const Options previous = options_;
+    Options applied = options;
+    if (requiresReinitForOptionChange(options_, options)) {
+        std::cerr << "[TouchScreenApp] setOptions() ignored IK-chain option changes after init; "
+                     "call init() again to update device/init pose/urdf/link/joint chain\n";
+        applied.move_to_init_position_before_start = previous.move_to_init_position_before_start;
+        applied.move_to_init_position = previous.move_to_init_position;
+        applied.init_joint_positions = previous.init_joint_positions;
+        applied.init_movej_vel = previous.init_movej_vel;
+        applied.init_movej_acc = previous.init_movej_acc;
+        applied.urdf_path = previous.urdf_path;
+        applied.base_link = previous.base_link;
+        applied.flange_link = previous.flange_link;
+        applied.camera_link = previous.camera_link;
+        applied.control_joint_names = previous.control_joint_names;
+    }
+
+    options_ = applied;
+    if (!applyOptions()) {
+        options_ = previous;
+        applyOptions();
+        last_status_ = Status::INVALID_CONFIG;
+    }
+}
+
+bool TouchScreenApp::setOptionsFromConfig() {
+    cmvr::config::TouchScreenAppConfig config;
+    if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) {
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+    return setOptionsFromConfig(config);
+}
+
+bool TouchScreenApp::setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config) {
+    Options options;
+    if (!optionsFromConfig(config, options)) {
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+    setOptions(options);
+    return last_status_ != Status::INVALID_CONFIG;
 }
 
 bool TouchScreenApp::startFromPixel(int u, int v) {
@@ -148,6 +449,10 @@ bool TouchScreenApp::step() {
         return false;
     }
 
+    if (dexhand_) {
+        updateTouchPressure();
+    }
+
     switch (phase_) {
         case Phase::IDLE:
             last_status_ = Status::IDLE;
@@ -234,8 +539,9 @@ const char* TouchScreenApp::statusToString(const Status status) {
         case Status::ALIGNING: return "ALIGNING";
         case Status::ALIGN_REACHED: return "ALIGN_REACHED";
         case Status::TOUCHING: return "TOUCHING";
+        case Status::TACTILE_UNAVAILABLE: return "TACTILE_UNAVAILABLE";
         case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED";
-        case Status::TOUCH_TIMEOUT: return "TOUCH_TIMEOUT";
+        case Status::TOUCH_FORWARD_TIMEOUT: return "TOUCH_FORWARD_TIMEOUT";
         case Status::RETRACTING: return "RETRACTING";
         case Status::DONE: return "DONE";
         case Status::STOPPED: return "STOPPED";
@@ -245,10 +551,181 @@ const char* TouchScreenApp::statusToString(const Status status) {
     return "UNKNOWN";
 }
 
+bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& config,
+                                       Options& options_out) {
+    Options options;
+
+    if (config.has_move_to_init_position_before_start()) {
+        options.move_to_init_position_before_start = config.move_to_init_position_before_start();
+    }
+    if (config.has_move_to_init_position()) {
+        options.move_to_init_position = config.move_to_init_position();
+    }
+    if (config.init_joint_positions_size() > 0) {
+        options.init_joint_positions.clear();
+        options.init_joint_positions.reserve(static_cast<size_t>(config.init_joint_positions_size()));
+        for (const auto& joint : config.init_joint_positions()) {
+            options.init_joint_positions.emplace_back(
+                joint.has_joint_name() ? joint.joint_name() : std::string{},
+                joint.has_rad() ? joint.rad() : 0.0);
+        }
+    }
+    if (config.has_init_movej_vel()) {
+        options.init_movej_vel = config.init_movej_vel();
+    }
+    if (config.has_init_movej_acc()) {
+        options.init_movej_acc = config.init_movej_acc();
+    }
+
+    if (config.has_urdf_path()) {
+        options.urdf_path = config.urdf_path();
+    }
+    if (config.has_base_link()) {
+        options.base_link = config.base_link();
+    }
+    if (config.has_flange_link()) {
+        options.flange_link = config.flange_link();
+    }
+    if (config.has_camera_link()) {
+        options.camera_link = config.camera_link();
+    }
+
+    if (config.has_tag_size_m()) {
+        options.tag_size_m = config.tag_size_m();
+    }
+    if (config.has_depth_policy()) {
+        options.depth_policy = toDepthPolicy(config.depth_policy());
+    }
+    if (config.has_target_point_method()) {
+        options.target_point_method = toTargetPointMethod(config.target_point_method());
+    }
+
+    if (config.has_hover_target_in_camera()) {
+        applyVec3FromConfig(config.hover_target_in_camera(), options.hover_target_in_camera);
+    }
+    if (config.has_target_rx()) {
+        options.target_rx = config.target_rx();
+    }
+    if (config.has_target_ry()) {
+        options.target_ry = config.target_ry();
+    }
+    if (config.has_target_rz()) {
+        options.target_rz = config.target_rz();
+    }
+
+    if (config.has_ibvs_lambda()) {
+        options.ibvs_lambda = config.ibvs_lambda();
+    }
+    if (config.has_ibvs_mu()) {
+        options.ibvs_mu = config.ibvs_mu();
+    }
+    if (config.has_ibvs_qdot_max()) {
+        options.ibvs_qdot_max = config.ibvs_qdot_max();
+    }
+    if (config.has_ibvs_vmax6()) {
+        Eigen::Matrix<double, 6, 1> ibvs_vmax;
+        ibvs_vmax << options.ibvs_vmax6[0], options.ibvs_vmax6[1], options.ibvs_vmax6[2],
+            options.ibvs_vmax6[3], options.ibvs_vmax6[4], options.ibvs_vmax6[5];
+        applyTwist6FromConfig(config.ibvs_vmax6(), ibvs_vmax);
+        for (int i = 0; i < 6; ++i) {
+            options.ibvs_vmax6[static_cast<size_t>(i)] = ibvs_vmax[i];
+        }
+    }
+    if (config.has_enable_joint_limit_avoidance()) {
+        options.enable_joint_limit_avoidance = config.enable_joint_limit_avoidance();
+    }
+    if (config.has_joint_limit_avoidance_gain()) {
+        options.joint_limit_avoidance_gain = config.joint_limit_avoidance_gain();
+    }
+    if (config.has_joint_limit_avoidance_margin_ratio()) {
+        options.joint_limit_avoidance_margin_ratio = config.joint_limit_avoidance_margin_ratio();
+    }
+    if (config.has_joint_limit_avoidance_max_push()) {
+        options.joint_limit_avoidance_max_push = config.joint_limit_avoidance_max_push();
+    }
+
+    if (config.has_r_camera_to_visp()) {
+        applyMatrix3dFromConfig(config.r_camera_to_visp(), options.R_camera_to_visp);
+    }
+    if (config.has_r_camera_to_urdf()) {
+        applyMatrix3dFromConfig(config.r_camera_to_urdf(), options.R_camera_to_urdf);
+    }
+
+    if (config.control_joint_names_size() > 0) {
+        options.control_joint_names.assign(config.control_joint_names().begin(),
+                                           config.control_joint_names().end());
+    }
+
+    if (config.has_align_xy_threshold_m()) {
+        options.align_xy_threshold_m = config.align_xy_threshold_m();
+    }
+    if (config.has_align_z_threshold_m()) {
+        options.align_z_threshold_m = config.align_z_threshold_m();
+    }
+    if (config.has_align_rot_threshold_rad()) {
+        options.align_rot_threshold_rad = config.align_rot_threshold_rad();
+    }
+    if (config.has_align_stable_frames()) {
+        options.align_stable_frames = config.align_stable_frames();
+    }
+    if (config.has_align_timeout_s()) {
+        options.align_timeout_s = config.align_timeout_s();
+    }
+    if (config.has_pause_after_align_reached()) {
+        options.pause_after_align_reached = config.pause_after_align_reached();
+    }
+
+    if (config.has_touch_twist_base()) {
+        applyTwist6FromConfig(config.touch_twist_base(), options.touch_twist_base);
+    }
+    if (config.has_touch_acceleration()) {
+        options.touch_acceleration = config.touch_acceleration();
+    }
+    if (config.has_touch_forward_duration_s()) {
+        options.touch_forward_duration_s = config.touch_forward_duration_s();
+    }
+
+    if (config.has_dwell_time_s()) {
+        options.dwell_time_s = config.dwell_time_s();
+    }
+    if (config.has_retract_twist_base()) {
+        applyTwist6FromConfig(config.retract_twist_base(), options.retract_twist_base);
+    }
+    if (config.has_retract_acceleration()) {
+        options.retract_acceleration = config.retract_acceleration();
+    }
+    if (config.has_retract_duration_s()) {
+        options.retract_duration_s = config.retract_duration_s();
+    }
+
+    if (config.has_tactile_finger()) {
+        options.tactile_finger = toFingerType(config.tactile_finger());
+    }
+    if (config.has_tactile_region()) {
+        options.tactile_region = toTactileRegion(config.tactile_region());
+    }
+    if (config.has_tactile_pressure_sum_threshold()) {
+        options.tactile_pressure_sum_threshold = config.tactile_pressure_sum_threshold();
+    }
+    if (config.has_tactile_pressure_peak_threshold()) {
+        options.tactile_pressure_peak_threshold = config.tactile_pressure_peak_threshold();
+    }
+
+    options_out = options;
+    return true;
+}
+
 bool TouchScreenApp::applyOptions() {
     if (!perception_) {
         return false;
     }
+    if (!options_.R_camera_to_visp.allFinite() || !options_.R_camera_to_urdf.allFinite()) {
+        return false;
+    }
+    if (!std::isfinite(options_.touch_forward_duration_s) ||
+        options_.touch_forward_duration_s < 0.0) {
+        return false;
+    }
 
     perception_->setTagSize(options_.tag_size_m);
     tracker_.setTargetPointMethod(options_.target_point_method);
@@ -260,15 +737,8 @@ bool TouchScreenApp::applyOptions() {
                                  options_.joint_limit_avoidance_gain,
                                  options_.joint_limit_avoidance_margin_ratio,
                                  options_.joint_limit_avoidance_max_push);
-    // ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp);
+    ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp);
-    // ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
+    ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
-
-    Eigen::Matrix3d RxPi;
-    RxPi << 1,0,0,
-            0,-1,0,
-            0,0,-1;
-    ibvs_.setAlignCameraToUrdf(RxPi);
-    ibvs_.setAlignCameraToVisp(RxPi);
     return true;
 }
 
@@ -395,9 +865,22 @@ bool TouchScreenApp::stepAligning() {
     }
 
     last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera;
+    const auto* current_tag = perception_->findTag(tag_id);
+    if (!current_tag) {
+        sendZeroJointVelocity();
+        align_stable_count_ = 0;
+        last_status_ = Status::ALIGN_WAITING_TRACK;
+        return true;
+    }
+
+    const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
+    const Eigen::Matrix3d R_target =
+        rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz);
+    const double rot_error_rad = rotationErrorRad(R_current, R_target);
     if (std::abs(last_align_error_camera_.x()) <= options_.align_xy_threshold_m &&
         std::abs(last_align_error_camera_.y()) <= options_.align_xy_threshold_m &&
-        std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m) {
+        std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m &&
+        rot_error_rad <= options_.align_rot_threshold_rad) {
         ++align_stable_count_;
     } else {
         align_stable_count_ = 0;
@@ -424,10 +907,38 @@ bool TouchScreenApp::stepTouching() {
         }
     }
 
-    updateTouchPressure();
+    if (!dexhand_) {
-    if (last_touch_pressure_sum_ >= options_.tactile_pressure_sum_threshold ||
+        enterFailed(Status::TACTILE_UNAVAILABLE);
+        return false;
+    }
+    const auto touchTriggered = [&]() {
+        return last_touch_pressure_sum_ >= options_.tactile_pressure_sum_threshold ||
                (options_.tactile_pressure_peak_threshold > 0.0 &&
-         last_touch_pressure_peak_ >= options_.tactile_pressure_peak_threshold)) {
+                last_touch_pressure_peak_ >= options_.tactile_pressure_peak_threshold);
+    };
+    const auto handleTouchTriggered = [&]() {
+        if (options_.dwell_time_s < 0.0) {
+            try {
+                if (!robot_->speedL(toStdVector6(options_.retract_twist_base),
+                                    options_.retract_acceleration,
+                                    0.0,
+                                    cmvr::CartesianFrame::Tool)) {
+                    enterFailed(Status::ROBOT_COMMAND_FAILED);
+                    return false;
+                }
+            } catch (...) {
+                enterFailed(Status::ROBOT_COMMAND_FAILED);
+                return false;
+            }
+            phase_ = Phase::RETRACTING;
+            phase_after_retract_ = Phase::DONE;
+            final_status_after_retract_ = Status::DONE;
+            phase_start_time_ = Clock::now();
+            retract_command_started_ = true;
+            last_status_ = Status::TOUCH_TRIGGERED;
+            return true;
+        }
+
         try {
             robot_->stopSpeedL();
         } catch (...) {
@@ -438,17 +949,23 @@ bool TouchScreenApp::stepTouching() {
         phase_start_time_ = Clock::now();
         last_status_ = Status::TOUCH_TRIGGERED;
         return true;
+    };
+
+    if (touchTriggered()) {
+        return handleTouchTriggered();
     }
 
     const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
-    if (elapsed > options_.touch_timeout_s) {
+    if (options_.touch_forward_duration_s > 0.0 &&
-        try {
+        elapsed >= options_.touch_forward_duration_s) {
-            robot_->stopSpeedL();
+        if (!updateTouchPressure()) {
-        } catch (...) {
+            enterFailed(Status::TACTILE_UNAVAILABLE);
-            enterFailed(Status::ROBOT_COMMAND_FAILED);
             return false;
         }
-        if (!startRetractPhase(Phase::FAILED, Status::TOUCH_TIMEOUT)) {
+        if (touchTriggered()) {
+            return handleTouchTriggered();
+        }
+        if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) {
             enterFailed(Status::ROBOT_COMMAND_FAILED);
             return false;
         }
@@ -494,6 +1011,12 @@ bool TouchScreenApp::stepRetracting() {
         return false;
     }
     holdCurrentControlledPosition();
+    if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) &&
+        !moveToInitPositionIfEnabled()) {
+        phase_ = Phase::FAILED;
+        last_status_ = Status::ROBOT_COMMAND_FAILED;
+        return false;
+    }
     phase_ = phase_after_retract_;
     last_status_ = final_status_after_retract_;
     return phase_ != Phase::FAILED;
@@ -578,6 +1101,23 @@ bool TouchScreenApp::holdCurrentControlledPosition() const {
     return true;
 }
 
+bool TouchScreenApp::moveToInitPositionIfEnabled() const {
+    if (!options_.move_to_init_position) {
+        return true;
+    }
+    if (!robot_ || options_.init_joint_positions.empty()) {
+        return false;
+    }
+
+    try {
+        auto init_cmd = options_.init_joint_positions;
+        robot_->moveJ(init_cmd, options_.init_movej_vel, options_.init_movej_acc);
+    } catch (...) {
+        return false;
+    }
+    return true;
+}
+
 bool TouchScreenApp::startTouchPhase() {
     if (!robot_) {
         return false;
@@ -585,7 +1125,8 @@ bool TouchScreenApp::startTouchPhase() {
     try {
         if (!robot_->speedL(toStdVector6(options_.touch_twist_base),
                             options_.touch_acceleration,
-                            0.0)) {
+                            0.0,
+                            cmvr::CartesianFrame::Tool)) {
             return false;
         }
     } catch (...) {
@@ -607,7 +1148,8 @@ bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract,
     try {
         if (!robot_->speedL(toStdVector6(options_.retract_twist_base),
                             options_.retract_acceleration,
-                            0.0)) {
+                            0.0,
+                            cmvr::CartesianFrame::Tool)) {
             return false;
         }
     } catch (...) {
@@ -634,7 +1176,7 @@ void TouchScreenApp::enterFailed(const Status status) {
     phase_ = Phase::FAILED;
     touch_command_started_ = false;
     retract_command_started_ = false;
-    last_status_ = status;
+    last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED;
 }
 
 bool TouchScreenApp::updateTouchPressure() {
@@ -644,7 +1186,8 @@ bool TouchScreenApp::updateTouchPressure() {
         return false;
     }
 
-    const auto& sensors = dexhand_->getSensorData();
+    const auto& sensors = dexhand_->getSensorData(options_.tactile_finger,
+                                                  toDeviceTactileRegion(options_.tactile_region));
     double sum = 0.0;
     double peak = 0.0;
 

cmvr-es/applications/src/touch_screen_app_test.cpp

@@ -5,21 +5,13 @@
 #include <thread>
 
 #include "applications/include/touch_screen_app.h"
-#include "device_manager/include/device_manager.h"
+
+#include "include/device_manager.h"
 #include "service/grpc/include/server_runner.h"
 namespace {
 
 constexpr const char* kConfigPath =
     "/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml";
-// 这些 id 需要与现场配置一致；保持为示例调用中的写法。
-constexpr const char* kRobotId = "hc01";
-constexpr const char* kDexhandId = "hand2";
-constexpr const char* kCameraId = "right_hand_cam";
-constexpr const char* kUrdfPath =
-    "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf";
-constexpr const char* kBaseLink = "PELVIS_S";
-constexpr const char* kFlangeLink = "R_WRIST_R_S";
-constexpr const char* kCameraLink = "R_CAM";
 constexpr int kTargetU = 1280 / 2.0;
 constexpr int kTargetV = 720 / 2.0;
 
@@ -27,117 +19,75 @@ constexpr int kTargetV = 720 / 2.0;
 void run_touch_once(int u, int v) {
 
     const XmlNode config(kConfigPath);
-    cmvr::service::ServerRunner runner;
+    // auto& dm = cmvr::device::DeviceManager::getInstance();
-    runner.start(config);
+    cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
-    if (!config.hasChild("DeviceManager")) {
-        std::cerr << "DeviceManager node not found\n";
-        return;
-    }
-
-    auto& dm = cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
-
-    auto robot = dm.getDevice<cmvr::device::AbstractRobot>(kRobotId);
-    // auto dexhand = dm.getDevice<cmvr::device::AbstractDexHand>(kDexhandId);
-    auto camera = dm.getDevice<cmvr::device::AbstractCamera>(kCameraId);
-    std::shared_ptr<cmvr::device::AbstractDexHand> dexhand = nullptr;
-
-    camera->start();
-
-    std::vector<cmvr::device::JointPoint> init_cmd{};
-    init_cmd = {
-        {"R_SHOULDER_P", -0.2423},
-        {"R_SHOULDER_R", 1.2929},
-        {"R_SHOULDER_Y", 1.61},
-        {"R_ELBOW_R",     1.58},
-        {"R_WRIST_P",    -2.8792},
-        {"R_WRIST_Y",    0.1150},
-        {"R_WRIST_R",    -0.08},
-    };
-    robot->moveJ(init_cmd,1.0,2.0);
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
 
 
+    // cmvr::service::ServerRunner runner;
+    // runner.start(config);
 
+    ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
 
     cmvr::app::TouchScreenApp app;
-    cmvr::app::TouchScreenApp::Options opt;
+    ASSERT_TRUE(app.init())
+        << "TouchScreenApp init failed";
 
-    opt.urdf_path = kUrdfPath;
+    ASSERT_TRUE(app.startFromPixel(u, v)) << "startFromPixel failed";
-    opt.base_link = kBaseLink;
-    opt.flange_link = kFlangeLink;
-    opt.camera_link = kCameraLink;
-    opt.tag_size_m = 0.02;
-    opt.ibvs_vmax6 = {0.04, 0.04, 0.04, 0.04, 0.04, 0.04};
-
-    opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.30);
-    opt.pause_after_align_reached = true;
-
-    // 只测试视觉对齐阶段：禁止进入真实下压。
-    opt.touch_twist_base <<
-        0.0, 0.0, 0.0,
-        0.0, 0.0, 0.0;
-    opt.touch_acceleration = 0.6;
-
-    opt.retract_twist_base <<
-        0.0, 0.0, 0.03,
-        0.0, 0.0, 0.0;
-    opt.retract_duration_s = 0.20;
-
-    opt.tactile_finger = cmvr::device::FingerType::INDEX;
-    opt.tactile_region = cmvr::app::TouchScreenApp::TactileRegion::FINGER;
-    opt.tactile_pressure_sum_threshold = 1e12;
-    opt.align_timeout_s = 100.0;
-
-    if (!app.init(robot, dexhand, camera, opt)) {
-        std::cerr << "TouchScreenApp init failed\n";
-        return;
-    }
-
-    if (!app.startFromPixel(u, v)) {
-        std::cerr << "startFromPixel failed\n";
-        return;
-    }
 
+    bool align_reached = false;
+    bool touch_triggered = false;
     while (app.isBusy()) {
-        if (!app.step()) {
+        const bool step_ok = app.step();
-            std::cerr << "touch failed, status="
+        const auto& p_c_target = app.tracker().lastTargetInCamera();
-                      << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
-                      << "\n";
-            break;
-        }
         std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase())
                   << ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
                   << ", active_tag=" << app.lastActiveTagId()
+                  << ", target_c=[" << p_c_target.x() << ", "
+                  << p_c_target.y() << ", "
+                  << p_c_target.z() << "]"
+                  << ", pressure_sum=" << app.lastTouchPressureSum()
+                  << ", pressure_peak=" << app.lastTouchPressurePeak()
                   << ", err_c=[" << app.lastAlignErrorCamera().x() << ", "
                   << app.lastAlignErrorCamera().y() << ", "
                   << app.lastAlignErrorCamera().z() << "]\n";
 
         if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) {
-            std::cout << "align reached\n";
+            std::cout << "align reached, target_c=[" << p_c_target.x() << ", "
+                      << p_c_target.y() << ", "
+                      << p_c_target.z() << "]\n";
+            align_reached = true;
+        }
+        if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) {
+            std::cout << "touch triggered, pressure_sum=" << app.lastTouchPressureSum()
+                      << ", pressure_peak=" << app.lastTouchPressurePeak() << "\n";
+            touch_triggered = true;
+        }
+        if (!step_ok) {
+            const auto failed_status = app.lastStatus();
             app.stop();
-            break;
+            FAIL() << "touch flow failed, status="
+                   << cmvr::app::TouchScreenApp::statusToString(failed_status);
         }
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-    }
-
-    if (app.isFinished()) {
-        std::cout << "touch done\n";
-    } else if (app.isFailed()) {
-        std::cout << "touch failed: "
-                  << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
-                  << "\n";
-    }
-
-    while (true) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
     }
 
+    const auto final_status = app.lastStatus();
+    ASSERT_TRUE(align_reached)
+        << "align was never reached, final status="
+        << cmvr::app::TouchScreenApp::statusToString(final_status);
+    ASSERT_TRUE(touch_triggered)
+        << "touch was never triggered, final status="
+        << cmvr::app::TouchScreenApp::statusToString(final_status);
+    ASSERT_TRUE(app.isFinished())
+        << "touch did not finish successfully, final status="
+        << cmvr::app::TouchScreenApp::statusToString(final_status);
+    std::cout << "touch done, final status="
+              << cmvr::app::TouchScreenApp::statusToString(final_status) << "\n";
+    app.stop();
 }
 
 } // namespace
 
 TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) {
-
     run_touch_once(kTargetU, kTargetV);
 }

cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt

@@ -7,7 +7,7 @@ rh56dftp_dexhands {
 
 rh56dftp_dexhands {
     id: "hand2"
-    ip: "192.168.1.214"
+    ip: "192.168.1.224"
     port: 6000
-    enable: false
+    enable: true
 }

cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt

@@ -0,0 +1,123 @@
+robot_id: "hc01"
+dexhand_id: "hand2"
+camera_id: "right_hand_cam"
+move_to_init_position_before_start: true
+move_to_init_position: true
+init_joint_positions {
+  joint_name: "R_SHOULDER_P"
+  rad: -0.2423
+}
+init_joint_positions {
+  joint_name: "R_SHOULDER_R"
+  rad: 1.2929
+}
+init_joint_positions {
+  joint_name: "R_SHOULDER_Y"
+  rad: 1.61
+}
+init_joint_positions {
+  joint_name: "R_ELBOW_R"
+  rad: 1.58
+}
+init_joint_positions {
+  joint_name: "R_WRIST_P"
+  rad: -2.8792
+}
+init_joint_positions {
+  joint_name: "R_WRIST_Y"
+  rad: 0.1150
+}
+init_joint_positions {
+  joint_name: "R_WRIST_R"
+  rad: -0.08
+}
+init_movej_vel: 1.0
+init_movej_acc: 2.0
+
+urdf_path: "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
+base_link: "PELVIS_S"
+flange_link: "R_WRIST_R_S"
+camera_link: "R_CAM"
+
+tag_size_m: 0.02
+depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
+target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
+
+hover_target_in_camera {
+  x: 0.0
+  y: 0.0
+  z: 0.20
+}
+target_rx: 3.14159265358979323846
+target_ry: 0.0
+target_rz: 0.0
+
+ibvs_lambda: 0.6
+ibvs_mu: 0.1
+ibvs_qdot_max: 0.15
+ibvs_vmax6 {
+  vx: 0.15
+  vy: 0.15
+  vz: 0.20
+  wx: 0.6
+  wy: 0.6
+  wz: 0.6
+}
+enable_joint_limit_avoidance: true
+joint_limit_avoidance_gain: 0.2
+joint_limit_avoidance_margin_ratio: 0.15
+joint_limit_avoidance_max_push: 0.25
+
+r_camera_to_visp {
+  m00: 1.0
+  m11: 1.0
+  m22: 1.0
+}
+r_camera_to_urdf {
+  m00: 1.0
+  m11: 1.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"
+
+align_xy_threshold_m: 0.003
+align_z_threshold_m: 0.010
+align_rot_threshold_rad: 0.08726646259971647
+align_stable_frames: 5
+align_timeout_s: 100.0
+pause_after_align_reached: false
+
+touch_twist_base {
+  vx: 0.0
+  vy: -0.08
+  vz: 0.0
+  wx: 0.0
+  wy: 0.0
+  wz: 0.0
+}
+touch_acceleration: 3.0
+touch_forward_duration_s: 1.2
+
+dwell_time_s: -1.0
+retract_twist_base {
+  vx: 0.0
+  vy: 0.08
+  vz: 0.0
+  wx: 0.0
+  wy: 0.0
+  wz: 0.0
+}
+retract_acceleration: 3.0
+retract_duration_s: 1.0
+
+tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
+tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP
+tactile_pressure_sum_threshold: 300.0
+tactile_pressure_peak_threshold: 0.0

cmvr-es/common/utils/config_helper/include/config_helper.h

@@ -9,6 +9,7 @@
 #include "cmvr/config/dexhand_config/dexhand_config.pb.h"
 #include "cmvr/config/microphone_config/microphone_config.pb.h"
 #include "cmvr/config/speaker_config/speaker_conifg.pb.h"
+#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
 
 #define GET_CONFIG(file, para)                                             \
     ([&]() -> bool {                                                       \
@@ -71,6 +72,16 @@ namespace cmvr
             return GET_CONFIG(speaker_config_file, config);
         }
 
+        static bool getTouchScreenAppConfig(config::TouchScreenAppConfig& config)
+        {
+            return GET_CONFIG(touch_screen_app_config_file, config);
+        }
+
+        static bool setTouchScreenAppConfig(const config::TouchScreenAppConfig& config)
+        {
+            return SET_CONFIG(config, touch_screen_app_config_file);
+        }
+
     private:
         // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx)
         template <class T>

cmvr-es/common/utils/config_helper/include/config_setting.h

@@ -5,3 +5,4 @@ DECLARE_string(camera_config_file);
 DECLARE_string(dexhand_config_file);
 DECLARE_string(microphone_config_file);
 DECLARE_string(speaker_config_file);
+DECLARE_string(touch_screen_app_config_file);

cmvr-es/common/utils/config_helper/src/config_setting.cpp

@@ -35,3 +35,6 @@ DEFINE_string(speaker_config_file,
              basePath() + "speaker_config/speaker_config.pb.txt",
               "The configuration file for speaker");
 
+DEFINE_string(touch_screen_app_config_file,
+             basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt",
+              "The configuration file for TouchScreenApp");

cmvr-es/devices/dexhand/abstract_dexhand.h

@@ -75,6 +75,15 @@ namespace cmvr::device{
         THUMB     // 大拇指
     };
 
+    // 触觉区域枚举（设备层）。
+    enum class TactileRegion {
+        TIP = 0,
+        FINGER,
+        PAD,
+        TIP_AND_FINGER,
+        THUMB_MIDDLE
+    };
+
     // 整只手的触觉传感器数据
     struct HandTactileSensors {
         // 五指的触觉数据（每根手指包含指端、指尖、指腹）
@@ -219,6 +228,11 @@ namespace cmvr::device{
         virtual void execPresetAct(int action_id) {}
         virtual void setForce(const std::vector<int>& finger_joint_force) {}
         virtual HandTactileSensors& getSensorData() { return hand_tactile_sensors_;}
+        virtual HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) {
+            (void)finger_type;
+            (void)tactile_region;
+            return getSensorData();
+        }
     protected:
         DexHandState status_;
         HandTactileSensors hand_tactile_sensors_;

cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h

@@ -61,6 +61,7 @@ namespace cmvr::device {
         void execPresetAct(int action_id) override;//这个无用
         void setForce(const std::vector<int>& finger_joint_force) override;
         HandTactileSensors& getSensorData() override;
+        HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) override;
     private:
         void updateState();
         void updateSensorData();

cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp

@@ -35,6 +35,21 @@ const int TOUCH_SENSOR_END_ADDR_PALM = 5123;
 // Modbus 每次最多读取寄存器的数量
 const int MAX_REGISTERS_PER_READ = 125;
 
+namespace {
+
+std::pair<int, int> fingerSensorRegisterRange(const FingerType finger_type) {
+    switch (finger_type) {
+        case PINKY: return {TOUCH_SENSOR_BASE_ADDR_PINKY, TOUCH_SENSOR_END_ADDR_PINKY};
+        case RING: return {TOUCH_SENSOR_BASE_ADDR_RING, TOUCH_SENSOR_END_ADDR_RING};
+        case MIDDLE: return {TOUCH_SENSOR_BASE_ADDR_MIDDLE, TOUCH_SENSOR_END_ADDR_MIDDLE};
+        case INDEX: return {TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX};
+        case THUMB: return {TOUCH_SENSOR_BASE_ADDR_THUMB, TOUCH_SENSOR_END_ADDR_THUMB};
+    }
+    return {TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX};
+}
+
+} // namespace
+
 ModbusController::ModbusController() {
     // 初始化寄存器字典
     regdict = {
@@ -443,3 +458,13 @@ HandTactileSensors& RH56DFTPDexhand::getSensorData()
     return  hand_tactile_sensors_;
 
 }
+
+HandTactileSensors& RH56DFTPDexhand::getSensorData(const FingerType finger_type,
+                                                   const TactileRegion tactile_region)
+{
+    (void)tactile_region;
+    const auto [start_addr, end_addr] = fingerSensorRegisterRange(finger_type);
+    const auto finger_register_values = controller_->readRegisterRange(start_addr, end_addr);
+    hand_tactile_sensors_.parseFinger(finger_type, finger_register_values);
+    return hand_tactile_sensors_;
+}

cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp

@@ -26,12 +26,12 @@ void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot
     statusword_t st{};
     st.value = motor_status->status_word();
 
-    if (st.op_mode_specific > 0) {
+    // if (st.op_mode_specific > 0) {
-        LOG(INFO) << st.op_mode_specific ;
+    //     LOG(INFO) << st.op_mode_specific ;
-    }
+    // }
 
 
-    LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ;
+    // LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ;
 
 
 

cmvr-es/devices/robot/abstract_robot.h

@@ -9,6 +9,7 @@
 #include "json/json.h"
 #include "../abstract_device.h"
 #include "../../utils/controller/include/cartesian_controller.h"
+#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h"
 #include "cmvr/msgs/geometry.pb.h"
 #include "cmvr/msgs/motor.pb.h"
 
@@ -108,7 +109,12 @@ namespace cmvr::device{
 
         virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
         virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
-        virtual bool speedL(const std::vector<double> &xd, double acceleration = 0.25, double time = 0.0) { throw std::runtime_error("Not implemented"); }
+        virtual bool speedL(const std::vector<double> &xd,
+                            double acceleration = 0.25,
+                            double time = 0.0,
+                            cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) {
+            throw std::runtime_error("Not implemented");
+        }
         virtual void stopSpeedL() { throw std::runtime_error("Not implemented"); }
 
         virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }

cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h

@@ -94,7 +94,10 @@ namespace cmvr::device{
 
         void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
 
-        bool speedL(const std::vector<double> &xd, double acceleration = 0.25, double time = 0.0) override;
+        bool speedL(const std::vector<double> &xd,
+                    double acceleration = 0.25,
+                    double time = 0.0,
+                    cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override;
         void stopSpeedL() override;
         Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase();
 
@@ -215,6 +218,7 @@ namespace cmvr::device{
         Eigen::Matrix<double, 6, 1> speedl_target_twist_{Eigen::Matrix<double, 6, 1>::Zero()};
         Eigen::Matrix<double, 6, 1> speedl_last_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
         double speedl_target_acceleration_{0.25};
+        cmvr::CartesianFrame speedl_target_frame_{cmvr::CartesianFrame::Base};
         double speedl_applied_acceleration_{0.25};
         std::uint64_t speedl_command_version_{0};
 

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp

@@ -87,7 +87,10 @@ void HumanoidRobot<DOF>::init() {
 
 
 template <int DOF>
-bool HumanoidRobot<DOF>::speedL(const std::vector<double>& xd, double acceleration, double time)
+bool HumanoidRobot<DOF>::speedL(const std::vector<double>& xd,
+                                double acceleration,
+                                double time,
+                                cmvr::CartesianFrame frame)
 {
     if (!ik_solver_) {
         throw runtime_error("speedL failed: ik_solver_ is not initialized");
@@ -115,6 +118,7 @@ bool HumanoidRobot<DOF>::speedL(const std::vector<double>& xd, double accelerati
         std::lock_guard<std::mutex> lock(speedl_mutex_);
         speedl_target_twist_ = target_twist;
         speedl_target_acceleration_ = acceleration;
+        speedl_target_frame_ = frame;
         speedl_command_active_ = true;
         command_version = ++speedl_command_version_;
     }
@@ -127,6 +131,7 @@ bool HumanoidRobot<DOF>::speedL(const std::vector<double>& xd, double accelerati
             std::lock_guard<std::mutex> lock(speedl_mutex_);
             if (speedl_command_version_ == command_version) {
                 speedl_target_twist_.setZero();
+                speedl_target_frame_ = cmvr::CartesianFrame::Base;
                 speedl_command_active_ = true;
                 ++speedl_command_version_;
                 should_stop = true;
@@ -149,6 +154,7 @@ void HumanoidRobot<DOF>::stopSpeedL()
     {
         std::lock_guard<std::mutex> lock(speedl_mutex_);
         speedl_target_twist_.setZero();
+        speedl_target_frame_ = cmvr::CartesianFrame::Base;
         speedl_command_active_ = true;
         ++speedl_command_version_;
     }
@@ -215,6 +221,7 @@ void HumanoidRobot<DOF>::stopSpeedLWorker_() {
         speedl_stop_requested_.store(true);
         speedl_command_active_ = false;
         speedl_target_twist_.setZero();
+        speedl_target_frame_ = cmvr::CartesianFrame::Base;
         speedl_last_command_twist_base_.setZero();
     }
     speedl_cv_.notify_all();
@@ -272,6 +279,7 @@ void HumanoidRobot<DOF>::speedLWorkerLoop_() {
     while (true) {
         Eigen::Matrix<double, 6, 1> target_twist = Eigen::Matrix<double, 6, 1>::Zero();
         double acceleration = 0.25;
+        cmvr::CartesianFrame target_frame = cmvr::CartesianFrame::Base;
         {
             std::unique_lock<std::mutex> lock(speedl_mutex_);
             speedl_cv_.wait(lock, [&]() {
@@ -283,6 +291,7 @@ void HumanoidRobot<DOF>::speedLWorkerLoop_() {
             }
             target_twist = speedl_target_twist_;
             acceleration = speedl_target_acceleration_;
+            target_frame = speedl_target_frame_;
         }
 
         next_tick = std::chrono::steady_clock::now();
@@ -300,6 +309,7 @@ void HumanoidRobot<DOF>::speedLWorkerLoop_() {
                 }
                 target_twist = speedl_target_twist_;
                 acceleration = speedl_target_acceleration_;
+                target_frame = speedl_target_frame_;
             }
 
             if (!updateSpeedLAccelerationConfig_(acceleration)) {
@@ -323,7 +333,7 @@ void HumanoidRobot<DOF>::speedLWorkerLoop_() {
                                         dt,
                                         q_now,
                                         qd_cmd,
-                                        cmvr::CartesianFrame::Base,
+                                        target_frame,
                                         true)) {
                 LOG(ERROR) << "speedL worker: speedLStep() failed";
                 send_zero();

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp

@@ -22,8 +22,11 @@
 #include "../../../../device_manager/include/device_manager.h"
 #include <libgen.h>
 
+#include "common/utils/image/image_process.h"
 #include "controller/include/ibvs_controller.h"
 #include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "perception/include/apriltag_perception.h"
+#include "perception/include/tag_relative_target_3d.h"
 #include "cmvr/msgs/can_card_parameter.grpc.pb.h"
 #include "../include/humanoid_robot.h"
 #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
@@ -78,188 +81,16 @@ std::unordered_map<std::string, double> makeRightArmQMap() {
     return q_map;
 }
 
-constexpr std::array<double, 7> kRightArmQMin = {
-    -3.14, // R_SHOULDER_P
-    -0.78, // R_SHOULDER_R
-    -3.14, // R_SHOULDER_Y
-     0.00, // R_ELBOW_R
-    -3.14, // R_WRIST_P
-    -0.78, // R_WRIST_Y
-    -0.55  // R_WRIST_R
-};
-
-constexpr std::array<double, 7> kRightArmQMax = {
-     3.14, // R_SHOULDER_P
-     1.57, // R_SHOULDER_R
-     3.14, // R_SHOULDER_Y
-     2.05, // R_ELBOW_R
-     3.14, // R_WRIST_P
-     0.78, // R_WRIST_Y
-     1.57  // R_WRIST_R
-};
-
-double applySoftJointLimitVelocity(double q,
-                                   double v,
-                                   double q_min,
-                                   double q_max,
-                                   double soft_margin,
-                                   double hard_margin) {
-    if (q_max <= q_min) return 0.0;
-    const double hard = std::max(1e-4, hard_margin);
-    const double soft = std::max(hard + 1e-4, soft_margin);
-
-    if (v < 0.0) {
-        const double q_hard = q_min + hard;
-        const double q_soft = q_min + soft;
-        if (q <= q_hard) return 0.0;
-        if (q < q_soft) {
-            const double s = std::clamp((q - q_hard) / (q_soft - q_hard), 0.0, 1.0);
-            return v * s;
-        }
-    } else if (v > 0.0) {
-        const double q_hard = q_max - hard;
-        const double q_soft = q_max - soft;
-        if (q >= q_hard) return 0.0;
-        if (q > q_soft) {
-            const double s = std::clamp((q_hard - q) / (q_hard - q_soft), 0.0, 1.0);
-            return v * s;
-        }
-    }
-    return v;
-}
-
 } // namespace
 
-TEST(HumanoidRobotTest,GetState) {
-    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
-    const XmlNode config(config_path);
-
-    if (!config.hasChild("DeviceManager")){
-        LOG(ERROR) << "Device Manager node not found";
-    }
-    auto dmgr_cfg = config.getChild("DeviceManager");
-    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
-
-    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
-    robot->torqueOff();
-    std::vector<cmvr::device::JointState> joints;
-
-    while (true) {
-        joints.clear();
-        robot->getJointsState(joints);
-        for (const auto &js : joints) {
-            LOG(INFO) << "Joint: " << js.name
-                      << ", Position: " << js.position
-                      << ", Velocity: " << js.velocity
-                      << std::endl;
-        }
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-    }
-}
-
-
-TEST(HumanoidRobotTest,MyRobotTest) {
-
-    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
-    const XmlNode config(config_path);
-
-    if (!config.hasChild("DeviceManager")){
-        LOG(ERROR) << "Device Manager node not found";
-    }
-    auto dmgr_cfg = config.getChild("DeviceManager");
-    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
-
-    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
-    robot->calibrateZeroQ("R_WRIST_R");
-
-    // std::vector<JointPoint> cmd{};
-
-}
-
-TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
-    const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
-    ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
-    auto dmgr_cfg = config.getChild("DeviceManager");
-    auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
-
-    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
-
-    auto q_map_now = makeRightArmQMap();
-    std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
-    auto refreshRightArmQ = [&]() -> bool {
-        robot->getJointQ(q_map_now);
-        for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
-            const auto it = q_map_now.find(kRightArmJointNames[i]);
-            if (it == q_map_now.end()) return false;
-            q_now[i] = it->second;
-        }
-        return true;
-    };
 
 
 
 
 
 
-    ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
 
 
-    std::vector<JointPoint> cmd{};
-    cmd = {
-        // {"R_SHOULDER_P", 0.0},
-        // {"R_SHOULDER_R", 0.0},
-        // {"R_SHOULDER_Y", 0.0},
-        // {"R_ELBOW_R",    0.0},
-        // {"R_WRIST_P",    0.0},
-        // {"R_WRIST_Y",    0.0},
-        {"R_WRIST_R",    0.2},
-    };
-    // auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
-    ASSERT_NO_THROW(robot->servoJ(cmd, 0.8, 0.01));
-    while (true) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(10000));
-    }
-
-}
-
-
-TEST(HumanoidRobotTest,speedJTest) {
-    const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
-    ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
-    auto dmgr_cfg = config.getChild("DeviceManager");
-    auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
-
-    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
-    std::vector<JointVelocityCommand> cmd{};
-    cmd = {
-        // {"R_SHOULDER_P", 0.0},
-        // {"R_SHOULDER_R", 0.0},
-        // {"R_SHOULDER_Y", 0.0},
-        // {"R_ELBOW_R",    0.0},
-        // {"R_WRIST_P",    0.0},
-        // {"R_WRIST_Y",    0.0},
-        {"R_WRIST_R",    0.2},
-    };
-    robot->speedJ(cmd);
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-    // robot->speedJ(0);
-    //
-    std::vector<JointPoint> cmd1{};
-    cmd1 = {
-        // {"R_SHOULDER_P", 0.0},
-        // {"R_SHOULDER_R", 0.0},
-        // {"R_SHOULDER_Y", 0.0},
-        // {"R_ELBOW_R",    0.0},
-        // {"R_WRIST_P",    0.0},
-        // {"R_WRIST_Y",    0.0},
-        {"R_WRIST_R",    0},
-    };
-    robot->moveJ(cmd1,2.0,10.0);
-    // robot->eStop();
-
-    //
-}
-
 TEST(HumanoidRobotTest, speedLSmokeTest) {
     const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
     ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
@@ -283,8 +114,8 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
     robot->moveJ(init_cmd,1.0,2.0);
     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
-    const std::vector<double> twist_pos = {0.08, 0, 0, 0.00, 0, 0};
+    const std::vector<double> twist_pos = {0.00, -0.08, 0, 0.00, 0, 0};
-    const std::vector<double> twist_neg = {-0.08, 0, 0, 0.00, 0, 0};
+    const std::vector<double> twist_neg = {0.00, 0.08, 0, 0.00, 0, 0};
     const double acceleration = 3.0;
     const double segment_time = 1.0;
     const double settle_time = 1.0;
@@ -302,7 +133,7 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
                             double duration,
                             const char* phase) {
         LOG(INFO) << "speedLSmokeTest phase: " << phase;
-        ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0));
+        ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0, cmvr::CartesianFrame::Tool));
         const int steps = static_cast<int>(std::ceil(duration / sample_dt));
         for (int i = 0; i < steps; ++i) {
             const Eigen::Matrix<double, 6, 1> cmd_twist = robot->getSpeedLCommandTwistBase();
@@ -379,177 +210,6 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
     // show(fig);
 }
 
-TEST(HumanoidRobotTest, SpeedLOpenLoopPlannerPlot) {
-    cmvr::PinocchioDlsIKSolver solver(
-        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
-        "PELVIS_S",
-        "R_WRIST_R_S",
-        "R_FINGER_TIP_FIXED");
-    ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed";
-
-    cmvr::PinocchioDlsIKSolver::SpeedLConfig speedl_config;
-    speedl_config.linear_velocity_max = 0.55;
-    speedl_config.linear_acceleration_max = 0.80;
-    speedl_config.linear_jerk_max = 3.30;
-    speedl_config.angular_velocity_max = 1.00;
-    speedl_config.angular_acceleration_max = 3.00;
-    speedl_config.angular_jerk_max = 12.0;
-    speedl_config.joint_acceleration_max = std::vector<double>(7, 8.0);
-    speedl_config.linear_target_replan_threshold = 1e-4;
-    speedl_config.angular_target_replan_threshold = 1e-4;
-    speedl_config.linear_reverse_cos_threshold = -0.8660254037844386;
-    speedl_config.linear_reverse_switch_speed_threshold = 1e-3;
-    ASSERT_TRUE(solver.configureSpeedL(speedl_config)) << "configureSpeedL failed";
-
-    std::vector<double> q_init = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0};
-    solver.update_joints_state(q_init);
-
-    const double dt = 0.002;
-    const int log_every = 50;
-    const double segment_time = 1.5;
-    const double stop_time = 1.0;
-    const double settle_time = 1.0;
-    const double linear_speed_cmd = 0.1;
-    const double z_speed_cmd = 0.10;
-    const double total_time = 2.0 * segment_time + stop_time;
-    const int active_steps = static_cast<int>(std::ceil(total_time / dt));
-    const int settle_steps = static_cast<int>(std::ceil(settle_time / dt));
-    const int total_steps = active_steps + settle_steps;
-
-    int ok_steps = 0;
-    std::vector<double> t_trace;
-    std::vector<double> target_vy_trace;
-    std::vector<double> target_vz_trace;
-    std::vector<double> command_vx_trace;
-    std::vector<double> command_vy_trace;
-    std::vector<double> command_vz_trace;
-    std::vector<std::vector<double>> qd_cmd_trace(kRightArmJointNames.size());
-    t_trace.reserve(total_steps);
-    target_vy_trace.reserve(total_steps);
-    target_vz_trace.reserve(total_steps);
-    command_vx_trace.reserve(total_steps);
-    command_vy_trace.reserve(total_steps);
-    command_vz_trace.reserve(total_steps);
-    for (auto& v : qd_cmd_trace) v.reserve(total_steps);
-
-    for (int step = 0; step < total_steps; ++step) {
-        const double t = static_cast<double>(step) * dt;
-
-        Eigen::Matrix<double, 6, 1> target_twist = Eigen::Matrix<double, 6, 1>::Zero();
-        if (t < segment_time) {
-            target_twist[1] = linear_speed_cmd;
-            target_twist[2] = z_speed_cmd;
-
-        } else if (t < 2.0 * segment_time) {
-            target_twist[1] = -linear_speed_cmd;
-            target_twist[2] = -z_speed_cmd;
-        } else if (t < total_time) {
-            target_twist.setZero();
-        } else {
-            target_twist.setZero();
-        }
-
-        std::vector<double> qd_cmd;
-        ASSERT_TRUE(solver.speedLStep(target_twist,
-                                      dt,
-                                      qd_cmd,
-                                      cmvr::CartesianFrame::Base,
-                                      true))
-            << "speedLStep failed at step " << step;
-
-        t_trace.push_back(t);
-        target_vy_trace.push_back(target_twist[1]);
-        target_vz_trace.push_back(target_twist[2]);
-        command_vx_trace.push_back(solver.getSpeedLCommandTwistBase()[0]);
-        command_vy_trace.push_back(solver.getSpeedLCommandTwistBase()[1]);
-        command_vz_trace.push_back(solver.getSpeedLCommandTwistBase()[2]);
-        for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
-            qd_cmd_trace[i].push_back(i < qd_cmd.size() ? qd_cmd[i] : 0.0);
-        }
-
-        ++ok_steps;
-
-        if ((step % log_every) == 0) {
-            std::cout << "[SPEEDL_OPEN_LOOP] step=" << step
-                      << " t=" << t
-                      << " target_vy=" << target_twist[1]
-                      << " target_vz=" << target_twist[2]
-                      << " phase="
-                      << (t < segment_time ? "pos" :
-                          (t < 2.0 * segment_time ? "neg" :
-                           (t < total_time ? "stop" : "settle")))
-                      << " cmd_qd=";
-            for (const auto& v : qd_cmd) {
-                std::cout << v << " ";
-            }
-            std::cout << std::endl;
-        }
-    }
-
-    if (!t_trace.empty()) {
-        using namespace matplot;
-        auto fig = figure(true);
-        fig->size(1600, 1200);
-        fig->font_size(16);
-
-        auto ax1 = subplot(3, 1, 0);
-        hold(ax1, true);
-        auto l_target_y = plot(ax1, t_trace, target_vy_trace, "k--");
-        l_target_y->line_width(2.0f);
-        auto l_target_z = plot(ax1, t_trace, target_vz_trace, "c--");
-        l_target_z->line_width(2.0f);
-        auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-");
-        l_cmd_x->line_width(2.0f);
-        auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-");
-        l_cmd_y->line_width(2.0f);
-        auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-");
-        l_cmd_z->line_width(2.0f);
-        title(ax1, "speedL target vy/vz vs command vxyz");
-        xlabel(ax1, "time [s]");
-        ylabel(ax1, "linear cmd [m/s]");
-        legend(ax1, {"target vy", "target vz", "command vx", "command vy", "command vz"});
-        grid(ax1, true);
-
-        auto ax2 = subplot(3, 1, 1);
-        hold(ax2, true);
-        std::vector<std::string> joint_labels;
-        joint_labels.reserve(kRightArmJointNames.size());
-        for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
-            auto line = plot(ax2, t_trace, qd_cmd_trace[i]);
-            line->line_width(1.8f);
-            joint_labels.emplace_back(kRightArmJointNames[i]);
-        }
-        title(ax2, "solver qd_cmd");
-        xlabel(ax2, "time [s]");
-        ylabel(ax2, "joint vel [rad/s]");
-        legend(ax2, joint_labels);
-        grid(ax2, true);
-
-        auto ax3 = subplot(3, 1, 2);
-        hold(ax3, true);
-        std::vector<double> qd_norm_trace;
-        qd_norm_trace.reserve(t_trace.size());
-        for (size_t k = 0; k < t_trace.size(); ++k) {
-            double norm = 0.0;
-            for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
-                const double v = qd_cmd_trace[i][k];
-                norm += v * v;
-            }
-            qd_norm_trace.push_back(std::sqrt(norm));
-        }
-        auto qd_norm_line = plot(ax3, t_trace, qd_norm_trace, "m-");
-        qd_norm_line->line_width(2.0f);
-        title(ax3, "solver qd_cmd norm");
-        xlabel(ax3, "time [s]");
-        ylabel(ax3, "norm [rad/s]");
-        legend(ax3, {"||qd_cmd||"});
-        grid(ax3, true);
-
-        show(fig);
-    }
-
-    EXPECT_GT(ok_steps, 0) << "No successful open-loop speedL steps.";
-}
 
 TEST(HumanoidRobotTest,IBVSWithRealRobot) {
     const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
@@ -558,7 +218,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
     auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
 
     auto robot = dmgr.getDevice<AbstractRobot>("hc01");
-    auto camera = dmgr.getDevice<AbstractCamera>("cam2");
+    auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam");
     ASSERT_NE(robot, nullptr);
     ASSERT_NE(camera, nullptr);
 
@@ -595,28 +255,45 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
         }
     } guard{camera, robot};
 
+    const double tag_size_m = 0.12;
+    const int tracked_tag_id = 0;
+    const double target_x = 0.0;
+    const double target_y = 0.0;
+    const double target_z = 0.40;
+    const cmvr::IbvsController::DepthMode ibvs_depth_mode = cmvr::IbvsController::DepthMode::MONOCULAR;
+
+    auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
+    perception->setTagSize(tag_size_m);
+    cmvr::perception::AprilTagPerception::Options perception_opt;
+    switch (ibvs_depth_mode) {
+        case cmvr::IbvsController::DepthMode::MONOCULAR:
+            perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
+            break;
+        case cmvr::IbvsController::DepthMode::PREFER_DEPTH:
+            perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER;
+            break;
+        case cmvr::IbvsController::DepthMode::DEPTH_ONLY:
+            perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
+            break;
+    }
+
     cmvr::IbvsController ibvs_controller;
     ibvs_controller.setMu(0.1);
     ibvs_controller.setLambda(0.6);
-    ibvs_controller.setQdotMax(0.6);
+    ibvs_controller.setTrackedTagId(tracked_tag_id);
-    ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
+    ibvs_controller.setTarget(target_x, target_y, target_z);
-    ibvs_controller.setTrackedTagId(0);
+    ibvs_controller.setDepthMode(ibvs_depth_mode);
-    ibvs_controller.setTarget(0.0, 0.0, 0.40);
-    ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR);
     ibvs_controller.setDepthZGain(1.0);
-    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
+    // Eigen::Matrix3d RxPi;
-    // ibvs_controller.setAlignCameraToVisp(I);   // RealSense optical -> ViSP
+    // RxPi << 1,0,0,
-    // ibvs_controller.setAlignCameraToUrdf(I);   // optical -> URDF 的 R_CAM
+    //         0,-1,0,
-    Eigen::Matrix3d RxPi;
+    //         0,0,-1;
-    RxPi << 1,0,0,
+    // ibvs_controller.setAlignCameraToUrdf(RxPi);
-            0,-1,0,
+    // ibvs_controller.setAlignCameraToVisp(RxPi);
-            0,0,-1;
-    ibvs_controller.setAlignCameraToUrdf(RxPi);
-    ibvs_controller.setAlignCameraToVisp(RxPi);
 
 
 
-    // ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0});
+    // ibvs_controller.setVelocityLimit6({0.3, 0.3, 0.3, 0.3, 0.3, 0.3});
     // ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005});
     ibvs_controller.setQdotMax(0.15);
     ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
@@ -629,6 +306,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
                                      "R_WRIST_R_S",
                                      "R_CAM"))
         << "IbvsController init failed";
+    ibvs_controller.setPerception(perception);
 
     auto q_map_now = makeRightArmQMap();
     robot->getJointQ(q_map_now);
@@ -653,23 +331,25 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
     const int max_steps = 30000;
     const int log_every = 1;
     const int cycle_ms = 10;
-    const double soft_margin = 0.20;   // rad
-    const double hard_margin = 0.04;   // rad
-    const double qdot_deadband = 0.01; // rad/s
-    const double qdot_lpf_alpha = 0.75;
-    const double qdot_acc_limit = 1.5; // rad/s^2
-    const double qdot_max_send = 0.25; // rad/s
 
     int ok_steps = 0;
     int fail_steps = 0;
-    std::vector<double> qdot_lpf(kRightArmJointNames.size(), 0.0);
+    int perception_fail_steps = 0;
-    std::vector<double> qdot_cmd_prev(kRightArmJointNames.size(), 0.0);
+    int compute_fail_steps = 0;
-    auto t_prev = std::chrono::steady_clock::now();
+    auto sendZeroVelocity = [&]() {
+        robot->speedJ(0);
+    };
+
+    for (int i = 0; i < 10; ++i) {
+        perception->update(perception_opt);
+        std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    }
 
     for (int step = 0; step < max_steps; ++step) {
         robot->getJointQ(q_map_now);
         if (!refreshRightArmQ()) {
             ++fail_steps;
+            sendZeroVelocity();
             if ((step % log_every) == 0) {
                 std::cout << "[IBVS_REAL] step=" << step << " joint extract failed\n";
             }
@@ -677,30 +357,46 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
             continue;
         }
 
-        const auto t_now = std::chrono::steady_clock::now();
+        const bool perception_ok = perception->update(perception_opt);
-        double dt = std::chrono::duration<double>(t_now - t_prev).count();
+        if (!perception_ok) {
-        t_prev = t_now;
+            ++perception_fail_steps;
-        dt = std::clamp(dt, 0.001, 0.1);
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL] step=" << step
+                          << " perception failed: "
+                          << cmvr::perception::AprilTagPerception::statusToString(perception->lastStatus())
+                          << std::endl;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
 
         std::vector<double> qdot_next;
         const bool ok = ibvs_controller.compute(q_now, qdot_next);
-
+        if (!ok) {
-
+            ++compute_fail_steps;
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL] step=" << step
+                          << " compute failed: "
+                          << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus());
+                if (perception->hasTags()) {
+                    std::cout << " visible_tags=";
+                    for (const auto& tag : perception->tags()) {
+                        std::cout << tag.id << " ";
+                    }
+                }
+                std::cout << std::endl;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
 
         std::vector<JointVelocityCommand> qd_send;
         for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
-            qdot_lpf[i] = qdot_lpf_alpha * qdot_lpf[i] + (1.0 - qdot_lpf_alpha) * qdot_next[i];
+            qd_send.push_back({kRightArmJointNames[i], qdot_next[i]});
-            double v = applySoftJointLimitVelocity(q_now[i], qdot_lpf[i],
-                                                   kRightArmQMin[i], kRightArmQMax[i],
-                                                   soft_margin, hard_margin);
-            if (std::abs(v) < qdot_deadband) {
-                v = 0.0;
-            }
-            const double dv_max = qdot_acc_limit * dt;
-            v = std::clamp(v, qdot_cmd_prev[i] - dv_max, qdot_cmd_prev[i] + dv_max);
-            v = std::clamp(v, -qdot_max_send, qdot_max_send);
-            qdot_cmd_prev[i] = v;
-            qd_send.push_back({kRightArmJointNames[i],v});
         }
 
         robot->speedJ(qd_send);
@@ -728,9 +424,315 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
     std::cout << "[IBVS_REAL] finished steps=" << max_steps
               << " ok_steps=" << ok_steps
               << " fail_steps=" << fail_steps
+              << " perception_fail_steps=" << perception_fail_steps
+              << " compute_fail_steps=" << compute_fail_steps
               << std::endl;
 
     robot->speedJ(0);
 
     EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
 }
+
+TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
+    const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
+    ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
+    auto dmgr_cfg = config.getChild("DeviceManager");
+    auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
+
+    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
+    auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam");
+    ASSERT_NE(robot, nullptr);
+    ASSERT_NE(camera, nullptr);
+
+    std::vector<JointPoint> init_cmd{};
+    init_cmd = {
+        {"R_SHOULDER_P", -0.2423},
+        {"R_SHOULDER_R", 1.2929},
+        {"R_SHOULDER_Y", 1.61},
+        {"R_ELBOW_R",     1.58},
+        {"R_WRIST_P",    -2.8792},
+        {"R_WRIST_Y",    0.1150},
+        {"R_WRIST_R",    -0.08},
+    };
+    robot->moveJ(init_cmd, 1.0, 2.0);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+    ASSERT_NO_THROW(camera->start());
+    struct RuntimeGuard {
+        std::shared_ptr<AbstractCamera> camera;
+        std::shared_ptr<AbstractRobot> robot;
+        ~RuntimeGuard() {
+            if (camera) {
+                try {
+                    camera->stop();
+                } catch (...) {
+                }
+            }
+            if (robot) {
+                try {
+                    robot->eStop();
+                } catch (...) {
+                }
+            }
+        }
+    } guard{camera, robot};
+
+    const double tag_size_m = 0.12;
+    const int preferred_tag_id = 0;
+    const Eigen::Vector3d hover_target_in_camera(0.0, 0.0, 0.40);
+    const cmvr::IbvsController::DepthMode ibvs_depth_mode = cmvr::IbvsController::DepthMode::MONOCULAR;
+
+    auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
+    perception->setTagSize(tag_size_m);
+    cmvr::perception::AprilTagPerception::Options perception_opt;
+    switch (ibvs_depth_mode) {
+        case cmvr::IbvsController::DepthMode::MONOCULAR:
+            perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
+            break;
+        case cmvr::IbvsController::DepthMode::PREFER_DEPTH:
+            perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER;
+            break;
+        case cmvr::IbvsController::DepthMode::DEPTH_ONLY:
+            perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
+            break;
+    }
+
+    cmvr::perception::TagRelativeTarget3D tracker(perception);
+    tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
+    tracker.setActiveTagSwitchPolicy(4, 1.2);
+    tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08);
+
+    cmvr::IbvsController ibvs_controller;
+    ibvs_controller.setMu(0.1);
+    ibvs_controller.setLambda(0.6);
+    ibvs_controller.setDepthMode(ibvs_depth_mode);
+    ibvs_controller.setDepthZGain(1.0);
+
+    ibvs_controller.setQdotMax(0.15);
+    ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
+
+    ASSERT_TRUE(ibvs_controller.init(
+                                     "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
+                                     "PELVIS_S",
+                                     "R_WRIST_R_S",
+                                     "R_CAM"))
+        << "IbvsController init failed";
+    ibvs_controller.setPerception(perception);
+
+    auto q_map_now = makeRightArmQMap();
+    robot->getJointQ(q_map_now);
+    std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
+    auto refreshRightArmQ = [&]() -> bool {
+        for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
+            const auto it = q_map_now.find(kRightArmJointNames[i]);
+            if (it == q_map_now.end()) {
+                return false;
+            }
+            q_now[i] = it->second;
+        }
+        return true;
+    };
+
+    ASSERT_TRUE(refreshRightArmQ())
+        << "Failed to extract right-arm 7 joints from robot state";
+    ibvs_controller.reset(q_now);
+
+    auto sendZeroVelocity = [&]() {
+        robot->speedJ(0);
+    };
+
+    auto projectPreferredTagCenter = [&](int& u, int& v) -> bool {
+        const auto* tag = perception->findTag(preferred_tag_id);
+        if (!tag) return false;
+        Eigen::Vector2d uv = Eigen::Vector2d::Zero();
+        const Eigen::Vector3d p_c_tag = tag->T_c_t.block<3, 1>(0, 3);
+        if (!cmvr::ImageProcess::projectCameraPointToPixel(perception->intrinsics(), p_c_tag, uv)) {
+            return false;
+        }
+        u = static_cast<int>(std::lround(uv.x()));
+        v = static_cast<int>(std::lround(uv.y()));
+        const cv::Mat& color = perception->color();
+        return !color.empty() && u >= 0 && v >= 0 && u < color.cols && v < color.rows;
+    };
+
+    for (int i = 0; i < 10; ++i) {
+        perception->update(perception_opt);
+        std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    }
+
+    const int max_steps = 30000;
+    const int log_every = 1;
+    const int cycle_ms = 10;
+
+    int ok_steps = 0;
+    int fail_steps = 0;
+    int perception_fail_steps = 0;
+    int tracking_fail_steps = 0;
+    int compute_fail_steps = 0;
+    bool target_locked = false;
+    bool ibvs_target_initialized = false;
+    int lock_u = -1;
+    int lock_v = -1;
+
+    for (int step = 0; step < max_steps; ++step) {
+        robot->getJointQ(q_map_now);
+        if (!refreshRightArmQ()) {
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL_TRACK] step=" << step << " joint extract failed\n";
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
+
+        const bool perception_ok = perception->update(perception_opt);
+        if (!perception_ok) {
+            ++perception_fail_steps;
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL_TRACK] step=" << step
+                          << " perception failed: "
+                          << cmvr::perception::AprilTagPerception::statusToString(perception->lastStatus())
+                          << std::endl;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
+
+        bool tracking_ok = false;
+        if (!target_locked) {
+            if (projectPreferredTagCenter(lock_u, lock_v)) {
+                tracking_ok = tracker.startTrackingFromPixel(lock_u, lock_v);
+                if (tracking_ok) {
+                    target_locked = true;
+                    ibvs_target_initialized = false;
+                    if ((step % log_every) == 0) {
+                        std::cout << "[IBVS_REAL_TRACK] step=" << step
+                                  << " lock point success"
+                                  << " uv=(" << lock_u << ", " << lock_v << ")"
+                                  << " active_tag=" << tracker.activeTagId()
+                                  << std::endl;
+                    }
+                }
+            }
+        } else {
+            tracking_ok = tracker.track();
+        }
+
+        if (!tracking_ok) {
+            ++tracking_fail_steps;
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL_TRACK] step=" << step
+                          << " tracker failed: "
+                          << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus());
+                if (perception->hasTags()) {
+                    std::cout << " visible_tags=";
+                    for (const auto& tag : perception->tags()) {
+                        std::cout << tag.id << " ";
+                    }
+                }
+                std::cout << std::endl;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
+
+        const int active_tag_id = tracker.activeTagId();
+        Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero();
+        if (active_tag_id < 0 || !tracker.getAnchorInTag(active_tag_id, p_t_target)) {
+            ++tracking_fail_steps;
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL_TRACK] step=" << step
+                          << " anchor unavailable, active_tag=" << active_tag_id
+                          << std::endl;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
+
+        ibvs_controller.setTrackedTagId(active_tag_id);
+        if (!ibvs_target_initialized || tracker.lastSwitched()) {
+            if (!ibvs_controller.setTargetFromPointInTag(p_t_target, hover_target_in_camera)) {
+                ++tracking_fail_steps;
+                ++fail_steps;
+                sendZeroVelocity();
+                if ((step % log_every) == 0) {
+                    std::cout << "[IBVS_REAL_TRACK] step=" << step
+                              << " setTargetFromPointInTag failed"
+                              << std::endl;
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+                continue;
+            }
+            ibvs_target_initialized = true;
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL_TRACK] step=" << step
+                          << " setTargetFromPointInTag ok"
+                          << " active_tag=" << active_tag_id
+                          << " p_t_target=[" << p_t_target.x() << ", "
+                          << p_t_target.y() << ", " << p_t_target.z() << "]"
+                          << std::endl;
+            }
+        }
+
+        std::vector<double> qdot_next;
+        const bool ok = ibvs_controller.compute(q_now, qdot_next);
+        if (!ok) {
+            ++compute_fail_steps;
+            ++fail_steps;
+            sendZeroVelocity();
+            if ((step % log_every) == 0) {
+                std::cout << "[IBVS_REAL_TRACK] step=" << step
+                          << " compute failed: "
+                          << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus())
+                          << " active_tag=" << active_tag_id
+                          << std::endl;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+            continue;
+        }
+
+        std::vector<JointVelocityCommand> qd_send;
+        for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
+            qd_send.push_back({kRightArmJointNames[i], qdot_next[i]});
+        }
+
+        robot->speedJ(qd_send);
+        ++ok_steps;
+
+        if ((step % log_every) == 0) {
+            const auto& t_co = ibvs_controller.lastTagPositionVisp();
+            const auto& v_c = ibvs_controller.lastCameraTwistVisp();
+            const auto& p_c_target = tracker.lastTargetInCamera();
+            std::cout << "[IBVS_REAL_TRACK] step=" << step
+                      << " active_tag=" << active_tag_id
+                      << " switched=" << static_cast<int>(tracker.lastSwitched())
+                      << " target_p_c=[" << p_c_target.x() << ", " << p_c_target.y() << ", " << p_c_target.z() << "]"
+                      << " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]"
+                      << " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2]
+                      << ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]"
+                      << " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage())
+                      << std::endl;
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
+    }
+
+    std::cout << "[IBVS_REAL_TRACK] finished steps=" << max_steps
+              << " ok_steps=" << ok_steps
+              << " fail_steps=" << fail_steps
+              << " perception_fail_steps=" << perception_fail_steps
+              << " tracking_fail_steps=" << tracking_fail_steps
+              << " compute_fail_steps=" << compute_fail_steps
+              << std::endl;
+
+    robot->speedJ(0);
+
+    EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps.";
+}

cmvr-es/service/CMakeLists.txt

@@ -18,6 +18,7 @@ target_link_libraries(service PRIVATE
         osqp
         cmvr_es::device_manager
         cmvr_es::controller
+        cmvr_es::applications
         protobuf::libprotobuf
 )
 
@@ -81,4 +82,3 @@ target_link_libraries(grpc_hlc_client_test
         fcl
         cmvr_es::device_manager
 )
-

cmvr-es/service/grpc/include/grpc_hlc_service.h

@@ -3,7 +3,10 @@
 //
 #pragma once
 
-#include "device_manager/include/device_manager.h"
+#include <mutex>
+#include <string>
+
+#include "applications/include/touch_screen_app.h"
 #include "cmvr/api/hlc_service.grpc.pb.h"
 
 namespace cmvr {
@@ -15,7 +18,11 @@ namespace cmvr {
             grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
 
         private:
-            device::DeviceManager& dmgr_;
+            bool ensureTouchAppInitialized(std::string& error_message);
+
+            std::mutex touch_mutex_;
+            app::TouchScreenApp touch_app_;
+            bool touch_app_initialized_{false};
         };
 
 

cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp

@@ -26,8 +26,8 @@ TEST(GrpcHlcClientTest, MyTest) {
     *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime();
 
 
-    request.set_u(12);
+    request.set_u(600);
-    request.set_v(13);
+    request.set_v(360);
     request.set_max_force(1300);
 
 

cmvr-es/service/grpc/src/grpc_hlc_service.cpp

@@ -4,37 +4,137 @@
 
 
 #include "../include/grpc_hlc_service.h"
+
+#include <chrono>
+#include <stdexcept>
+#include <thread>
+
+#include <glog/logging.h>
 #include <google/protobuf/util/time_util.h>
-#include "devices/robot/humanoid_robot/include/humanoid_robot.h"
+#include "applications/include/touch_screen_app.h"
-#include "controller/include/touch_controller.h"
 
 
 using namespace cmvr::service;
-using namespace cmvr::device;
 using namespace cmvr::api;
 using google::protobuf::util::TimeUtil;
 
-gRPCHlcServiceImpl::gRPCHlcServiceImpl():dmgr_(DeviceManager::getInstance()){}
+namespace {
 
-grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
+std::string buildTouchFailureMessage(const cmvr::app::TouchScreenApp& app,
-
+                                     const std::string& prefix) {
-    grpc::Status ret = grpc::Status::OK;
+    return prefix + ", phase=" +
-
+           cmvr::app::TouchScreenApp::phaseToString(app.phase()) +
-    // try {
+           ", status=" +
-    //     std::shared_ptr<AbstractRobot> robot = nullptr;
+           cmvr::app::TouchScreenApp::statusToString(app.lastStatus());
-    //     auto cam = dmgr_.getDevice<AbstractCamera>("cam4");
-    //     auto hand = dmgr_.getDevice<AbstractDexHand>("hand1");
-    //     ctrl::TouchController touch_controller(robot,hand,cam);;
-    //     touch_controller.touch(request->u(),request->v(),request->max_force());
-    //     response->mutable_header()->set_success(true);
-    //     response->mutable_header()->set_error_message("");
-    // }catch (const std::exception& e) {
-    //     response->mutable_header()->set_success(false);
-    //     response->mutable_header()->set_error_message(e.what());
-    //     ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    // }
-    // *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
-    return ret;
 }
 
+void fillTouchResponse(Touch_Response* response,
+                       const bool success,
+                       const std::string& error_message) {
+    response->mutable_header()->set_success(success);
+    response->mutable_header()->set_error_message(error_message);
+    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
+}
 
+} // namespace
+
+gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default;
+
+bool gRPCHlcServiceImpl::ensureTouchAppInitialized(std::string& error_message) {
+    if (touch_app_initialized_) {
+        return true;
+    }
+
+    LOG(INFO) << "[gRPCHlcServiceImpl] init TouchScreenApp from config";
+    if (!touch_app_.init()) {
+        error_message = buildTouchFailureMessage(touch_app_, "TouchScreenApp init failed");
+        touch_app_initialized_ = false;
+        return false;
+    }
+
+    touch_app_initialized_ = true;
+    return true;
+}
+
+grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
+    std::lock_guard<std::mutex> lock(touch_mutex_);
+
+    try {
+        LOG(INFO) << "[gRPCHlcServiceImpl] touch: request.device_id="
+                  << request->header().device_id()
+                  << " (ignored, using TouchScreenApp config)"
+                  << ", u=" << request->u()
+                  << ", v=" << request->v()
+                  << ", max_force ignored";
+
+        std::string init_error;
+        if (!ensureTouchAppInitialized(init_error)) {
+            fillTouchResponse(response, false, init_error);
+            return grpc::Status(grpc::StatusCode::INTERNAL, init_error);
+        }
+
+        if (!touch_app_.startFromPixel(request->u(), request->v())) {
+            throw std::runtime_error(
+                buildTouchFailureMessage(touch_app_, "TouchScreenApp startFromPixel failed"));
+        }
+
+        bool align_reached = false;
+        bool touch_triggered = false;
+        auto last_logged_status = cmvr::app::TouchScreenApp::Status::IDLE;
+        while (touch_app_.isBusy()) {
+            if (context != nullptr && context->IsCancelled()) {
+                touch_app_.stop();
+                const std::string error = "touch request cancelled";
+                fillTouchResponse(response, false, error);
+                return grpc::Status(grpc::StatusCode::CANCELLED, error);
+            }
+
+            if (!touch_app_.step()) {
+                throw std::runtime_error(
+                    buildTouchFailureMessage(touch_app_, "touch flow failed"));
+            }
+
+            if (touch_app_.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) {
+                align_reached = true;
+            }
+            if (touch_app_.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) {
+                touch_triggered = true;
+            }
+
+            if (touch_app_.lastStatus() != last_logged_status) {
+                LOG(INFO) << "[gRPCHlcServiceImpl] touch status="
+                          << cmvr::app::TouchScreenApp::statusToString(touch_app_.lastStatus())
+                          << ", phase="
+                          << cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase())
+                          << ", active_tag=" << touch_app_.lastActiveTagId()
+                          << ", pressure_sum=" << touch_app_.lastTouchPressureSum()
+                          << ", pressure_peak=" << touch_app_.lastTouchPressurePeak();
+                last_logged_status = touch_app_.lastStatus();
+            }
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+
+        if (!align_reached) {
+            throw std::runtime_error(
+                buildTouchFailureMessage(touch_app_, "touch exited without ALIGN_REACHED"));
+        }
+        if (!touch_triggered) {
+            throw std::runtime_error(
+                buildTouchFailureMessage(touch_app_, "touch exited without TOUCH_TRIGGERED"));
+        }
+        if (!touch_app_.isFinished()) {
+            throw std::runtime_error(
+                buildTouchFailureMessage(touch_app_, "touch did not finish successfully"));
+        }
+
+        LOG(INFO) << "[gRPCHlcServiceImpl] touch done"
+                  << ", final_status="
+                  << cmvr::app::TouchScreenApp::statusToString(touch_app_.lastStatus());
+        fillTouchResponse(response, true, "");
+        return grpc::Status::OK;
+    } catch (const std::exception& e) {
+        fillTouchResponse(response, false, e.what());
+        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
+    }
+}

protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto

@@ -0,0 +1,122 @@
+syntax = "proto3";
+
+package cmvr.config;
+
+message TouchScreenVec3 {
+  optional double x = 1;
+  optional double y = 2;
+  optional double z = 3;
+}
+
+message TouchScreenTwist6 {
+  optional double vx = 1;
+  optional double vy = 2;
+  optional double vz = 3;
+  optional double wx = 4;
+  optional double wy = 5;
+  optional double wz = 6;
+}
+
+message TouchScreenMatrix3d {
+  optional double m00 = 1;
+  optional double m01 = 2;
+  optional double m02 = 3;
+  optional double m10 = 4;
+  optional double m11 = 5;
+  optional double m12 = 6;
+  optional double m20 = 7;
+  optional double m21 = 8;
+  optional double m22 = 9;
+}
+
+message TouchScreenInitJointPoint {
+  optional string joint_name = 1;
+  optional double rad = 2;
+}
+
+enum TouchScreenDepthPolicy {
+  TOUCH_SCREEN_DEPTH_POLICY_NONE = 0;
+  TOUCH_SCREEN_DEPTH_POLICY_PREFER = 1;
+  TOUCH_SCREEN_DEPTH_POLICY_REQUIRE = 2;
+}
+
+enum TouchScreenTargetPointMethod {
+  TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE = 0;
+  TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE = 1;
+}
+
+enum TouchScreenFingerType {
+  TOUCH_SCREEN_FINGER_TYPE_PINKY = 0;
+  TOUCH_SCREEN_FINGER_TYPE_RING = 1;
+  TOUCH_SCREEN_FINGER_TYPE_MIDDLE = 2;
+  TOUCH_SCREEN_FINGER_TYPE_INDEX = 3;
+  TOUCH_SCREEN_FINGER_TYPE_THUMB = 4;
+}
+
+enum TouchScreenTactileRegion {
+  TOUCH_SCREEN_TACTILE_REGION_TIP = 0;
+  TOUCH_SCREEN_TACTILE_REGION_FINGER = 1;
+  TOUCH_SCREEN_TACTILE_REGION_PAD = 2;
+  TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER = 3;
+  TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4;
+}
+
+message TouchScreenAppConfig {
+  optional string robot_id = 40;
+  optional string dexhand_id = 41;
+  optional string camera_id = 42;
+  optional bool move_to_init_position = 43;
+  repeated TouchScreenInitJointPoint init_joint_positions = 44;
+  optional double init_movej_vel = 45;
+  optional double init_movej_acc = 46;
+  optional bool move_to_init_position_before_start = 47;
+
+  optional string urdf_path = 1;
+  optional string base_link = 2;
+  optional string flange_link = 3;
+  optional string camera_link = 4;
+
+  optional double tag_size_m = 5;
+  optional TouchScreenDepthPolicy depth_policy = 6;
+  optional TouchScreenTargetPointMethod target_point_method = 7;
+
+  TouchScreenVec3 hover_target_in_camera = 8;
+  optional double target_rx = 9;
+  optional double target_ry = 10;
+  optional double target_rz = 11;
+
+  optional double ibvs_lambda = 12;
+  optional double ibvs_mu = 13;
+  optional double ibvs_qdot_max = 14;
+  TouchScreenTwist6 ibvs_vmax6 = 15;
+  optional bool enable_joint_limit_avoidance = 16;
+  optional double joint_limit_avoidance_gain = 17;
+  optional double joint_limit_avoidance_margin_ratio = 18;
+  optional double joint_limit_avoidance_max_push = 19;
+
+  TouchScreenMatrix3d r_camera_to_visp = 20;
+  TouchScreenMatrix3d r_camera_to_urdf = 21;
+
+  repeated string control_joint_names = 22;
+
+  optional double align_xy_threshold_m = 23;
+  optional double align_z_threshold_m = 24;
+  optional double align_rot_threshold_rad = 25;
+  optional int32 align_stable_frames = 26;
+  optional double align_timeout_s = 27;
+  optional bool pause_after_align_reached = 28;
+
+  TouchScreenTwist6 touch_twist_base = 29;
+  optional double touch_acceleration = 30;
+  optional double touch_forward_duration_s = 31;
+
+  optional double dwell_time_s = 32;
+  TouchScreenTwist6 retract_twist_base = 33;
+  optional double retract_acceleration = 34;
+  optional double retract_duration_s = 35;
+
+  optional TouchScreenFingerType tactile_finger = 36;
+  optional TouchScreenTactileRegion tactile_region = 37;
+  optional double tactile_pressure_sum_threshold = 38;
+  optional double tactile_pressure_peak_threshold = 39;
+}