feat : add ibvs acc limit

cmvr-es/applications/include/touch_screen_app.h

@@ -116,10 +116,11 @@ 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]`。
+        // 相机 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}};
+        // 分量小于等于 0 表示该维度不启用加速度限幅。
-        // 相机 twist 一阶低通滤波系数，范围 [0, 1]。
+        std::array<double, 6> ibvs_amax6{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
-        double ibvs_twist_filter_alpha{0.35};
+        // 相机 twist 一阶低通滤波系数；取值在 (0, 1) 时启用低通，默认 1.0 表示不过滤。
+        double ibvs_twist_filter_alpha{1.0};
         // 是否启用关节限位回避。
         bool enable_joint_limit_avoidance{true};
         // 关节限位回避增益。

cmvr-es/applications/src/touch_screen_app.cpp

@@ -1171,7 +1171,7 @@ bool TouchScreenApp::stepAligning(const double dt) {
     }
 
     std::vector<double> qdot_cmd;
-    if (!ibvs_.compute(q_now, ibvs_dt, qdot_cmd)) {
+    if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) {
         switch (ibvs_.lastComputeStatus()) {
             case IbvsController::ComputeStatus::NO_NEW_FRAME:
             case IbvsController::ComputeStatus::NO_TAG:
@@ -1226,6 +1226,14 @@ bool TouchScreenApp::stepAligning(const double dt) {
     }
 
     if (align_stable_count_ >= options_.align_stable_frames) {
+        std::cout << "[TouchScreenApp][ALIGN_REACHED] tag_id=" << tag_id
+                  << ", err_xyz=[" << last_align_error_camera_.x() << ", "
+                  << last_align_error_camera_.y() << ", "
+                  << last_align_error_camera_.z() << "]"
+                  << ", err_rxyz=[" << rot_error_vec.x() << ", "
+                  << rot_error_vec.y() << ", "
+                  << rot_error_vec.z() << "]"
+                  << std::endl;
         hardStopIbvsMotion();
         phase_ = Phase::ALIGN_REACHED;
         phase_start_time_ = Clock::now();

cmvr-es/common/config/cabin_robot.xml

@@ -58,7 +58,7 @@
                             <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
                             <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
                         </RightArmCan>
-                        <HeadCan id = " " devId = " " channelId ="2" enable="true">
+                        <HeadCan id = " " devId = " " channelId ="2" enable="false">
                             <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>

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

@@ -53,26 +53,26 @@ target_ry: 0.0
 target_rz: 0.0
 align_mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
 
-ibvs_lambda: 0.3
+ibvs_lambda: 0.4
 ibvs_mu: 0.1
-ibvs_qdot_max: 0.35
+ibvs_qdot_max: 1.0
 ibvs_vmax6 {
-  vx: 0.55
+  vx: 1.0
-  vy: 0.55
+  vy: 1.0
-  vz: 0.55
+  vz: 1.0
   wx: 0.6
   wy: 0.6
   wz: 0.6
 }
 ibvs_amax6 {
-  vx: 0.4
+  vx: 2.4
-  vy: 0.4
+  vy: 2.4
-  vz: 0.5
+  vz: 4.5
-  wx: 1.5
+  wx: 2.5
-  wy: 1.5
+  wy: 2.5
-  wz: 1.5
+  wz: 2.5
 }
-ibvs_twist_filter_alpha: 0.35
+ibvs_twist_filter_alpha: 1.0
 enable_joint_limit_avoidance: true
 joint_limit_avoidance_gain: 0.2
 joint_limit_avoidance_margin_ratio: 0.15
@@ -98,15 +98,15 @@ control_joint_names: "R_WRIST_Y"
 control_joint_names: "R_WRIST_R"
 
 align_error_threshold6 {
-  x: 0.006
+  x: 0.005
-  y: 0.006
+  y: 0.005
-  z: 0.006
+  z: 0.01
   rx: 0.1026646259971647
   ry: 0.1026646259971647
   rz: 0.1026646259971647
 }
 align_stable_frames: 2
-align_timeout_s: 30.0
+align_timeout_s: 20.0
 pause_after_align_reached: false
 
 touch_twist_base {

cmvr-es/controller/include/ibvs_controller.h

@@ -105,16 +105,15 @@ public:
                  std::vector<double>& q_cmd_out);
 
     /**
-     * @brief 根据当前关节状态计算关节速度命令。
+     * @brief 根据当前关节状态和控制周期计算关节速度命令。
      * @param joints_angle 当前关节角。
+     * @param dt 控制周期，单位秒。
      * @param qdot_out 输出的关节速度命令。
      * @return 计算成功返回 `true`。
-     *
-     * @note 不带 dt 的接口内部会使用一个保守默认周期来执行 twist 限加速度，
-     *       更推荐使用带 dt 的 compute 接口。
      */
-    bool compute(const std::vector<double>& joints_angle,
+    bool computeQdot(const std::vector<double>& joints_angle,
-                 std::vector<double>& qdot_out);
+                     double dt,
+                     std::vector<double>& qdot_out);
 
     /**
      * @brief 设置 ViSP 视觉伺服增益。
@@ -195,13 +194,15 @@ public:
 
     /**
      * @brief 设置相机 twist 六维加速度限幅。
-     * @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度。
+     * @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度；
+     *              分量小于等于 0 时表示该维度不启用加速度限幅。
      */
     void setAccelerationLimit6(const std::array<double, 6>& amax6);
 
     /**
      * @brief 设置相机 twist 一阶低通滤波系数。
-     * @param alpha 滤波系数，范围 [0, 1]；0 表示不启用，1 表示不过滤。
+     * @param alpha 滤波系数，范围 [0, 1]；取值在 (0, 1) 时启用一阶低通，
+     *              其余情况按不过滤处理。
      */
     void setTwistFilterAlpha(double alpha);
 
@@ -313,11 +314,11 @@ private:
 
     std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
 
-    // 新增：相机 twist 六维加速度限幅
+    // 相机 twist 六维加速度限幅；<=0 表示该维度默认不启用。
-    std::array<double, 6> amax6_{{0.4, 0.4, 0.5, 1.5, 1.5, 1.5}};
+    std::array<double, 6> amax6_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
 
-    // 新增：相机 twist 一阶低通滤波系数
+    // 相机 twist 一阶低通滤波系数；默认 1.0 表示不过滤。
-    double twist_lpf_alpha_{0.35};
+    double twist_lpf_alpha_{1.0};
 
     bool limit_avoidance_enabled_{false};
     double limit_avoidance_gain_{0.2};

cmvr-es/controller/src/ibvs_controller.cpp

@@ -14,6 +14,65 @@
 
 namespace cmvr {
 
+namespace {
+
+Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
+    const Eigen::AngleAxisd aa(R);
+    if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) {
+        return Eigen::Vector3d::Zero();
+    }
+    return aa.axis() * aa.angle();
+}
+
+Eigen::Matrix<double, 6, 1> computeAdaptiveAccelScale(const double tag_size_m,
+                                                      const vpHomogeneousMatrix& cMo,
+                                                      const double target_x,
+                                                      const double target_y,
+                                                      const double target_z,
+                                                      const double target_rx,
+                                                      const double target_ry,
+                                                      const double target_rz) {
+    Eigen::Matrix<double, 6, 1> scale = Eigen::Matrix<double, 6, 1>::Ones();
+
+    Eigen::Vector3d pos_err;
+    pos_err << cMo[0][3] - target_x, cMo[1][3] - target_y, cMo[2][3] - target_z;
+
+    vpRotationMatrix R_des_visp;
+    R_des_visp.buildFrom(target_rx, target_ry, target_rz);
+    Eigen::Matrix3d R_des = Eigen::Matrix3d::Identity();
+    Eigen::Matrix3d R_cur = Eigen::Matrix3d::Identity();
+    for (int r = 0; r < 3; ++r) {
+        for (int c = 0; c < 3; ++c) {
+            R_des(r, c) = R_des_visp[r][c];
+            R_cur(r, c) = cMo[r][c];
+        }
+    }
+    const Eigen::Vector3d rot_err = rotvecFromRotationMatrix(R_des.transpose() * R_cur);
+
+    const double xy_ref = std::max(0.5 * tag_size_m, 0.01);
+    const double z_ref = std::max(tag_size_m, 0.03);
+    const double rot_ref = 0.20; // about 11.5 deg
+    constexpr double kMinScale = 0.15;
+
+    const auto axisScale = [&](const double err_abs, const double ref) {
+        if (!std::isfinite(err_abs) || !std::isfinite(ref) || ref <= 1e-9) {
+            return 1.0;
+        }
+        const double ratio = std::clamp(err_abs / ref, 0.0, 1.0);
+        return kMinScale + (1.0 - kMinScale) * ratio;
+    };
+
+    scale[0] = axisScale(std::abs(pos_err.x()), xy_ref);
+    scale[1] = axisScale(std::abs(pos_err.y()), xy_ref);
+    scale[2] = axisScale(std::abs(pos_err.z()), z_ref);
+    scale[3] = axisScale(std::abs(rot_err.x()), rot_ref);
+    scale[4] = axisScale(std::abs(rot_err.y()), rot_ref);
+    scale[5] = axisScale(std::abs(rot_err.z()), rot_ref);
+    return scale;
+}
+
+} // namespace
+
 const char* IbvsController::statusToString(ComputeStatus status) {
     switch (status) {
         case ComputeStatus::OK: return "ok";
@@ -139,10 +198,14 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
     return true;
 }
 
-bool IbvsController::compute(const std::vector<double>& joints_angle,
+bool IbvsController::computeQdot(const std::vector<double>& joints_angle,
-                             std::vector<double>& qdot_out) {
+                                 double dt,
-    constexpr double kDefaultDt = 0.02; // 50 Hz
+                                 std::vector<double>& qdot_out) {
-    return computeInternal(joints_angle, kDefaultDt, qdot_out);
+    if (dt <= 0.0 || !std::isfinite(dt)) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+    return computeInternal(joints_angle, dt, qdot_out);
 }
 
 bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
@@ -275,11 +338,32 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
         resetTwistCommandState();
     }
 
-    // 2) 加速度限幅
+    // 2) 加速度限幅：误差大时放宽，误差小时收紧；减速/刹车保持满加速度
     Eigen::Matrix<double, 6, 1> v_acc_limited = v_camera_cmd_prev_;
+    const Eigen::Matrix<double, 6, 1> accel_scale =
+        computeAdaptiveAccelScale(tag_size_m_,
+                                  cMo,
+                                  target_x_,
+                                  target_y_,
+                                  target_z_,
+                                  target_rx_,
+                                  target_ry_,
+                                  target_rz_);
     for (int i = 0; i < 6; ++i) {
-        const double amax = std::max(0.0, amax6_[i]);
+        const double amax = amax6_[i];
-        const double dv_max = amax * dt;
+        if (!std::isfinite(amax) || amax <= 0.0) {
+            v_acc_limited[i] = v_raw[i];
+            continue;
+        }
+
+        double amax_eff = amax;
+        const bool same_direction = v_raw[i] * v_camera_cmd_prev_[i] >= 0.0;
+        const bool speeding_up = std::abs(v_raw[i]) > std::abs(v_camera_cmd_prev_[i]) + 1e-12;
+        if (same_direction && speeding_up) {
+            amax_eff *= accel_scale[i];
+        }
+
+        const double dv_max = amax_eff * 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;

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

@@ -627,7 +627,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
         }
 
         std::vector<double> qdot_next;
-        const bool ok = ibvs_controller.compute(q_now, qdot_next);
+        const bool ok = ibvs_controller.computeQdot(q_now, static_cast<double>(cycle_ms) / 1000.0, qdot_next);
         if (!ok) {
             ++compute_fail_steps;
             ++fail_steps;
@@ -936,7 +936,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
         }
 
         std::vector<double> qdot_next;
-        const bool ok = ibvs_controller.compute(q_now, qdot_next);
+        const bool ok = ibvs_controller.computeQdot(q_now, static_cast<double>(cycle_ms) / 1000.0, qdot_next);
         if (!ok) {
             ++compute_fail_steps;
             ++fail_steps;

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

@@ -93,8 +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();
+        constexpr double kControlDt = 0.01;
-        bool first_step = true;
+        auto next_step_time = std::chrono::steady_clock::now();
         while (touch_app_.isBusy()) {
             if (context != nullptr && context->IsCancelled()) {
                 touch_app_.stop();
@@ -103,16 +103,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
                 return grpc::Status(grpc::StatusCode::CANCELLED, error);
             }
 
-            const auto now = std::chrono::steady_clock::now();
+            if (!touch_app_.step(kControlDt)) {
-            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"));
             }
@@ -136,7 +127,13 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
                 last_logged_status = touch_app_.lastStatus();
             }
 
-            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            next_step_time += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+                std::chrono::duration<double>(kControlDt));
+            const auto after_step = std::chrono::steady_clock::now();
+            if (next_step_time < after_step) {
+                next_step_time = after_step;
+            }
+            std::this_thread::sleep_until(next_step_time);
         }
 
         if (!align_reached) {