From 2807fd158fba43ef74774a691743b415f239f9ce Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 3 Apr 2026 09:21:57 +0800 Subject: [PATCH] feat : add ibvs acc limit --- .../applications/include/touch_screen_app.h | 9 +- cmvr-es/applications/src/touch_screen_app.cpp | 10 +- cmvr-es/common/config/cabin_robot.xml | 2 +- .../touch_screen_app_config.pb.txt | 32 +++--- cmvr-es/controller/include/ibvs_controller.h | 25 ++--- cmvr-es/controller/src/ibvs_controller.cpp | 98 +++++++++++++++++-- .../src/humanoid_robot_test.cpp | 4 +- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 23 ++--- 8 files changed, 147 insertions(+), 56 deletions(-) diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index f905d824..840b8738 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/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 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}; + // 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`； + // 分量小于等于 0 表示该维度不启用加速度限幅。 + std::array ibvs_amax6{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + // 相机 twist 一阶低通滤波系数；取值在 (0, 1) 时启用低通，默认 1.0 表示不过滤。 + double ibvs_twist_filter_alpha{1.0}; // 是否启用关节限位回避。 bool enable_joint_limit_avoidance{true}; // 关节限位回避增益。 diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index 98aaa287..310787f9 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -1171,7 +1171,7 @@ bool TouchScreenApp::stepAligning(const double dt) { } std::vector 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(); diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index 25928296..d67dffac 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -58,7 +58,7 @@ - + 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 0aeab8ef..512ea159 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 @@ -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 - vy: 0.55 - vz: 0.55 + vx: 1.0 + vy: 1.0 + vz: 1.0 wx: 0.6 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 + vx: 2.4 + vy: 2.4 + vz: 4.5 + wx: 2.5 + wy: 2.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 - y: 0.006 - z: 0.006 + x: 0.005 + y: 0.005 + 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 { diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index a621daf9..d8afb684 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -105,16 +105,15 @@ public: std::vector& 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& joints_angle, - std::vector& qdot_out); + bool computeQdot(const std::vector& joints_angle, + double dt, + std::vector& qdot_out); /** * @brief 设置 ViSP 视觉伺服增益。 @@ -195,13 +194,15 @@ public: /** * @brief 设置相机 twist 六维加速度限幅。 - * @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度。 + * @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度； + * 分量小于等于 0 时表示该维度不启用加速度限幅。 */ void setAccelerationLimit6(const std::array& 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 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 六维加速度限幅；<=0 表示该维度默认不启用。 + std::array amax6_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - // 新增：相机 twist 一阶低通滤波系数 - double twist_lpf_alpha_{0.35}; + // 相机 twist 一阶低通滤波系数；默认 1.0 表示不过滤。 + double twist_lpf_alpha_{1.0}; bool limit_avoidance_enabled_{false}; double limit_avoidance_gain_{0.2}; diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index ef449949..e09773a7 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/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 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 scale = Eigen::Matrix::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& joints_angle, return true; } -bool IbvsController::compute(const std::vector& joints_angle, - std::vector& qdot_out) { - constexpr double kDefaultDt = 0.02; // 50 Hz - return computeInternal(joints_angle, kDefaultDt, qdot_out); +bool IbvsController::computeQdot(const std::vector& joints_angle, + double dt, + std::vector& 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& joint_names) const { @@ -275,11 +338,32 @@ bool IbvsController::computeInternal(const std::vector& joints_angle, resetTwistCommandState(); } - // 2) 加速度限幅 + // 2) 加速度限幅：误差大时放宽，误差小时收紧；减速/刹车保持满加速度 Eigen::Matrix v_acc_limited = v_camera_cmd_prev_; + const Eigen::Matrix 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 dv_max = amax * dt; + const double amax = amax6_[i]; + 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; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index a74c59e6..d1b47e4e 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -627,7 +627,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { } std::vector qdot_next; - const bool ok = ibvs_controller.compute(q_now, qdot_next); + const bool ok = ibvs_controller.computeQdot(q_now, static_cast(cycle_ms) / 1000.0, qdot_next); if (!ok) { ++compute_fail_steps; ++fail_steps; @@ -936,7 +936,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) { } std::vector qdot_next; - const bool ok = ibvs_controller.compute(q_now, qdot_next); + const bool ok = ibvs_controller.computeQdot(q_now, static_cast(cycle_ms) / 1000.0, qdot_next); if (!ok) { ++compute_fail_steps; ++fail_steps; diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index 280e1fbd..5adbe90e 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/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(); - bool first_step = true; + constexpr double kControlDt = 0.01; + 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(); - 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)) { + if (!touch_app_.step(kControlDt)) { 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::duration(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) {