feat:add ibvs joint pos limit

cmvr-es/controller/include/ibvs_controller.h

@@ -229,6 +229,11 @@ private:
      */
     bool computeInternal(const std::vector<double>& joints_angle,
                          std::vector<double>& qdot_out);
+    /**
+     * @brief 按 URDF 关节上下限对关节命令做硬裁剪。
+     * @param q 关节命令，函数内原地修改。
+     */
+    void clampJointCommandInPlace(std::vector<double>& q) const;
     /**
      * @brief 根据当前目标位姿反解 tag 平面的深度控制点。
      */
@@ -300,6 +305,12 @@ private:
 
     // 速度 IK 求解器。
     std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
+    // 是否成功读取到 URDF 关节限位。
+    bool has_joint_position_limits_{false};
+    // 本链关节位置下限（rad）。
+    std::vector<double> q_lower_limits_;
+    // 本链关节位置上限（rad）。
+    std::vector<double> q_upper_limits_;
     // 最近一次用于控制的 tag id。
     int last_used_tag_id_{-1};
 

cmvr-es/controller/src/ibvs_controller.cpp

@@ -82,6 +82,14 @@ bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& 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<double>& 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<double>& joints_angle,
     }
     if (q_cmd_.size() != joints_angle.size()) {
         q_cmd_ = joints_angle;
+        clampJointCommandInPlace(q_cmd_);
     }
 
     std::vector<double> qdot;
@@ -328,6 +338,7 @@ bool IbvsController::compute(const std::vector<double>& 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<double>& joints_angle,
     return computeInternal(joints_angle, qdot_out);
 }
 
+void IbvsController::clampJointCommandInPlace(std::vector<double>& 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();

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<double>::infinity());
 
+    /**
+     * @brief 读取本链关节位置限位（来自 URDF）。
+     * @param lower 输出下限，size=chain_dof_。
+     * @param upper 输出上限，size=chain_dof_。
+     * @return 成功返回 `true`。
+     */
+    bool getJointPositionLimits(std::vector<double>& lower,
+                                std::vector<double>& 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; }

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<double>& cur_angle,
     return true;
 }
 
+bool PinocchioDlsIKSolver::getJointPositionLimits(std::vector<double>& lower,
+                                                  std::vector<double>& 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 <Eigen/SVD>
 #include <cmath>
 #include <iostream>