feat:add ibvs acc limit

cmvr-es/applications/include/touch_screen_app.h

@@ -116,6 +116,10 @@ public:
         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}};
+        // 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`。
+        std::array<double, 6> ibvs_amax6{{0.4, 0.4, 0.5, 1.5, 1.5, 1.5}};
+        // 相机 twist 一阶低通滤波系数，范围 [0, 1]。
+        double ibvs_twist_filter_alpha{0.35};
         // 是否启用关节限位回避。
         bool enable_joint_limit_avoidance{true};
         // 关节限位回避增益。
@@ -221,7 +225,7 @@ public:
     bool setTouchSpeedlForwardL(double forward_l);
 
     bool startFromPixel(int u, int v);
-    bool step();
+    bool step(double dt);
     void stop();
 
     Phase phase() const { return phase_; }
@@ -255,7 +259,7 @@ private:
     void setOptions(const Options& options);
     bool applyOptions();
     bool validateControlJointNames() const;
-    bool stepAligning();
+    bool stepAligning(double dt);
     bool stepTouching();
     bool stepDwelling();
     bool stepRetracting();
@@ -263,6 +267,7 @@ private:
     bool readControlledJointPositions(std::vector<double>& q_out) const;
     bool sendJointVelocity(const std::vector<double>& qdot) const;
     bool sendZeroJointVelocity() const;
+    void hardStopIbvsMotion();
     bool holdCurrentControlledPosition() const;
     bool moveToInitPositionIfEnabled() const;
     bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;

cmvr-es/applications/src/touch_screen_app.cpp

@@ -606,11 +606,15 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
     return true;
 }
 
-bool TouchScreenApp::step() {
+bool TouchScreenApp::step(const double dt) {
     if (!initialized_) {
         last_status_ = Status::NOT_INITIALIZED;
         return false;
     }
+    if (!std::isfinite(dt) || dt <= 0.0) {
+        last_status_ = Status::INVALID_CONFIG;
+        return false;
+    }
 
     if (dexhand_) {
         const bool tactile_ok = updateTouchPressure();
@@ -629,7 +633,7 @@ bool TouchScreenApp::step() {
             last_status_ = Status::IDLE;
             return true;
         case Phase::ALIGNING:
-            return stepAligning();
+            return stepAligning(dt);
         case Phase::ALIGN_REACHED:
             last_status_ = Status::ALIGN_REACHED;
             if (options_.pause_after_align_reached) {
@@ -669,6 +673,7 @@ void TouchScreenApp::stop() {
     }
 
     sendZeroJointVelocity();
+    ibvs_.resetTwistCommandState();
     holdCurrentControlledPosition();
 
     phase_ = Phase::IDLE;
@@ -814,6 +819,18 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
             options.ibvs_vmax6[static_cast<size_t>(i)] = ibvs_vmax[i];
         }
     }
+    if (config.has_ibvs_amax6()) {
+        Eigen::Matrix<double, 6, 1> ibvs_amax;
+        ibvs_amax << options.ibvs_amax6[0], options.ibvs_amax6[1], options.ibvs_amax6[2],
+            options.ibvs_amax6[3], options.ibvs_amax6[4], options.ibvs_amax6[5];
+        applyTwist6FromConfig(config.ibvs_amax6(), ibvs_amax);
+        for (int i = 0; i < 6; ++i) {
+            options.ibvs_amax6[static_cast<size_t>(i)] = ibvs_amax[i];
+        }
+    }
+    if (config.has_ibvs_twist_filter_alpha()) {
+        options.ibvs_twist_filter_alpha = std::clamp(config.ibvs_twist_filter_alpha(), 0.0, 1.0);
+    }
     if (config.has_align_error_threshold6()) {
         Eigen::Matrix<double, 6, 1> align_err;
         align_err << options.align_error_threshold6[0], options.align_error_threshold6[1], options.align_error_threshold6[2],
@@ -1007,6 +1024,8 @@ bool TouchScreenApp::applyOptions() {
     ibvs_.setMu(options_.ibvs_mu);
     ibvs_.setQdotMax(options_.ibvs_qdot_max);
     ibvs_.setVelocityLimit6(options_.ibvs_vmax6);
+    ibvs_.setAccelerationLimit6(options_.ibvs_amax6);
+    ibvs_.setTwistFilterAlpha(options_.ibvs_twist_filter_alpha);
     ibvs_.setJointLimitAvoidance(options_.enable_joint_limit_avoidance,
                                  options_.joint_limit_avoidance_gain,
                                  options_.joint_limit_avoidance_margin_ratio,
@@ -1038,20 +1057,21 @@ bool TouchScreenApp::validateControlJointNames() const {
     return false;
 }
 
-bool TouchScreenApp::stepAligning() {
+bool TouchScreenApp::stepAligning(const double dt) {
     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;
     }
+    const double ibvs_dt = std::clamp(dt, 0.005, 0.05);
 
     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();
+        hardStopIbvsMotion();
         last_status_ = Status::ALIGN_WAITING_PERCEPTION;
         return true;
     }
@@ -1069,7 +1089,7 @@ bool TouchScreenApp::stepAligning() {
     }
 
     if (!tracking_ok) {
-        sendZeroJointVelocity();
+        hardStopIbvsMotion();
         align_stable_count_ = 0;
         last_status_ = Status::ALIGN_WAITING_TRACK;
         return true;
@@ -1078,7 +1098,7 @@ bool TouchScreenApp::stepAligning() {
     const int tag_id = tracker_.activeTagId();
     last_active_tag_id_ = tag_id;
     if (tag_id < 0) {
-        sendZeroJointVelocity();
+        hardStopIbvsMotion();
         align_stable_count_ = 0;
         last_status_ = Status::ALIGN_WAITING_TRACK;
         return true;
@@ -1086,7 +1106,7 @@ bool TouchScreenApp::stepAligning() {
 
     Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero();
     if (!tracker_.getAnchorInTag(tag_id, p_t_target)) {
-        sendZeroJointVelocity();
+        hardStopIbvsMotion();
         align_stable_count_ = 0;
         last_status_ = Status::ALIGN_WAITING_TRACK;
         return true;
@@ -1094,7 +1114,7 @@ bool TouchScreenApp::stepAligning() {
 
     const auto* current_tag = perception_->findTag(tag_id);
     if (!current_tag) {
-        sendZeroJointVelocity();
+        hardStopIbvsMotion();
         align_stable_count_ = 0;
         last_status_ = Status::ALIGN_WAITING_TRACK;
         return true;
@@ -1151,13 +1171,13 @@ bool TouchScreenApp::stepAligning() {
     }
 
     std::vector<double> qdot_cmd;
-    if (!ibvs_.compute(q_now, qdot_cmd)) {
+    if (!ibvs_.compute(q_now, ibvs_dt, 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();
+                hardStopIbvsMotion();
                 align_stable_count_ = 0;
                 last_status_ = Status::ALIGN_WAITING_TRACK;
                 return true;
@@ -1206,7 +1226,7 @@ bool TouchScreenApp::stepAligning() {
     }
 
     if (align_stable_count_ >= options_.align_stable_frames) {
-        sendZeroJointVelocity();
+        hardStopIbvsMotion();
         phase_ = Phase::ALIGN_REACHED;
         phase_start_time_ = Clock::now();
         touch_command_started_ = false;
@@ -1390,6 +1410,11 @@ bool TouchScreenApp::sendZeroJointVelocity() const {
     return sendJointVelocity(zero);
 }
 
+void TouchScreenApp::hardStopIbvsMotion() {
+    sendZeroJointVelocity();
+    ibvs_.resetTwistCommandState();
+}
+
 bool TouchScreenApp::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const {
     if (!robot_) {
         return false;
@@ -1622,7 +1647,7 @@ void TouchScreenApp::enterFailed(const Status status) {
         }
     } catch (...) {
     }
-    sendZeroJointVelocity();
+    hardStopIbvsMotion();
     holdCurrentControlledPosition();
     phase_ = Phase::FAILED;
     touch_command_started_ = false;

cmvr-es/applications/src/touch_screen_app_test.cpp

@@ -1,5 +1,6 @@
 #include "gtest/gtest.h"
 
+#include <algorithm>
 #include <chrono>
 #include <iostream>
 #include <thread>
@@ -36,8 +37,19 @@ void run_touch_once(int u, int v) {
 
     bool align_reached = false;
     bool touch_triggered = false;
+    auto last_step_time = std::chrono::steady_clock::now();
+    bool first_step = true;
     while (app.isBusy()) {
-        const bool step_ok = app.step();
+        const auto now = std::chrono::steady_clock::now();
+        double dt = 0.02;
+        if (!first_step) {
+            dt = std::chrono::duration<double>(now - last_step_time).count();
+            dt = std::clamp(dt, 0.005, 0.05);
+        }
+        last_step_time = now;
+        first_step = false;
+
+        const bool step_ok = app.step(dt);
         const auto& p_c_target = app.tracker().lastTargetInCamera();
         std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase())
                   << ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())

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

@@ -64,6 +64,15 @@ ibvs_vmax6 {
   wy: 0.6
   wz: 0.6
 }
+ibvs_amax6 {
+  vx: 0.4
+  vy: 0.4
+  vz: 0.5
+  wx: 1.5
+  wy: 1.5
+  wz: 1.5
+}
+ibvs_twist_filter_alpha: 0.35
 enable_joint_limit_avoidance: true
 joint_limit_avoidance_gain: 0.2
 joint_limit_avoidance_margin_ratio: 0.15

cmvr-es/controller/include/ibvs_controller.h

@@ -109,6 +109,9 @@ public:
      * @param joints_angle 当前关节角。
      * @param qdot_out 输出的关节速度命令。
      * @return 计算成功返回 `true`。
+     *
+     * @note 不带 dt 的接口内部会使用一个保守默认周期来执行 twist 限加速度，
+     *       更推荐使用带 dt 的 compute 接口。
      */
     bool compute(const std::vector<double>& joints_angle,
                  std::vector<double>& qdot_out);
@@ -190,6 +193,25 @@ public:
      */
     void setVelocityLimit6(const std::array<double, 6>& vmax6);
 
+    /**
+     * @brief 设置相机 twist 六维加速度限幅。
+     * @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度。
+     */
+    void setAccelerationLimit6(const std::array<double, 6>& amax6);
+
+    /**
+     * @brief 设置相机 twist 一阶低通滤波系数。
+     * @param alpha 滤波系数，范围 [0, 1]；0 表示不启用，1 表示不过滤。
+     */
+    void setTwistFilterAlpha(double alpha);
+
+    /**
+     * @brief 将内部相机 twist 平滑状态同步到静止状态。
+     *
+     * 适用于上层已经执行硬停（直接发 0 速）后，避免下一次恢复控制时继续沿用旧的 twist 历史。
+     */
+    void resetTwistCommandState();
+
     /**
      * @brief 设置关节限位回避参数。
      * @param enable 是否启用。
@@ -214,31 +236,14 @@ public:
      */
     void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
 
-    // 最近一帧是否检测到了当前被跟踪的 tag。
     bool isTagDetected() const { return last_tag_detected_; }
-
-    // 最近一次 `compute()` 的状态。
     ComputeStatus lastComputeStatus() const { return last_compute_status_; }
-
-    // 计算状态转字符串。
     static const char* statusToString(ComputeStatus status);
-
-    // 最近一次 `compute()` 实际使用的深度来源。
     DepthUsage lastDepthUsage() const { return last_depth_usage_; }
-
-    // 深度来源转字符串。
     static const char* depthUsageToString(DepthUsage usage);
-
-    // 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
     const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
-
-    // 当前配置要跟踪的 tag id。
     int trackedTagId() const { return tracked_tag_id_; }
-
-    // 最近一次成功控制时实际使用的 tag id。
     int lastUsedTagId() const { return last_used_tag_id_; }
-
-    // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
     const Eigen::Matrix<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
 
     /**
@@ -252,10 +257,12 @@ private:
     /**
      * @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。
      * @param joints_angle 当前关节角。
+     * @param dt 控制周期，单位秒。
      * @param qdot_out 输出的关节速度命令。
      * @return 计算成功返回 `true`。
      */
     bool computeInternal(const std::vector<double>& joints_angle,
+                         double dt,
                          std::vector<double>& qdot_out);
 
     /**
@@ -281,128 +288,72 @@ private:
     void initTask();
 
 private:
-    // 控制器和 IK 求解器是否已成功初始化。
     bool initialized_{false};
-
-    // IK 链基座 frame 名称。
     std::string base_frame_name_;
-
-    // URDF 中相机 frame 名称，对应坐标系 `u`。
     std::string camera_frame_name_;
-
-    // 共享感知前端。
     std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
 
-    // ViSP 视觉伺服增益。
     double lambda_{0.7};
-
-    // 控制模型使用的 tag 边长，单位米。
     double tag_size_m_{0.12};
-
-    // 控制模型使用的 tag 半边长，单位米。
     double tag_half_{0.06};
 
-    // 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 x 坐标，单位米。
     double target_x_{0.0};
-
-    // 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 y 坐标，单位米。
     double target_y_{0.0};
-
-    // 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 z 坐标，单位米。
     double target_z_{0.33};
 
-    // 期望位姿 `cMo_des` 的旋转参数 rx，单位弧度。
     double target_rx_{3.14159265358979323846};
-
-    // 期望位姿 `cMo_des` 的旋转参数 ry，单位弧度。
     double target_ry_{0.0};
-
-    // 期望位姿 `cMo_des` 的旋转参数 rz，单位弧度。
     double target_rz_{0.0};
 
-    // DLS IK 阻尼系数。
     double mu_{0.02};
-
-    // 关节速度上限。
     double qdot_max_{0.6};
 
-    // 深度使用模式。
     DepthMode depth_mode_{DepthMode::MONOCULAR};
-
-    // 深度闭环比例增益。
     double depth_z_kp_{1.0};
 
-    // 相机 twist 六维限幅。
     std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
 
-    // 是否启用关节限位回避。
+    // 新增：相机 twist 六维加速度限幅
+    std::array<double, 6> amax6_{{0.4, 0.4, 0.5, 1.5, 1.5, 1.5}};
+
+    // 新增：相机 twist 一阶低通滤波系数
+    double twist_lpf_alpha_{0.35};
+
     bool limit_avoidance_enabled_{false};
-
-    // 关节限位回避增益。
     double limit_avoidance_gain_{0.2};
-
-    // 关节限位回避触发边界比例。
     double limit_avoidance_margin_ratio_{0.15};
-
-    // 单关节最大推回速度。
     double limit_avoidance_max_push_{0.25};
 
-    // 用于深度闭环的 tag 平面控制点，位于 tag 坐标系 `t` 的 xy 平面，单位米。
     Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
 
-    // 从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。
     Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
-
-    // 从 `AbstractCamera` 相机坐标系 `cam` 到 URDF 相机坐标系 `u` 的旋转矩阵。
     Eigen::Matrix3d R_camera_urdf_{Eigen::Matrix3d::Identity()};
 
-    // ViSP 视觉伺服任务。
     std::unique_ptr<vpServo> task_{nullptr};
-
-    // tag 四个角点在 tag 坐标系 `t` 中的 3D 模型点。
     vpPoint obj_pts_[4];
-
-    // 当前观测特征。
     vpFeaturePoint s_cur_[4];
-
-    // 期望特征。
     vpFeaturePoint s_star_[4];
 
-    // 当前配置要跟踪的 tag id。
     int tracked_tag_id_{-1};
-
-    // 速度 IK 求解器。
     std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
 
-    // 是否成功读取 URDF 中的关节位置限位。
     bool has_joint_position_limits_{false};
-
-    // 关节位置下限。
     Eigen::VectorXd q_lower_limits_;
-
-    // 关节位置上限。
     Eigen::VectorXd q_upper_limits_;
 
-    // 最近一次成功控制时实际使用的 tag id。
     int last_used_tag_id_{-1};
-
-    // 内部积分得到的关节位置命令缓存。
     std::vector<double> q_cmd_;
 
-    // 最近一帧是否检测到了当前被跟踪的 tag。
     bool last_tag_detected_{false};
-
-    // 最近一次 `compute()` 的状态。
     ComputeStatus last_compute_status_{ComputeStatus::NOT_READY};
-
-    // 最近一次 `compute()` 实际使用的深度来源。
     DepthUsage last_depth_usage_{DepthUsage::NONE};
 
-    // 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
     Eigen::Vector3d last_tag_pos_visp_{Eigen::Vector3d::Zero()};
-
-    // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
     Eigen::Matrix<double, 6, 1> last_v_camera_visp_{Eigen::Matrix<double, 6, 1>::Zero()};
+
+    // 新增：上一拍实际发送给 IK 的相机 twist（ViSP 相机系）
+    Eigen::Matrix<double, 6, 1> v_camera_cmd_prev_{Eigen::Matrix<double, 6, 1>::Zero()};
+    bool has_v_camera_cmd_prev_{false};
 };
 
 } // namespace cmvr

cmvr-es/controller/src/ibvs_controller.cpp

@@ -42,6 +42,7 @@ const char* IbvsController::depthUsageToString(DepthUsage usage) {
 IbvsController::IbvsController() {
     R_cv_.setIdentity();
     R_camera_urdf_.setIdentity();
+    resetTwistCommandState();
 
     updateDepthControlPointInTag();
     initTask();
@@ -77,7 +78,6 @@ bool IbvsController::init(const std::string& urdf_path,
 
 void IbvsController::setPerception(const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception) {
     perception_ = perception;
-    // 注入后立刻同步一次 tag size（用于控制模型）
     syncTagSizeFromPerception(true);
 }
 
@@ -92,10 +92,7 @@ void IbvsController::syncTagSizeFromPerception(bool force) {
     tag_size_m_ = s;
     tag_half_ = 0.5 * tag_size_m_;
 
-    // 深度控制点约束依赖 tag_half_
     updateDepthControlPointInTag();
-
-    // 任务几何依赖 tag_half_
     initTask();
 }
 
@@ -113,6 +110,7 @@ void IbvsController::reset(const std::vector<double>& q_init) {
     last_depth_usage_ = DepthUsage::NONE;
     last_tag_pos_visp_.setZero();
     last_v_camera_visp_.setZero();
+    resetTwistCommandState();
 }
 
 bool IbvsController::compute(const std::vector<double>& joints_angle,
@@ -128,7 +126,7 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
     }
 
     std::vector<double> qdot;
-    if (!computeInternal(joints_angle, qdot)) {
+    if (!computeInternal(joints_angle, dt, qdot)) {
         return false;
     }
 
@@ -143,7 +141,8 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
 
 bool IbvsController::compute(const std::vector<double>& joints_angle,
                              std::vector<double>& qdot_out) {
-    return computeInternal(joints_angle, qdot_out);
+    constexpr double kDefaultDt = 0.02; // 50 Hz
+    return computeInternal(joints_angle, kDefaultDt, qdot_out);
 }
 
 bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
@@ -155,6 +154,7 @@ bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) c
 }
 
 bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
+                                     double dt,
                                      std::vector<double>& qdot_out) {
     last_depth_usage_ = DepthUsage::NONE;
     last_tag_detected_ = false;
@@ -170,6 +170,10 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
         last_compute_status_ = ComputeStatus::NOT_READY;
         return false;
     }
+    if (dt <= 0.0 || !std::isfinite(dt)) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
     if (joints_angle.empty()) {
         last_compute_status_ = ComputeStatus::INVALID_INPUT;
         return false;
@@ -179,10 +183,8 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
         return false;
     }
 
-    // 每帧确保 tag_size 同步（你可能运行时调 perception->setTagSize()）
     syncTagSizeFromPerception(false);
 
-    // perception 必须先 update()，这里以 color 是否为空作为“是否ready”的简单判据
     if (perception_->color().empty()) {
         last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
         return false;
@@ -205,7 +207,6 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
     const vpHomogeneousMatrix& cMo = tag->cMo;
     last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
 
-    // intrinsics
     const auto& intr = perception_->intrinsics();
     const double fx = static_cast<double>(intr.fx);
     const double fy = static_cast<double>(intr.fy);
@@ -250,7 +251,7 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
         s_cur_[i].buildFrom(x, y, Z);
     }
 
-    last_depth_usage_ = depth_ctrl_used ? DepthUsage::DEPTH_ONLY : DepthUsage::POSE_ONLY;
+    last_depth_usage_ = depth_ctrl_used ? DepthUsage::MIXED : DepthUsage::POSE_ONLY;
 
     vpColVector v_c = task_->computeControlLaw();
 
@@ -259,9 +260,41 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
         v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
     }
 
-    // 限幅+缓存（ViSP camera系）
+    // ---------- 视觉速度整形：速度限幅 + 加速度限幅 + 一阶低通 ----------
+    Eigen::Matrix<double, 6, 1> v_raw;
     for (int i = 0; i < 6; ++i) {
-        v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
+        v_raw[i] = std::isfinite(v_c[i]) ? v_c[i] : 0.0;
+    }
+
+    // 1) 速度限幅
+    for (int i = 0; i < 6; ++i) {
+        v_raw[i] = SupportFunctions::clamp(v_raw[i], -vmax6_[i], vmax6_[i]);
+    }
+
+    if (!has_v_camera_cmd_prev_) {
+        resetTwistCommandState();
+    }
+
+    // 2) 加速度限幅
+    Eigen::Matrix<double, 6, 1> v_acc_limited = v_camera_cmd_prev_;
+    for (int i = 0; i < 6; ++i) {
+        const double amax = std::max(0.0, amax6_[i]);
+        const double dv_max = amax * dt;
+        const double dv_des = v_raw[i] - v_camera_cmd_prev_[i];
+        const double dv = SupportFunctions::clamp(dv_des, -dv_max, dv_max);
+        v_acc_limited[i] = v_camera_cmd_prev_[i] + dv;
+    }
+
+    // 3) 一阶低通
+    const double alpha = std::clamp(twist_lpf_alpha_, 0.0, 1.0);
+    if (alpha <= 0.0 || alpha >= 1.0) {
+        v_camera_cmd_prev_ = v_acc_limited;
+    } else {
+        v_camera_cmd_prev_ = alpha * v_acc_limited + (1.0 - alpha) * v_camera_cmd_prev_;
+    }
+
+    for (int i = 0; i < 6; ++i) {
+        v_c[i] = v_camera_cmd_prev_[i];
         last_v_camera_visp_[i] = v_c[i];
     }
 
@@ -346,17 +379,17 @@ void IbvsController::setTarget(double x,
     initTask();
 }
 
-    bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
+bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
-                                                 const Eigen::Vector3d& p_c_target_des,
+                                             const Eigen::Vector3d& p_c_target_des,
-                                                 double rx,
+                                             double rx,
-                                                 double ry,
+                                             double ry,
-                                                 double rz) {
+                                             double rz) {
     if (!p_t_target.allFinite() || !p_c_target_des.allFinite()) {
         return false;
     }
 
     vpRotationMatrix R_des_visp;
-    R_des_visp.buildFrom(rx, ry, rz);  // 注意：这是 rotvec（theta*u）
+    R_des_visp.buildFrom(rx, ry, rz);
 
     Eigen::Matrix3d R_des;
     for (int r = 0; r < 3; ++r) {
@@ -399,6 +432,20 @@ void IbvsController::setVelocityLimit6(const std::array<double, 6>& vmax6) {
     vmax6_ = vmax6;
 }
 
+void IbvsController::setAccelerationLimit6(const std::array<double, 6>& amax6) {
+    amax6_ = amax6;
+}
+
+void IbvsController::setTwistFilterAlpha(double alpha) {
+    twist_lpf_alpha_ = std::clamp(alpha, 0.0, 1.0);
+}
+
+void IbvsController::resetTwistCommandState() {
+    v_camera_cmd_prev_.setZero();
+    last_v_camera_visp_.setZero();
+    has_v_camera_cmd_prev_ = true;
+}
+
 void IbvsController::setJointLimitAvoidance(bool enable,
                                             double gain,
                                             double margin_ratio,
@@ -440,7 +487,6 @@ void IbvsController::updateDepthControlPointInTag() {
 
     depth_control_point_tag_ = A.fullPivLu().solve(b);
 
-    // 限制在 tag 边界内
     depth_control_point_tag_.x() = SupportFunctions::clamp(depth_control_point_tag_.x(), -tag_half_, tag_half_);
     depth_control_point_tag_.y() = SupportFunctions::clamp(depth_control_point_tag_.y(), -tag_half_, tag_half_);
 }

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

@@ -5,6 +5,7 @@
 
 #include "../include/grpc_hlc_service.h"
 
+#include <algorithm>
 #include <chrono>
 #include <stdexcept>
 #include <thread>
@@ -92,6 +93,8 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
         bool align_reached = false;
         bool touch_triggered = false;
         auto last_logged_status = cmvr::app::TouchScreenApp::Status::IDLE;
+        auto last_step_time = std::chrono::steady_clock::now();
+        bool first_step = true;
         while (touch_app_.isBusy()) {
             if (context != nullptr && context->IsCancelled()) {
                 touch_app_.stop();
@@ -100,7 +103,16 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
                 return grpc::Status(grpc::StatusCode::CANCELLED, error);
             }
 
-            if (!touch_app_.step()) {
+            const auto now = std::chrono::steady_clock::now();
+            double dt = 0.02;
+            if (!first_step) {
+                dt = std::chrono::duration<double>(now - last_step_time).count();
+                dt = std::clamp(dt, 0.005, 0.05);
+            }
+            last_step_time = now;
+            first_step = false;
+
+            if (!touch_app_.step(dt)) {
                 throw std::runtime_error(
                     buildTouchFailureMessage(touch_app_, "touch flow failed"));
             }

protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto

@@ -104,6 +104,8 @@ message TouchScreenAppConfig {
   optional double ibvs_mu = 13;
   optional double ibvs_qdot_max = 14;
   TouchScreenTwist6 ibvs_vmax6 = 15;
+  TouchScreenTwist6 ibvs_amax6 = 57;
+  optional double ibvs_twist_filter_alpha = 58;
   optional bool enable_joint_limit_avoidance = 16;
   optional double joint_limit_avoidance_gain = 17;
   optional double joint_limit_avoidance_margin_ratio = 18;