From 720d851cd0327cd08dd0d29dd34a76604e50b35c Mon Sep 17 00:00:00 2001 From: lgv Date: Sat, 28 Feb 2026 16:29:16 +0800 Subject: [PATCH] feat: add null space aviod --- cmvr-es/controller/include/ibvs_controller.h | 17 ++++++ cmvr-es/controller/src/controller_test.cpp | 5 +- cmvr-es/controller/src/ibvs_controller.cpp | 30 +++++++++-- .../src/humanoid_robot_test.cpp | 3 +- .../include/pinocchio_dls_ik_solver.h | 19 +++++++ .../ik_solver/src/pinocchio_dls_ik_solver.cpp | 54 +++++++++++++++++++ model/xiaoyan_description/dual_arm.xml | 2 +- 7 files changed, 121 insertions(+), 9 deletions(-) diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index d4a97fa7..ca92a5b8 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -161,6 +161,18 @@ public: * @param vmax6 线速度/角速度六维限幅。 */ void setVelocityLimit6(const std::array& vmax6); + /** + * @brief 设置关节限位避障 null-space 项参数。 + * + * @param enable 是否启用。 + * @param gain 避障增益（rad/s 量纲）。 + * @param margin_ratio 上下限触发边界占关节行程比例 (0, 0.5)。 + * @param max_push 每关节最大推回速度（rad/s），<=0 表示不额外限幅。 + */ + void setJointLimitAvoidance(bool enable, + double gain = 0.2, + double margin_ratio = 0.05, + double max_push = 0.25); /** * @brief 设置 AbstractCamera 坐标系到 ViSP 坐标系旋转。 * @param R_cv 旋转矩阵。 @@ -280,6 +292,11 @@ private: double depth_z_kp_{1.0}; // 相机 twist 六维限幅。 std::array vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; + // 关节限位避障 null-space 参数。 + 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 平面中用于采样深度的控制点。 Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()}; diff --git a/cmvr-es/controller/src/controller_test.cpp b/cmvr-es/controller/src/controller_test.cpp index aeb9686f..247d9e93 100644 --- a/cmvr-es/controller/src/controller_test.cpp +++ b/cmvr-es/controller/src/controller_test.cpp @@ -531,7 +531,7 @@ protected: // MuJoCo 渲染线程与控制线程不同步：允许复用最近一帧，避免长时间 no_new_frame。 mujoco_camera_->setConsumeNewFrameOnly(false); - q_home_ = {0.25, 1.00, M_PI / 2 - 0.2, M_PI / 2 - 0.2, -M_PI + 0.5, 0.0, 0.0}; + q_home_ = {-0.2423, 1.2929,1.61, 1.58, -2.8792, 0.1150,-0.08}; ibvs_controller_ = std::make_unique(); ibvs_controller_->setMu(mu_); @@ -540,7 +540,8 @@ protected: ibvs_controller_->setDepthZGain(1.0); ibvs_controller_->setVelocityLimit6(vmax6_); ibvs_controller_->setTrackedTagId(tracked_tag_id_); - ibvs_controller_->setTarget(0.0,0.0,0.35); + ibvs_controller_->setTarget(0.0,0.0,0.4); + ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25); const Eigen::Matrix3d R_align = (Eigen::Matrix3d() << 1, 0, 0, diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index 40ef367f..bc464e6c 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/controller/src/ibvs_controller.cpp @@ -83,6 +83,10 @@ bool IbvsController::init(const std::shared_ptr& camera, initialized_ = dls_solver_->init(); if (initialized_) { + dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_, + limit_avoidance_gain_, + limit_avoidance_margin_ratio_, + limit_avoidance_max_push_); has_joint_position_limits_ = dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_); } else { @@ -340,11 +344,11 @@ bool IbvsController::compute(const std::vector& joints_angle, } - std::cout << "b_cmd: " ; - for (const auto& it : q_cmd_) { - std::cout << it << " "; - } - std::cout << std::endl; + // std::cout << "b_cmd: " ; + // for (const auto& it : q_cmd_) { + // std::cout << it << " "; + // } + // std::cout << std::endl; clampJointCommandInPlace(q_cmd_); q_cmd_out = q_cmd_; @@ -420,6 +424,22 @@ void IbvsController::setVelocityLimit6(const std::array& vmax6) { vmax6_ = vmax6; } +void IbvsController::setJointLimitAvoidance(bool enable, + double gain, + double margin_ratio, + double max_push) { + limit_avoidance_enabled_ = enable; + limit_avoidance_gain_ = std::max(0.0, gain); + limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); + limit_avoidance_max_push_ = max_push; + if (dls_solver_) { + dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_, + limit_avoidance_gain_, + limit_avoidance_margin_ratio_, + limit_avoidance_max_push_); + } +} + void IbvsController::setAlignCameraToVisp(const Eigen::Matrix3d& R_cv) { R_cv_ = R_cv; } 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 b4914c69..65b99282 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 @@ -201,6 +201,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { ibvs_controller.setMu(0.1); ibvs_controller.setLambda(0.6); ibvs_controller.setQdotMax(0.6); + ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); ibvs_controller.setTagSize(0.12); ibvs_controller.setTrackedTagId(0); ibvs_controller.setTarget(0.0, 0.0, 0.40); @@ -221,6 +222,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { // ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0}); // ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005}); ibvs_controller.setQdotMax(0.15); + ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); @@ -321,4 +323,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { - diff --git a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h index 7108b4aa..19d3d133 100644 --- a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h +++ b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h @@ -78,6 +78,19 @@ public: double damping = -1.0, double qdot_abs_max = std::numeric_limits::infinity()); + /** + * @brief 配置关节限位避障 null-space 项。 + * + * @param enable 是否启用。 + * @param gain 避障增益（rad/s 量纲）。 + * @param margin_ratio 在上下限附近触发的边界带占总行程比例 (0, 0.5)。 + * @param max_push 每关节最大推回速度（rad/s），<=0 表示不额外限幅。 + */ + void setJointLimitAvoidance(bool enable, + double gain = 0.2, + double margin_ratio = 0.15, + double max_push = 0.25); + /** * @brief 读取本链关节位置限位（来自 URDF）。 * @param lower 输出下限，size=chain_dof_。 @@ -122,6 +135,12 @@ private: Eigen::VectorXd q_lower_chain_; Eigen::VectorXd q_upper_chain_; + // null-space 关节限位避障参数 + 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}; + // base pose cache (PELVIS_S fixed) bool base_pose_cached_{false}; pinocchio::SE3 oM_base_cached_; diff --git a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp index 2f2f7a37..96d1fcb8 100644 --- a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp +++ b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp @@ -289,6 +289,16 @@ bool PinocchioDlsIKSolver::fk(const std::string& base_link, return true; } +void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable, + double gain, + double margin_ratio, + double max_push) { + limit_avoidance_enabled_ = enable; + limit_avoidance_gain_ = std::max(0.0, gain); + limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); + limit_avoidance_max_push_ = max_push; +} + bool PinocchioDlsIKSolver::velocityIk(const std::vector& cur_angle, const Eigen::Matrix& ee_velocity, std::vector& joints_vel, @@ -359,6 +369,50 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector& cur_angle, A.diagonal().array() += (lambda * lambda); Eigen::VectorXd qdot = J.transpose() * A.ldlt().solve(twist_base); + // 6.1) null-space 关节限位避障：在主任务零空间叠加“离限位推回”速度。 + if (limit_avoidance_enabled_ && + limit_avoidance_gain_ > 0.0 && + chain_v_dof_ == chain_dof_ && + q_lower_chain_.size() == chain_dof_ && + q_upper_chain_.size() == chain_dof_) { + const Eigen::VectorXd q_chain = q_full.segment(chain_q_start_, chain_dof_); + Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(chain_v_dof_); + + for (int i = 0; i < chain_dof_; ++i) { + const double lo = q_lower_chain_[i]; + const double hi = q_upper_chain_[i]; + if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) { + continue; + } + + const double span = hi - lo; + const double margin = std::max(1e-4, limit_avoidance_margin_ratio_ * span); + double push = 0.0; + + if (q_chain[i] < lo + margin) { + const double s = (lo + margin - q_chain[i]) / margin; // 0..1+ + push += limit_avoidance_gain_ * s * s; + } else if (q_chain[i] > hi - margin) { + const double s = (q_chain[i] - (hi - margin)) / margin; // 0..1+ + push -= limit_avoidance_gain_ * s * s; + } + + if (limit_avoidance_max_push_ > 0.0) { + push = clampd(push, -limit_avoidance_max_push_, limit_avoidance_max_push_); + } + qdot_avoid[i] = push; + } + + if (qdot_avoid.squaredNorm() > 1e-16) { + const Eigen::Matrix A_inv = + A.ldlt().solve(Eigen::Matrix::Identity()); + const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; // n x 6 + const Eigen::MatrixXd N = + Eigen::MatrixXd::Identity(chain_v_dof_, chain_v_dof_) - J_pinv * J; + qdot += N * qdot_avoid; + } + } + // 7) 关节速度限幅：取调用者限幅与 URDF velocityLimit 的更严格值。 joints_vel.resize(chain_v_dof_); for (int i = 0; i < chain_v_dof_; ++i) { diff --git a/model/xiaoyan_description/dual_arm.xml b/model/xiaoyan_description/dual_arm.xml index 79a2c3b0..12ec88e5 100644 --- a/model/xiaoyan_description/dual_arm.xml +++ b/model/xiaoyan_description/dual_arm.xml @@ -66,7 +66,7 @@ - +