feat: add touch screen app class

CMakeLists.txt

@@ -121,6 +121,7 @@ target_link_libraries(cmvr_es PRIVATE
         cmvr_es::planner
         cmvr_es::device::humanoid_robot
         cmvr_es::common
+        cmvr_es::applications
 )
 
 install(TARGETS cmvr_es RUNTIME DESTINATION bin)

cmvr-es/CMakeLists.txt

@@ -11,5 +11,6 @@ add_subdirectory(planner)
 
 add_subdirectory(ik_solver)
 add_subdirectory(data_center)
+add_subdirectory(applications)
 add_subdirectory(simulate)
 add_subdirectory(common)

cmvr-es/applications/CMakeLists.txt

@@ -0,0 +1,25 @@
+add_library(applications
+       src/touch_screen_app.cpp
+)
+
+target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(applications PUBLIC
+        cmvr_es::controller
+)
+
+add_library(cmvr_es::applications ALIAS applications)
+install(TARGETS applications LIBRARY DESTINATION lib)
+
+add_executable(touch_screen_app_test
+        src/touch_screen_app_test.cpp
+)
+
+target_link_libraries(touch_screen_app_test PRIVATE
+        cmvr_es::applications
+        cmvr_es::device_manager
+        gtest
+        gtest_main
+        pthread
+        glog
+)

cmvr-es/applications/include/touch_screen_app.h

@@ -0,0 +1,225 @@
+#pragma once
+
+#ifndef CMVR_ES_TOUCH_SCREEN_APP_H
+#define CMVR_ES_TOUCH_SCREEN_APP_H
+
+#include <array>
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "controller/include/ibvs_controller.h"
+#include "devices/camera/abstract_camera.h"
+#include "devices/dexhand/abstract_dexhand.h"
+#include "devices/robot/abstract_robot.h"
+#include "perception/include/apriltag_perception.h"
+#include "perception/include/tag_relative_target_3d.h"
+
+namespace cmvr::app {
+
+class TouchScreenApp {
+public:
+    enum class Phase {
+        IDLE = 0,
+        ALIGNING,
+        ALIGN_REACHED,
+        TOUCHING,
+        DWELLING,
+        RETRACTING,
+        DONE,
+        FAILED
+    };
+
+    enum class Status {
+        IDLE = 0,
+        NOT_INITIALIZED,
+        INVALID_CONFIG,
+        CONTROL_JOINT_MISMATCH,
+        ALIGN_WAITING_PERCEPTION,
+        ALIGN_WAITING_TRACK,
+        ALIGN_TARGET_SETUP_FAILED,
+        ALIGN_COMPUTE_FAILED,
+        ALIGN_TIMEOUT,
+        ALIGNING,
+        ALIGN_REACHED,
+        TOUCHING,
+        TOUCH_TRIGGERED,
+        TOUCH_TIMEOUT,
+        RETRACTING,
+        DONE,
+        STOPPED,
+        ROBOT_STATE_FAILED,
+        ROBOT_COMMAND_FAILED
+    };
+
+    enum class TactileRegion {
+        TIP = 0,
+        FINGER,
+        PAD,
+        TIP_AND_FINGER,
+        THUMB_MIDDLE
+    };
+
+    struct Options {
+        // IBVS / IK 初始化参数。
+        std::string urdf_path;
+        std::string base_link{"PELVIS_S"};
+        std::string flange_link{"R_WRIST_R_S"};
+        std::string camera_link;
+
+        // 视觉感知参数。
+        double tag_size_m{0.12};
+        perception::AprilTagPerception::DepthPolicy depth_policy{
+            perception::AprilTagPerception::DepthPolicy::NONE};
+        perception::TagRelativeTarget3D::TargetPointMethod target_point_method{
+            perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE};
+
+        // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。
+        Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.12};
+        double target_rx{3.14159265358979323846};
+        double target_ry{0.0};
+        double target_rz{0.0};
+
+        // IBVS 参数。
+        double ibvs_lambda{0.7};
+        double ibvs_mu{0.02};
+        double ibvs_qdot_max{0.6};
+        std::array<double, 6> ibvs_vmax6{{0.4, 0.4, 0.4, 0.4, 0.4, 0.4}};
+        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_max_push{0.25};
+        Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()};
+        Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()};
+
+        // 关节控制链，默认右臂 7 轴。
+        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"};
+
+        // 视觉对准收敛判据。
+        double align_xy_threshold_m{0.003};
+        double align_z_threshold_m{0.010};
+        int align_stable_frames{5};
+        double align_timeout_s{10.0};
+        bool pause_after_align_reached{false};
+
+        // 触控阶段：speedL 目标 twist（base_link 系）。
+        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()};
+        double touch_acceleration{0.6};
+        double touch_timeout_s{2.0};
+
+        // 接触后停留与回退。
+        double dwell_time_s{0.05};
+        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()};
+        double retract_acceleration{0.8};
+        double retract_duration_s{0.20};
+
+        // 指尖触觉判据。
+        device::FingerType tactile_finger{device::FingerType::INDEX};
+        TactileRegion tactile_region{TactileRegion::FINGER};
+        double tactile_pressure_sum_threshold{3000.0};
+        double tactile_pressure_peak_threshold{0.0};
+    };
+
+    TouchScreenApp();
+    ~TouchScreenApp() = default;
+
+    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);
+
+    void setOptions(const Options& options);
+    const Options& options() const { return options_; }
+
+    bool startFromPixel(int u, int v);
+    bool step();
+    void stop();
+
+    Phase phase() const { return phase_; }
+    Status lastStatus() const { return last_status_; }
+    static const char* phaseToString(Phase phase);
+    static const char* statusToString(Status status);
+
+    bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
+                                 phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
+                                 phase_ == Phase::RETRACTING; }
+    bool isFinished() const { return phase_ == Phase::DONE; }
+    bool isFailed() const { return phase_ == Phase::FAILED; }
+
+    int targetU() const { return target_u_; }
+    int targetV() const { return target_v_; }
+    double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
+    double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
+    int lastActiveTagId() const { return last_active_tag_id_; }
+    const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
+
+    const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
+    const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
+    const IbvsController& ibvs() const { return ibvs_; }
+
+private:
+    using Clock = std::chrono::steady_clock;
+
+    bool applyOptions();
+    bool validateControlJointNames() const;
+    bool stepAligning();
+    bool stepTouching();
+    bool stepDwelling();
+    bool stepRetracting();
+
+    bool readControlledJointPositions(std::vector<double>& q_out) const;
+    bool sendJointVelocity(const std::vector<double>& qdot) const;
+    bool sendZeroJointVelocity() const;
+    bool holdCurrentControlledPosition() const;
+
+    bool startTouchPhase();
+    bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
+    void enterFailed(Status status);
+
+    bool updateTouchPressure();
+
+private:
+    std::shared_ptr<device::AbstractRobot> robot_{nullptr};
+    std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
+    std::shared_ptr<device::AbstractCamera> camera_{nullptr};
+
+    std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
+    perception::TagRelativeTarget3D tracker_;
+    IbvsController ibvs_;
+    Options options_{};
+
+    Phase phase_{Phase::IDLE};
+    Phase phase_after_retract_{Phase::DONE};
+    Status last_status_{Status::NOT_INITIALIZED};
+
+    bool initialized_{false};
+    bool target_locked_{false};
+    bool ibvs_target_initialized_{false};
+    bool touch_command_started_{false};
+    bool retract_command_started_{false};
+
+    int target_u_{-1};
+    int target_v_{-1};
+    int align_stable_count_{0};
+    int last_active_tag_id_{-1};
+
+    double last_touch_pressure_sum_{0.0};
+    double last_touch_pressure_peak_{0.0};
+    Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
+
+    Clock::time_point phase_start_time_{};
+    Status final_status_after_retract_{Status::DONE};
+};
+
+using touch_screen_app = TouchScreenApp;
+
+} // namespace cmvr::app
+
+#endif // CMVR_ES_TOUCH_SCREEN_APP_H

cmvr-es/applications/src/touch_screen_app.cpp

@@ -0,0 +1,704 @@
+#include "applications/include/touch_screen_app.h"
+
+#include <algorithm>
+#include <cmath>
+#include <exception>
+#include <unordered_map>
+
+namespace cmvr::app {
+
+namespace {
+
+std::vector<double> toStdVector6(const Eigen::Matrix<double, 6, 1>& twist) {
+    std::vector<double> out(6, 0.0);
+    for (int i = 0; i < 6; ++i) {
+        out[static_cast<size_t>(i)] = twist[i];
+    }
+    return out;
+}
+
+void accumulateMatrixStats(const std::vector<std::vector<device::TactilePoint>>& matrix,
+                           double& sum_out,
+                           double& peak_out) {
+    for (const auto& row : matrix) {
+        for (const auto value : row) {
+            const double v = static_cast<double>(value);
+            sum_out += v;
+            peak_out = std::max(peak_out, v);
+        }
+    }
+}
+
+} // namespace
+
+TouchScreenApp::TouchScreenApp()
+    : tracker_(nullptr) {}
+
+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) {
+    robot_ = robot;
+    dexhand_ = dexhand;
+    camera_ = camera;
+    options_ = options;
+
+    if (!robot_ || !dexhand_ || !camera_) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    if (options_.urdf_path.empty() || options_.camera_link.empty() || options_.control_joint_names.empty()) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    perception_ = std::make_shared<perception::AprilTagPerception>(camera_);
+    perception_->setTagSize(options_.tag_size_m);
+
+    tracker_.setPerception(perception_);
+    tracker_.setTargetPointMethod(options_.target_point_method);
+
+    if (!ibvs_.init(options_.urdf_path,
+                    options_.base_link,
+                    options_.flange_link,
+                    options_.camera_link)) {
+        initialized_ = false;
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    ibvs_.setPerception(perception_);
+    if (!validateControlJointNames()) {
+        initialized_ = false;
+        last_status_ = Status::CONTROL_JOINT_MISMATCH;
+        return false;
+    }
+
+    initialized_ = applyOptions();
+    if (initialized_) {
+        tracker_.clear();
+        tracker_.resetActiveTagTracking();
+        ibvs_.reset();
+        phase_ = Phase::IDLE;
+        phase_after_retract_ = Phase::DONE;
+        final_status_after_retract_ = Status::DONE;
+        target_locked_ = false;
+        ibvs_target_initialized_ = false;
+        touch_command_started_ = false;
+        retract_command_started_ = false;
+        align_stable_count_ = 0;
+        last_active_tag_id_ = -1;
+        last_touch_pressure_sum_ = 0.0;
+        last_touch_pressure_peak_ = 0.0;
+        last_align_error_camera_.setZero();
+        last_status_ = Status::IDLE;
+    } else {
+        last_status_ = Status::INVALID_CONFIG;
+    }
+    return initialized_;
+}
+
+void TouchScreenApp::setOptions(const Options& options) {
+    options_ = options;
+    if (initialized_) {
+        applyOptions();
+    }
+}
+
+bool TouchScreenApp::startFromPixel(int u, int v) {
+    if (!initialized_) {
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+    if (u < 0 || v < 0) {
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
+
+    stop();
+    tracker_.clear();
+    tracker_.resetActiveTagTracking();
+    ibvs_.reset();
+
+    target_u_ = u;
+    target_v_ = v;
+    target_locked_ = false;
+    ibvs_target_initialized_ = false;
+    touch_command_started_ = false;
+    retract_command_started_ = false;
+    align_stable_count_ = 0;
+    last_touch_pressure_sum_ = 0.0;
+    last_touch_pressure_peak_ = 0.0;
+    last_active_tag_id_ = -1;
+    last_align_error_camera_.setZero();
+    phase_ = Phase::ALIGNING;
+    phase_after_retract_ = Phase::DONE;
+    final_status_after_retract_ = Status::DONE;
+    phase_start_time_ = Clock::now();
+    last_status_ = Status::ALIGN_WAITING_TRACK;
+    return true;
+}
+
+bool TouchScreenApp::step() {
+    if (!initialized_) {
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+
+    switch (phase_) {
+        case Phase::IDLE:
+            last_status_ = Status::IDLE;
+            return true;
+        case Phase::ALIGNING:
+            return stepAligning();
+        case Phase::ALIGN_REACHED:
+            last_status_ = Status::ALIGN_REACHED;
+            if (options_.pause_after_align_reached) {
+                return true;
+            }
+            if (!startTouchPhase()) {
+                enterFailed(Status::ROBOT_COMMAND_FAILED);
+                return false;
+            }
+            return true;
+        case Phase::TOUCHING:
+            return stepTouching();
+        case Phase::DWELLING:
+            return stepDwelling();
+        case Phase::RETRACTING:
+            return stepRetracting();
+        case Phase::DONE:
+            last_status_ = Status::DONE;
+            return true;
+        case Phase::FAILED:
+            return false;
+    }
+    last_status_ = Status::INVALID_CONFIG;
+    return false;
+}
+
+void TouchScreenApp::stop() {
+    if (robot_) {
+        try {
+            robot_->stopSpeedL();
+        } catch (...) {
+        }
+    }
+
+    sendZeroJointVelocity();
+    holdCurrentControlledPosition();
+
+    phase_ = Phase::IDLE;
+    phase_after_retract_ = Phase::DONE;
+    final_status_after_retract_ = Status::DONE;
+    target_locked_ = false;
+    ibvs_target_initialized_ = false;
+    touch_command_started_ = false;
+    retract_command_started_ = false;
+    align_stable_count_ = 0;
+    last_active_tag_id_ = -1;
+    last_touch_pressure_sum_ = 0.0;
+    last_touch_pressure_peak_ = 0.0;
+    last_align_error_camera_.setZero();
+    last_status_ = Status::STOPPED;
+}
+
+const char* TouchScreenApp::phaseToString(const Phase phase) {
+    switch (phase) {
+        case Phase::IDLE: return "IDLE";
+        case Phase::ALIGNING: return "ALIGNING";
+        case Phase::ALIGN_REACHED: return "ALIGN_REACHED";
+        case Phase::TOUCHING: return "TOUCHING";
+        case Phase::DWELLING: return "DWELLING";
+        case Phase::RETRACTING: return "RETRACTING";
+        case Phase::DONE: return "DONE";
+        case Phase::FAILED: return "FAILED";
+    }
+    return "UNKNOWN";
+}
+
+const char* TouchScreenApp::statusToString(const Status status) {
+    switch (status) {
+        case Status::IDLE: return "IDLE";
+        case Status::NOT_INITIALIZED: return "NOT_INITIALIZED";
+        case Status::INVALID_CONFIG: return "INVALID_CONFIG";
+        case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH";
+        case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION";
+        case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK";
+        case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED";
+        case Status::ALIGN_COMPUTE_FAILED: return "ALIGN_COMPUTE_FAILED";
+        case Status::ALIGN_TIMEOUT: return "ALIGN_TIMEOUT";
+        case Status::ALIGNING: return "ALIGNING";
+        case Status::ALIGN_REACHED: return "ALIGN_REACHED";
+        case Status::TOUCHING: return "TOUCHING";
+        case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED";
+        case Status::TOUCH_TIMEOUT: return "TOUCH_TIMEOUT";
+        case Status::RETRACTING: return "RETRACTING";
+        case Status::DONE: return "DONE";
+        case Status::STOPPED: return "STOPPED";
+        case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED";
+        case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED";
+    }
+    return "UNKNOWN";
+}
+
+bool TouchScreenApp::applyOptions() {
+    if (!perception_) {
+        return false;
+    }
+
+    perception_->setTagSize(options_.tag_size_m);
+    tracker_.setTargetPointMethod(options_.target_point_method);
+    ibvs_.setLambda(options_.ibvs_lambda);
+    ibvs_.setMu(options_.ibvs_mu);
+    ibvs_.setQdotMax(options_.ibvs_qdot_max);
+    ibvs_.setVelocityLimit6(options_.ibvs_vmax6);
+    ibvs_.setJointLimitAvoidance(options_.enable_joint_limit_avoidance,
+                                 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_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
+    return true;
+}
+
+bool TouchScreenApp::validateControlJointNames() const {
+    std::vector<std::string> solver_joint_names;
+    if (!ibvs_.getChainJointNames(solver_joint_names)) {
+        return false;
+    }
+    if (solver_joint_names == options_.control_joint_names) {
+        return true;
+    }
+
+    std::cerr << "[TouchScreenApp] control_joint_names mismatch with IbvsController IK chain\n";
+    std::cerr << "  app joints   :";
+    for (const auto& name : options_.control_joint_names) {
+        std::cerr << " " << name;
+    }
+    std::cerr << "\n  solver joints:";
+    for (const auto& name : solver_joint_names) {
+        std::cerr << " " << name;
+    }
+    std::cerr << '\n';
+    return false;
+}
+
+bool TouchScreenApp::stepAligning() {
+    const auto now = Clock::now();
+    const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
+    if (elapsed > options_.align_timeout_s) {
+        enterFailed(Status::ALIGN_TIMEOUT);
+        return false;
+    }
+
+    perception::AprilTagPerception::Options perception_options;
+    perception_options.depth_policy = options_.depth_policy;
+    perception_options.detect_tags = true;
+    perception_options.fetch_encoded = false;
+    if (!perception_->update(perception_options)) {
+        sendZeroJointVelocity();
+        last_status_ = Status::ALIGN_WAITING_PERCEPTION;
+        return true;
+    }
+
+    bool tracking_ok = false;
+    if (!target_locked_) {
+        tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_);
+        if (tracking_ok) {
+            target_locked_ = true;
+            ibvs_target_initialized_ = false;
+            align_stable_count_ = 0;
+        }
+    } else {
+        tracking_ok = tracker_.track();
+    }
+
+    if (!tracking_ok) {
+        sendZeroJointVelocity();
+        align_stable_count_ = 0;
+        last_status_ = Status::ALIGN_WAITING_TRACK;
+        return true;
+    }
+
+    const int tag_id = tracker_.activeTagId();
+    last_active_tag_id_ = tag_id;
+    if (tag_id < 0) {
+        sendZeroJointVelocity();
+        align_stable_count_ = 0;
+        last_status_ = Status::ALIGN_WAITING_TRACK;
+        return true;
+    }
+
+    Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero();
+    if (!tracker_.getAnchorInTag(tag_id, p_t_target)) {
+        sendZeroJointVelocity();
+        align_stable_count_ = 0;
+        last_status_ = Status::ALIGN_WAITING_TRACK;
+        return true;
+    }
+
+    ibvs_.setTrackedTagId(tag_id);
+    if (!ibvs_target_initialized_ || tracker_.lastSwitched()) {
+        if (!ibvs_.setTargetFromPointInTag(p_t_target,
+                                           options_.hover_target_in_camera,
+                                           options_.target_rx,
+                                           options_.target_ry,
+                                           options_.target_rz)) {
+            enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
+            return false;
+        }
+        ibvs_target_initialized_ = true;
+    }
+
+    std::vector<double> q_now;
+    if (!readControlledJointPositions(q_now)) {
+        enterFailed(Status::ROBOT_STATE_FAILED);
+        return false;
+    }
+
+    std::vector<double> qdot_cmd;
+    if (!ibvs_.compute(q_now, qdot_cmd)) {
+        switch (ibvs_.lastComputeStatus()) {
+            case IbvsController::ComputeStatus::NO_NEW_FRAME:
+            case IbvsController::ComputeStatus::NO_TAG:
+            case IbvsController::ComputeStatus::TAG_MISMATCH:
+            case IbvsController::ComputeStatus::NO_DEPTH:
+                sendZeroJointVelocity();
+                align_stable_count_ = 0;
+                last_status_ = Status::ALIGN_WAITING_TRACK;
+                return true;
+            case IbvsController::ComputeStatus::OK:
+            case IbvsController::ComputeStatus::NOT_READY:
+            case IbvsController::ComputeStatus::BAD_IMAGE:
+            case IbvsController::ComputeStatus::INVALID_INPUT:
+            case IbvsController::ComputeStatus::IK_FAILED:
+            default:
+                enterFailed(Status::ALIGN_COMPUTE_FAILED);
+                return false;
+        }
+    }
+
+    if (!sendJointVelocity(qdot_cmd)) {
+        enterFailed(Status::ROBOT_COMMAND_FAILED);
+        return false;
+    }
+
+    last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera;
+    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) {
+        ++align_stable_count_;
+    } else {
+        align_stable_count_ = 0;
+    }
+
+    if (align_stable_count_ >= options_.align_stable_frames) {
+        sendZeroJointVelocity();
+        phase_ = Phase::ALIGN_REACHED;
+        phase_start_time_ = Clock::now();
+        touch_command_started_ = false;
+        last_status_ = Status::ALIGN_REACHED;
+        return true;
+    }
+
+    last_status_ = Status::ALIGNING;
+    return true;
+}
+
+bool TouchScreenApp::stepTouching() {
+    if (!touch_command_started_) {
+        if (!startTouchPhase()) {
+            enterFailed(Status::ROBOT_COMMAND_FAILED);
+            return false;
+        }
+    }
+
+    updateTouchPressure();
+    if (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)) {
+        try {
+            robot_->stopSpeedL();
+        } catch (...) {
+            enterFailed(Status::ROBOT_COMMAND_FAILED);
+            return false;
+        }
+        phase_ = Phase::DWELLING;
+        phase_start_time_ = Clock::now();
+        last_status_ = Status::TOUCH_TRIGGERED;
+        return true;
+    }
+
+    const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
+    if (elapsed > options_.touch_timeout_s) {
+        try {
+            robot_->stopSpeedL();
+        } catch (...) {
+            enterFailed(Status::ROBOT_COMMAND_FAILED);
+            return false;
+        }
+        if (!startRetractPhase(Phase::FAILED, Status::TOUCH_TIMEOUT)) {
+            enterFailed(Status::ROBOT_COMMAND_FAILED);
+            return false;
+        }
+        return true;
+    }
+
+    last_status_ = Status::TOUCHING;
+    return true;
+}
+
+bool TouchScreenApp::stepDwelling() {
+    const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
+    if (elapsed < options_.dwell_time_s) {
+        last_status_ = Status::TOUCH_TRIGGERED;
+        return true;
+    }
+
+    if (!startRetractPhase(Phase::DONE, Status::DONE)) {
+        enterFailed(Status::ROBOT_COMMAND_FAILED);
+        return false;
+    }
+    return true;
+}
+
+bool TouchScreenApp::stepRetracting() {
+    if (!retract_command_started_) {
+        if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) {
+            enterFailed(Status::ROBOT_COMMAND_FAILED);
+            return false;
+        }
+    }
+
+    const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
+    if (elapsed < options_.retract_duration_s) {
+        last_status_ = Status::RETRACTING;
+        return true;
+    }
+
+    try {
+        robot_->stopSpeedL();
+    } catch (...) {
+        enterFailed(Status::ROBOT_COMMAND_FAILED);
+        return false;
+    }
+    holdCurrentControlledPosition();
+    phase_ = phase_after_retract_;
+    last_status_ = final_status_after_retract_;
+    return phase_ != Phase::FAILED;
+}
+
+bool TouchScreenApp::readControlledJointPositions(std::vector<double>& q_out) const {
+    if (!robot_) {
+        return false;
+    }
+
+    std::vector<device::JointState> states;
+    robot_->getJointsState(states);
+    std::unordered_map<std::string, double> q_map;
+    q_map.reserve(states.size());
+    for (const auto& state : states) {
+        q_map[state.name] = state.position;
+    }
+
+    q_out.resize(options_.control_joint_names.size());
+    for (size_t i = 0; i < options_.control_joint_names.size(); ++i) {
+        const auto it = q_map.find(options_.control_joint_names[i]);
+        if (it == q_map.end()) {
+            return false;
+        }
+        q_out[i] = it->second;
+    }
+    return true;
+}
+
+bool TouchScreenApp::sendJointVelocity(const std::vector<double>& qdot) const {
+    if (!robot_ || qdot.size() != options_.control_joint_names.size()) {
+        return false;
+    }
+
+    std::vector<device::JointVelocityCommand> cmd;
+    cmd.reserve(qdot.size());
+    for (size_t i = 0; i < qdot.size(); ++i) {
+        cmd.push_back({options_.control_joint_names[i], qdot[i]});
+    }
+
+    try {
+        robot_->speedJ(cmd);
+    } catch (...) {
+        return false;
+    }
+    return true;
+}
+
+bool TouchScreenApp::sendZeroJointVelocity() const {
+    std::vector<double> zero(options_.control_joint_names.size(), 0.0);
+    return sendJointVelocity(zero);
+}
+
+bool TouchScreenApp::holdCurrentControlledPosition() const {
+    if (!robot_) {
+        return false;
+    }
+
+    std::vector<device::JointState> states;
+    robot_->getJointsState(states);
+    std::unordered_map<std::string, double> q_map;
+    q_map.reserve(states.size());
+    for (const auto& state : states) {
+        q_map[state.name] = state.position;
+    }
+
+    std::vector<device::JointPoint> joints;
+    joints.reserve(options_.control_joint_names.size());
+    for (const auto& name : options_.control_joint_names) {
+        const auto it = q_map.find(name);
+        if (it == q_map.end()) {
+            return false;
+        }
+        joints.emplace_back(name, it->second, 0.0);
+    }
+
+    try {
+        robot_->servoJ(joints, 0.02);
+    } catch (...) {
+        return false;
+    }
+    return true;
+}
+
+bool TouchScreenApp::startTouchPhase() {
+    if (!robot_) {
+        return false;
+    }
+    try {
+        if (!robot_->speedL(toStdVector6(options_.touch_twist_base),
+                            options_.touch_acceleration,
+                            0.0)) {
+            return false;
+        }
+    } catch (...) {
+        return false;
+    }
+    phase_ = Phase::TOUCHING;
+    phase_start_time_ = Clock::now();
+    touch_command_started_ = true;
+    retract_command_started_ = false;
+    last_status_ = Status::TOUCHING;
+    return true;
+}
+
+bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract,
+                                       const Status final_status_after_retract) {
+    if (!robot_) {
+        return false;
+    }
+    try {
+        if (!robot_->speedL(toStdVector6(options_.retract_twist_base),
+                            options_.retract_acceleration,
+                            0.0)) {
+            return false;
+        }
+    } catch (...) {
+        return false;
+    }
+    phase_ = Phase::RETRACTING;
+    phase_after_retract_ = next_phase_after_retract;
+    final_status_after_retract_ = final_status_after_retract;
+    phase_start_time_ = Clock::now();
+    retract_command_started_ = true;
+    last_status_ = Status::RETRACTING;
+    return true;
+}
+
+void TouchScreenApp::enterFailed(const Status status) {
+    try {
+        if (robot_) {
+            robot_->stopSpeedL();
+        }
+    } catch (...) {
+    }
+    sendZeroJointVelocity();
+    holdCurrentControlledPosition();
+    phase_ = Phase::FAILED;
+    touch_command_started_ = false;
+    retract_command_started_ = false;
+    last_status_ = status;
+}
+
+bool TouchScreenApp::updateTouchPressure() {
+    if (!dexhand_) {
+        last_touch_pressure_sum_ = 0.0;
+        last_touch_pressure_peak_ = 0.0;
+        return false;
+    }
+
+    const auto& sensors = dexhand_->getSensorData();
+    double sum = 0.0;
+    double peak = 0.0;
+
+    auto accumulate_finger = [&](const auto& finger_sensor) {
+        switch (options_.tactile_region) {
+            case TactileRegion::TIP:
+                accumulateMatrixStats(finger_sensor.tip.data, sum, peak);
+                break;
+            case TactileRegion::FINGER:
+                accumulateMatrixStats(finger_sensor.finger.data, sum, peak);
+                break;
+            case TactileRegion::PAD:
+                accumulateMatrixStats(finger_sensor.pad.data, sum, peak);
+                break;
+            case TactileRegion::TIP_AND_FINGER:
+                accumulateMatrixStats(finger_sensor.tip.data, sum, peak);
+                accumulateMatrixStats(finger_sensor.finger.data, sum, peak);
+                break;
+            case TactileRegion::THUMB_MIDDLE:
+                break;
+        }
+    };
+
+    switch (options_.tactile_finger) {
+        case device::FingerType::PINKY:
+            accumulate_finger(sensors.pinky);
+            break;
+        case device::FingerType::RING:
+            accumulate_finger(sensors.ring);
+            break;
+        case device::FingerType::MIDDLE:
+            accumulate_finger(sensors.middle);
+            break;
+        case device::FingerType::INDEX:
+            accumulate_finger(sensors.index);
+            break;
+        case device::FingerType::THUMB:
+            switch (options_.tactile_region) {
+                case TactileRegion::TIP:
+                    accumulateMatrixStats(sensors.thumb.tip.data, sum, peak);
+                    break;
+                case TactileRegion::FINGER:
+                    accumulateMatrixStats(sensors.thumb.finger.data, sum, peak);
+                    break;
+                case TactileRegion::PAD:
+                    accumulateMatrixStats(sensors.thumb.pad.data, sum, peak);
+                    break;
+                case TactileRegion::TIP_AND_FINGER:
+                    accumulateMatrixStats(sensors.thumb.tip.data, sum, peak);
+                    accumulateMatrixStats(sensors.thumb.finger.data, sum, peak);
+                    break;
+                case TactileRegion::THUMB_MIDDLE:
+                    accumulateMatrixStats(sensors.thumb.middle.data, sum, peak);
+                    break;
+            }
+            break;
+    }
+
+    last_touch_pressure_sum_ = sum;
+    last_touch_pressure_peak_ = peak;
+    return true;
+}
+
+} // namespace cmvr::app

cmvr-es/applications/src/touch_screen_app_test.cpp

@@ -0,0 +1,112 @@
+#include "gtest/gtest.h"
+
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include "applications/include/touch_screen_app.h"
+#include "device_manager/include/device_manager.h"
+
+namespace {
+
+constexpr const char* kConfigPath =
+    "/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml";
+// 这些 id 需要与现场配置一致；保持为示例调用中的写法。
+constexpr const char* kRobotId = "hc01";
+constexpr const char* kDexhandId = "dexhand1";
+constexpr const char* kCameraId = "cam1";
+constexpr const char* kUrdfPath =
+    "/home/lgv/cmvr/0-workspace/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 = 320;
+constexpr int kTargetV = 240;
+
+void run_touch_once(int u, int v) {
+    const XmlNode config(kConfigPath);
+    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);
+
+
+    cmvr::app::TouchScreenApp app;
+    cmvr::app::TouchScreenApp::Options opt;
+
+    opt.urdf_path = kUrdfPath;
+    opt.base_link = kBaseLink;
+    opt.flange_link = kFlangeLink;
+    opt.camera_link = kCameraLink;
+    opt.tag_size_m = 0.02;
+
+    opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.12);
+    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;
+
+    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;
+    }
+
+    while (app.isBusy()) {
+        if (!app.step()) {
+            std::cerr << "touch failed, status="
+                      << 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()
+                  << ", 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";
+            app.stop();
+            break;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    if (app.isFinished()) {
+        std::cout << "touch done\n";
+    } else if (app.isFailed()) {
+        std::cout << "touch failed: "
+                  << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
+                  << "\n";
+    }
+}
+
+} // namespace
+
+TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) {
+    run_touch_once(kTargetU, kTargetV);
+}

cmvr-es/controller/include/ibvs_controller.h

@@ -241,6 +241,13 @@ public:
     // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
     const Eigen::Matrix<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
 
+    /**
+     * @brief 获取当前 IK 链的关节名称（base->camera 顺序）。
+     * @param joint_names 输出关节名称列表。
+     * @return 已初始化且成功读取时返回 `true`。
+     */
+    bool getChainJointNames(std::vector<std::string>& joint_names) const;
+
 private:
     /**
      * @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。

cmvr-es/controller/src/controller_test.cpp

@@ -540,7 +540,7 @@ protected:
     ibvs_controller_->setDepthZGain(1.0);
     ibvs_controller_->setVelocityLimit6(vmax6_);
     ibvs_controller_->setTrackedTagId(tracked_tag_id_);
-    ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.0, 0),
+    ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.05, 0),
                                            Eigen::Vector3d(0.0, 0.0, 0.4));
     ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
 

cmvr-es/controller/src/ibvs_controller.cpp

@@ -146,6 +146,14 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
     return computeInternal(joints_angle, qdot_out);
 }
 
+bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
+    if (!dls_solver_) {
+        joint_names.clear();
+        return false;
+    }
+    return dls_solver_->getChainJointNames(joint_names);
+}
+
 bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
                                      std::vector<double>& qdot_out) {
     last_depth_usage_ = DepthUsage::NONE;
@@ -284,9 +292,15 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
         return false;
     }
 
+    Eigen::Map<const Eigen::VectorXd> q_chain(joints_angle.data(), static_cast<Eigen::Index>(joints_angle.size()));
+    Eigen::Map<const Eigen::VectorXd> qdot_vec(qdot.data(), static_cast<Eigen::Index>(qdot.size()));
+    const Eigen::VectorXd qdot_soft_limited = dls_solver_->applyJointSoftLimitVelocity(q_chain, qdot_vec);
+
     qdot_out.resize(qdot.size());
     for (size_t i = 0; i < qdot.size(); ++i) {
-        qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
+        qdot_out[i] = SupportFunctions::clamp(qdot_soft_limited[static_cast<Eigen::Index>(i)],
+                                              -qdot_max_,
+                                              qdot_max_);
     }
 
     last_compute_status_ = ComputeStatus::OK;

cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h

@@ -90,6 +90,22 @@ public:
             double damping = -1.0,
             double qdot_abs_max = std::numeric_limits<double>::infinity());
 
+    /**
+     * @brief 对靠近关节位置限位且仍继续向外运动的关节速度做软压缩。
+     *
+     * 该接口不改变主任务的求解方式，只在得到关节速度命令后做逐轴后处理。
+     * 软/硬边界参数来自当前 speedL 配置中的：
+     * - `joint_soft_limit_margin`
+     * - `joint_hard_limit_margin`
+     * - `enable_joint_soft_limit_velocity`
+     *
+     * @param q_chain 当前链关节位置，size=chain_dof_，单位 rad。
+     * @param qdot_des 待后处理的关节速度命令，size=chain_v_dof_，单位 rad/s。
+     * @return 经过软限位速度压缩后的关节速度命令。
+     */
+    Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain,
+                                                const Eigen::VectorXd& qdot_des);
+
     void setJointLimitAvoidance(bool enable,
                                 double gain = 0.2,
                                 double margin_ratio = 0.15,
@@ -245,8 +261,6 @@ private:
                                   Eigen::VectorXd* q_full_out = nullptr);
 
     Eigen::VectorXd applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const;
-    Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain,
-                                                const Eigen::VectorXd& qdot_des);
 
     Eigen::VectorXd applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des,
                                                  const Eigen::VectorXd& qdot_reference,