diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index 904cdfb2..656c7857 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -229,6 +229,11 @@ private: */ bool computeInternal(const std::vector& joints_angle, std::vector& qdot_out); + /** + * @brief 按 URDF 关节上下限对关节命令做硬裁剪。 + * @param q 关节命令，函数内原地修改。 + */ + void clampJointCommandInPlace(std::vector& q) const; /** * @brief 根据当前目标位姿反解 tag 平面的深度控制点。 */ @@ -300,6 +305,12 @@ private: // 速度 IK 求解器。 std::unique_ptr dls_solver_{nullptr}; + // 是否成功读取到 URDF 关节限位。 + bool has_joint_position_limits_{false}; + // 本链关节位置下限（rad）。 + std::vector q_lower_limits_; + // 本链关节位置上限（rad）。 + std::vector q_upper_limits_; // 最近一次用于控制的 tag id。 int last_used_tag_id_{-1}; diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index c378f198..f3551d53 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/controller/src/ibvs_controller.cpp @@ -82,6 +82,14 @@ bool IbvsController::init(const std::shared_ptr& camera, urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_); initialized_ = dls_solver_->init(); + if (initialized_) { + has_joint_position_limits_ = + dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_); + } else { + has_joint_position_limits_ = false; + q_lower_limits_.clear(); + q_upper_limits_.clear(); + } last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY; return initialized_; } @@ -91,6 +99,7 @@ void IbvsController::reset(const std::vector& q_init) { q_cmd_.clear(); } else { q_cmd_ = q_init; + clampJointCommandInPlace(q_cmd_); } last_tag_detected_ = false; last_used_tag_id_ = -1; @@ -316,6 +325,7 @@ bool IbvsController::compute(const std::vector& joints_angle, } if (q_cmd_.size() != joints_angle.size()) { q_cmd_ = joints_angle; + clampJointCommandInPlace(q_cmd_); } std::vector qdot; @@ -328,6 +338,7 @@ bool IbvsController::compute(const std::vector& joints_angle, // q_{k+1} = q_k + qdot * dt q_cmd_[i] += qdot[i] * dt; } + clampJointCommandInPlace(q_cmd_); q_cmd_out = q_cmd_; return true; @@ -338,6 +349,19 @@ bool IbvsController::compute(const std::vector& joints_angle, return computeInternal(joints_angle, qdot_out); } +void IbvsController::clampJointCommandInPlace(std::vector& q) const { + if (!has_joint_position_limits_) return; + if (q.size() != q_lower_limits_.size() || q.size() != q_upper_limits_.size()) return; + + for (size_t i = 0; i < q.size(); ++i) { + const double lo = q_lower_limits_[i]; + const double hi = q_upper_limits_[i]; + if (!std::isfinite(lo) && !std::isfinite(hi)) continue; + if (lo > hi) continue; + q[i] = SupportFunctions::clamp(q[i], lo, hi); + } +} + void IbvsController::setLambda(double lambda) { lambda_ = lambda; initTask(); 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 dafbeb4e..7108b4aa 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,15 @@ public: double damping = -1.0, double qdot_abs_max = std::numeric_limits::infinity()); + /** + * @brief 读取本链关节位置限位（来自 URDF）。 + * @param lower 输出下限，size=chain_dof_。 + * @param upper 输出上限，size=chain_dof_。 + * @return 成功返回 `true`。 + */ + bool getJointPositionLimits(std::vector& lower, + std::vector& upper) const; + void setMaxIters(int iters) { max_iters_ = iters; } void setDamping(double d) { damping_ = d; } void setEps(double pos_eps, double rot_eps) { pos_eps_ = pos_eps; rot_eps_ = rot_eps; } 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 daf51972..2f2f7a37 100644 --- a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp +++ b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp @@ -161,6 +161,25 @@ bool PinocchioDlsIKSolver::init() { << "': q_start=" << chain_q_start_ << " q_dof=" << chain_dof_ << ", v_start=" << chain_v_start_ << " v_dof=" << chain_v_dof_ << ", nq=" << model_.nq << " nv=" << model_.nv << "\n"; + std::cout << "[PinocchioDlsIKSolver] Chain joint position limits (rad):\n"; + for (const auto j : chain_joints) { + const int q_idx = model_.idx_qs[j]; + const int nq = model_.nqs[j]; + const std::string& jname = model_.names[j]; + if (nq <= 0) continue; + + for (int k = 0; k < nq; ++k) { + const int qi = q_idx - chain_q_start_ + k; + if (qi < 0 || qi >= chain_dof_) continue; + if (nq == 1) { + std::cout << " - " << jname + << ": [" << q_lower_chain_[qi] << ", " << q_upper_chain_[qi] << "]\n"; + } else { + std::cout << " - " << jname << "[" << k << "]" + << ": [" << q_lower_chain_[qi] << ", " << q_upper_chain_[qi] << "]\n"; + } + } + } std::cout << "[PinocchioDlsIKSolver] base pose cached (constant)\n"; return true; } @@ -361,6 +380,20 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector& cur_angle, return true; } +bool PinocchioDlsIKSolver::getJointPositionLimits(std::vector& lower, + std::vector& upper) const { + if (!initialized_ || chain_dof_ <= 0) { + return false; + } + if (q_lower_chain_.size() != chain_dof_ || q_upper_chain_.size() != chain_dof_) { + return false; + } + + lower.assign(q_lower_chain_.data(), q_lower_chain_.data() + q_lower_chain_.size()); + upper.assign(q_upper_chain_.data(), q_upper_chain_.data() + q_upper_chain_.size()); + return true; +} + #include #include #include