From 7148e1baf202dcd21c0dfb73a6b4ea49289d7d98 Mon Sep 17 00:00:00 2001 From: lgv Date: Mon, 30 Mar 2026 13:39:14 +0800 Subject: [PATCH] feat:add ibvs acc limit --- .../applications/include/touch_screen_app.h | 9 +- cmvr-es/applications/src/touch_screen_app.cpp | 49 ++++++-- .../src/touch_screen_app_test.cpp | 14 ++- .../touch_screen_app_config.pb.txt | 9 ++ cmvr-es/controller/include/ibvs_controller.h | 117 +++++------------- cmvr-es/controller/src/ibvs_controller.cpp | 84 ++++++++++--- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 14 ++- .../touch_screen_app_config.proto | 2 + 8 files changed, 180 insertions(+), 118 deletions(-) diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index cab6abe8..f905d824 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/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 ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; + // 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`。 + std::array 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& q_out) const; bool sendJointVelocity(const std::vector& qdot) const; bool sendZeroJointVelocity() const; + void hardStopIbvsMotion(); bool holdCurrentControlledPosition() const; bool moveToInitPositionIfEnabled() const; bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index ac634e97..98aaa287 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/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(i)] = ibvs_vmax[i]; } } + if (config.has_ibvs_amax6()) { + Eigen::Matrix 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(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 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(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 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; diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp index 6c6ae3de..014beaa6 100644 --- a/cmvr-es/applications/src/touch_screen_app_test.cpp +++ b/cmvr-es/applications/src/touch_screen_app_test.cpp @@ -1,5 +1,6 @@ #include "gtest/gtest.h" +#include #include #include #include @@ -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(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()) diff --git a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt index f4f7c78d..0aeab8ef 100644 --- a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt +++ b/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 diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index ce841b02..a621daf9 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/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& joints_angle, std::vector& qdot_out); @@ -190,6 +193,25 @@ public: */ void setVelocityLimit6(const std::array& vmax6); + /** + * @brief 设置相机 twist 六维加速度限幅。 + * @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度。 + */ + void setAccelerationLimit6(const std::array& 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& 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& joints_angle, + double dt, std::vector& 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 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 vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; - // 是否启用关节限位回避。 + // 新增：相机 twist 六维加速度限幅 + std::array 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 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 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 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 last_v_camera_visp_{Eigen::Matrix::Zero()}; + + // 新增：上一拍实际发送给 IK 的相机 twist（ViSP 相机系） + Eigen::Matrix v_camera_cmd_prev_{Eigen::Matrix::Zero()}; + bool has_v_camera_cmd_prev_{false}; }; } // namespace cmvr diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index e6ed12f6..ef449949 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/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& 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& q_init) { last_depth_usage_ = DepthUsage::NONE; last_tag_pos_visp_.setZero(); last_v_camera_visp_.setZero(); + resetTwistCommandState(); } bool IbvsController::compute(const std::vector& joints_angle, @@ -128,7 +126,7 @@ bool IbvsController::compute(const std::vector& joints_angle, } std::vector qdot; - if (!computeInternal(joints_angle, qdot)) { + if (!computeInternal(joints_angle, dt, qdot)) { return false; } @@ -143,7 +141,8 @@ bool IbvsController::compute(const std::vector& joints_angle, bool IbvsController::compute(const std::vector& joints_angle, std::vector& 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& joint_names) const { @@ -155,6 +154,7 @@ bool IbvsController::getChainJointNames(std::vector& joint_names) c } bool IbvsController::computeInternal(const std::vector& joints_angle, + double dt, std::vector& qdot_out) { last_depth_usage_ = DepthUsage::NONE; last_tag_detected_ = false; @@ -170,6 +170,10 @@ bool IbvsController::computeInternal(const std::vector& 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& 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& 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(intr.fx); const double fy = static_cast(intr.fy); @@ -250,7 +251,7 @@ bool IbvsController::computeInternal(const std::vector& 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& joints_angle, v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_); } - // 限幅+缓存（ViSP camera系） + // ---------- 视觉速度整形：速度限幅 + 加速度限幅 + 一阶低通 ---------- + Eigen::Matrix 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 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, - const Eigen::Vector3d& p_c_target_des, - double rx, - double ry, - double rz) { +bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target, + const Eigen::Vector3d& p_c_target_des, + double rx, + double ry, + 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& vmax6) { vmax6_ = vmax6; } +void IbvsController::setAccelerationLimit6(const std::array& 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_); } diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index 56f4b072..280e1fbd 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -5,6 +5,7 @@ #include "../include/grpc_hlc_service.h" +#include #include #include #include @@ -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(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")); } diff --git a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto index 862728e5..245d1346 100644 --- a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto +++ b/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;