feat: add ibvs class and test pass

cmvr-es/common/utils/image/image_process.h

@@ -0,0 +1,60 @@
+//
+// Created by lgv on 2026/2/26.
+//
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+#include <opencv2/core.hpp>
+
+namespace cmvr {
+
+// 在像素 (u,v) 的邻域内采样深度并取中值，输出单位米。
+// 支持:
+// - CV_32FC1: 米
+// - CV_16UC1: 毫米（内部转换为米）
+inline bool sampleDepthMeters(const cv::Mat& depth,
+                              int u,
+                              int v,
+                              double& z_m,
+                              int kernel_half = 1,
+                              double min_valid_m = 1e-4) {
+    if (depth.empty() || depth.channels() != 1) return false;
+    if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) return false;
+    if (kernel_half < 0) return false;
+
+    std::vector<double> valid;
+    valid.reserve(static_cast<size_t>((2 * kernel_half + 1) * (2 * kernel_half + 1)));
+
+    for (int dv = -kernel_half; dv <= kernel_half; ++dv) {
+        for (int du = -kernel_half; du <= kernel_half; ++du) {
+            const int uu = u + du;
+            const int vv = v + dv;
+            if (uu < 0 || vv < 0 || uu >= depth.cols || vv >= depth.rows) continue;
+
+            double z = 0.0;
+            if (depth.type() == CV_32FC1) {
+                z = static_cast<double>(depth.at<float>(vv, uu));
+            } else if (depth.type() == CV_16UC1) {
+                z = static_cast<double>(depth.at<uint16_t>(vv, uu)) * 1e-3;
+            } else {
+                return false;
+            }
+
+            if (std::isfinite(z) && z > min_valid_m) {
+                valid.push_back(z);
+            }
+        }
+    }
+
+    if (valid.empty()) return false;
+    std::nth_element(valid.begin(), valid.begin() + valid.size() / 2, valid.end());
+    z_m = valid[valid.size() / 2];
+    return true;
+}
+
+} // namespace cmvr
+

cmvr-es/controller/CMakeLists.txt

@@ -1,4 +1,6 @@
 
+find_package(VISP REQUIRED)
+
 
 #  如果报 relocation ... can not be used when making a shared object; recompile with -fPIC  ，说明SRC 中包含了test文件 ，test
 #  中链接 -lgtest -lgtest_main   ，这是静态库,导致 libcontroller.so 被迫依赖 gtest。
@@ -8,8 +10,8 @@
 #           ❌ 不要把 src/controller_test.cpp 放进来
 #)  其他动态库类似
 file(GLOB SRC
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_creator.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/pid_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/ibvs_controller.cpp
 
 )
 
@@ -20,6 +22,7 @@ target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(controller PUBLIC
         protobuf
+        cmvr_es::ik_solver
         cmvr_es::device::humanoid_robot
         gtest
         gtest_main
@@ -29,6 +32,9 @@ target_link_libraries(controller PUBLIC
         ccd
         fcl
         cmvr_es::device_manager
+        ${VISP_LIBRARIES}
+        pinocchio_default
+        pinocchio_parsers
 )
 
 add_library(cmvr_es::controller ALIAS controller)
@@ -39,7 +45,6 @@ add_library(cmvr_es::controller ALIAS controller)
 # --------------------------------------------------------
 # Unit test
 # --------------------------------------------------------
-find_package(VISP REQUIRED)
 find_package(realsense2 REQUIRED)
 add_executable(controller_test
         ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_test.cpp
@@ -52,6 +57,8 @@ target_link_libraries(controller_test
         cmvr_es::planner
         cmvr_es::proto
         cmvr_es::mujoco_viewer
+        cmvr_es::controller
+        cmvr_es::device::mujoco_camera
         gtest
         gtest_main
         pthread

cmvr-es/controller/include/controller.h

@@ -1,81 +0,0 @@
-//
-// Created by lgv on 11/27/25.
-//
-
-#pragma once
-
-#include <Eigen/Core>
-#include "common/consts/constant.h"
-namespace cmvr {
-
-    /// 当前机器人状态（输入）
-    struct ControlInput {
-        // 关节空间
-        Eigen::VectorXd q;   ///< 当前关节角
-        Eigen::VectorXd dq;  ///< 当前关节角速度
-
-        // 末端任务空间（阻抗控制用，可选）
-        Eigen::VectorXd x;   ///< 当前末端位姿（3或6维等）
-        Eigen::VectorXd dx;  ///< 当前末端速度
-        Eigen::MatrixXd Jx;  ///< 末端雅可比
-
-        // 视觉空间（视觉伺服用，可选）
-        Eigen::VectorXd s;   ///< 当前视觉特征
-        Eigen::MatrixXd Js;  ///< 视觉雅可比 ∂s/∂q
-    };
-
-    /// 控制目标（参考）
-    struct ControlReference {
-        // 关节空间参考（PID 等）
-        Eigen::VectorXd q_d;
-        Eigen::VectorXd dq_d;
-        Eigen::VectorXd ddq_d;
-
-        // 末端任务空间参考（阻抗）
-        Eigen::VectorXd x_d;
-        Eigen::VectorXd dx_d;
-
-        // 视觉空间参考（视觉伺服）
-        Eigen::VectorXd s_d;
-    };
-
-    /// @brief 抽象控制器基类：
-    class Controller {
-    public:
-        virtual ~Controller() = default;
-
-        /// 设置自由度数量
-        virtual void init(int dof) = 0;
-
-        /// 设置控制目标（关节 / 任务空间 / 视觉），
-        /// 各派生类只用自己关心的字段
-        virtual void setReference(const ControlReference& ref) = 0;
-
-        /// 核心：根据当前状态算控制量
-        virtual Eigen::VectorXd compute(const ControlInput& input, double dt) = 0;
-
-        // ========= 通用增益接口（可选，默认空实现） =========
-
-        /// 标量增益，例如视觉伺服里的 λ
-        virtual void setGain(double gain) {
-            UNUSED_VARIABLE(gain);
-        }
-
-        /// PID 三个向量增益
-        virtual void setGains(const Eigen::VectorXd& kp,
-                              const Eigen::VectorXd& ki,
-                              const Eigen::VectorXd& kd) {
-            UNUSED_VARIABLE(kp,ki,kd);
-        }
-
-        /// 阻抗控制用的 K, D 矩阵
-        virtual void setGains(const Eigen::MatrixXd& k,
-                              const Eigen::MatrixXd& d) {
-            UNUSED_VARIABLE(k,d);
-        }
-    };
-
-} // namespace cmvr
-
-
-

cmvr-es/controller/include/controller_creator.h

@@ -1,13 +0,0 @@
-//
-// Created by lgv on 11/27/25.
-//
-
-#ifndef CMVR_ES_CONTROLLER_CREATOR_H
-#define CMVR_ES_CONTROLLER_CREATOR_H
-
-
-class controller_creator {
-};
-
-
-#endif //CMVR_ES_CONTROLLER_CREATOR_H

cmvr-es/controller/include/ibvs_controller.h

@@ -0,0 +1,300 @@
+//
+// Created by lgv on 2026/2/26.
+//
+
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/detection/vpDetectorAprilTag.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+
+#include "devices/camera/abstract_camera.h"
+#include "ik_solver/include/pinocchio_dls_ik_solver.h"
+
+namespace cmvr {
+
+/**
+ * @brief 基于 AprilTag + ViSP + Pinocchio 的眼在手上 IBVS 控制器。
+ */
+class IbvsController {
+public:
+    /**
+     * @brief 深度使用模式。
+     */
+    enum class DepthMode {
+        MONOCULAR = 0, /**< 仅使用 AprilTag 位姿估计得到的深度。 */
+        PREFER_DEPTH,  /**< 优先使用深度图；无效时回退到位姿深度。 */
+        DEPTH_ONLY     /**< 必须使用深度图；无效则本次计算失败。 */
+    };
+
+    /**
+     * @brief `compute()` 最近一次状态。
+     */
+    enum class ComputeStatus {
+        OK = 0,        /**< 计算成功。 */
+        NOT_READY,     /**< 控制器未初始化完成。 */
+        NO_NEW_FRAME,  /**< 未取到可用新帧。 */
+        BAD_IMAGE,     /**< 图像格式或数据异常。 */
+        INVALID_INPUT, /**< 输入参数异常。 */
+        NO_DEPTH,      /**< 需要深度但深度不可用。 */
+        NO_TAG,        /**< 未检测到 AprilTag。 */
+        IK_FAILED      /**< 速度 IK 求解失败。 */
+    };
+
+    /**
+     * @brief 最近一次成功检测时使用的深度来源。
+     */
+    enum class DepthUsage {
+        NONE = 0,  /**< 本帧未使用深度。 */
+        POSE_ONLY, /**< 使用位姿估计深度。 */
+        DEPTH_ONLY, /**< 使用深度图深度。 */
+        MIXED      /**< 混合使用（预留）。 */
+    };
+
+    /**
+     * @brief 构造控制器并初始化默认任务参数。
+     */
+    IbvsController();
+
+    /**
+     * @brief 初始化控制器与 DLS 速度 IK 求解器。
+     * @param camera 相机对象。
+     * @param urdf_path URDF 文件路径。
+     * @param base_link 基坐标系 link 名称。
+     * @param flange_link 法兰 link 名称。
+     * @param camera_link 相机末端 link 名称（用于速度 IK）。
+     * @return 初始化成功返回 `true`。
+     */
+    bool init(const std::shared_ptr<device::AbstractCamera>& camera,
+              const std::string& urdf_path,
+              const std::string& base_link,
+              const std::string& flange_link,
+              const std::string& camera_link);
+
+    /**
+     * @brief 重置内部状态与关节命令缓存。
+     * @param q_init 初始关节命令；为空时清空缓存。
+     */
+    void reset(const std::vector<double>& q_init = {});
+
+    /**
+     * @brief 根据当前图像与关节角计算下一拍关节位置命令。
+     * @param joints_angle 当前关节角。
+     * @param dt 控制周期（秒）。
+     * @param q_cmd_out 输出的下一拍关节位置命令。
+     * @return 计算成功返回 `true`。
+     */
+    bool compute(const std::vector<double>& joints_angle,
+                 double dt,
+                 std::vector<double>& q_cmd_out);
+    /**
+     * @brief 根据当前图像与关节角计算目标关节速度命令。
+     * @param joints_angle 当前关节角。
+     * @param qdot_out 输出的关节速度命令（rad/s）。
+     * @return 计算成功返回 `true`。
+     */
+    bool compute(const std::vector<double>& joints_angle,
+                 std::vector<double>& qdot_out);
+
+    /**
+     * @brief 设置 ViSP 控制增益。
+     * @param lambda 控制增益 `lambda`。
+     */
+    void setLambda(double lambda);
+    /**
+     * @brief 设置 AprilTag 边长。
+     * @param tag_size_m tag 边长（米）。
+     */
+    void setTagSize(double tag_size_m);
+    /**
+     * @brief 设置期望目标位姿（`cMo_des`）。
+     * @param x 期望平移 x（米）。
+     * @param y 期望平移 y（米）。
+     * @param z 期望平移 z（米）。
+     * @param rx 期望旋转参数 rx（弧度，直接传给 `vpRotationMatrix::buildFrom`）。
+     * @param ry 期望旋转参数 ry（弧度，直接传给 `vpRotationMatrix::buildFrom`）。
+     * @param rz 期望旋转参数 rz（弧度，直接传给 `vpRotationMatrix::buildFrom`）。
+     */
+    void setTarget(double x,
+                   double y,
+                   double z,
+                   double rx = 3.14159265358979323846,
+                   double ry = 0.0,
+                   double rz = 0.0);
+    /**
+     * @brief 设置 DLS 阻尼。
+     * @param mu 阻尼系数。
+     */
+    void setMu(double mu);
+    /**
+     * @brief 设置关节速度绝对值上限。
+     * @param qdot_max 关节最大速度（rad/s）。
+     */
+    void setQdotMax(double qdot_max);
+    /**
+     * @brief 设置深度使用模式。
+     * @param mode 深度模式。
+     */
+    void setDepthMode(DepthMode mode);
+    /**
+     * @brief 设置深度闭环比例增益。
+     * @param kp 深度 `vz` 控制比例系数。
+     */
+    void setDepthZGain(double kp);
+    /**
+     * @brief 设置相机 twist 六维限幅。
+     * @param vmax6 线速度/角速度六维限幅。
+     */
+    void setVelocityLimit6(const std::array<double, 6>& vmax6);
+    /**
+     * @brief 设置 AbstractCamera 坐标系到 ViSP 坐标系旋转。
+     * @param R_cv 旋转矩阵。
+     */
+    void setAlignCameraToVisp(const Eigen::Matrix3d& R_cv);
+    /**
+     * @brief 设置 AbstractCamera 坐标系到 URDF 相机坐标系旋转。
+     * @param R_camera_urdf 旋转矩阵。
+     */
+    void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
+
+    /**
+     * @brief 最近一帧是否检测到 tag。
+     * @return 检测到返回 `true`。
+     */
+    bool isTagDetected() const { return last_tag_detected_; }
+    /**
+     * @brief 获取最近一次计算状态。
+     * @return 计算状态。
+     */
+    ComputeStatus lastComputeStatus() const { return last_compute_status_; }
+    /**
+     * @brief 计算状态转字符串。
+     * @param status 状态枚举。
+     * @return 状态字符串。
+     */
+    static const char* statusToString(ComputeStatus status);
+    /**
+     * @brief 获取最近一帧深度来源。
+     * @return 深度来源枚举。
+     */
+    DepthUsage lastDepthUsage() const { return last_depth_usage_; }
+    /**
+     * @brief 深度来源转字符串。
+     * @param usage 深度来源枚举。
+     * @return 深度来源字符串。
+     */
+    static const char* depthUsageToString(DepthUsage usage);
+    /**
+     * @brief 获取最近检测到的 tag 平移（ViSP 相机坐标系）。
+     * @return tag 平移向量。
+     */
+    const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
+    /**
+     * @brief 获取最近输出的相机 twist（ViSP 相机坐标系）。
+     * @return 六维 twist 向量。
+     */
+    const Eigen::Matrix<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
+
+private:
+    /**
+     * @brief 核心计算链路：图像 -> 相机 twist -> 关节速度。
+     * @param joints_angle 当前关节角。
+     * @param qdot_out 输出的关节速度命令（rad/s）。
+     * @return 计算成功返回 `true`。
+     */
+    bool computeInternal(const std::vector<double>& joints_angle,
+                         std::vector<double>& qdot_out);
+    /**
+     * @brief 根据当前目标位姿反解 tag 平面的深度控制点。
+     */
+    void updateDepthControlPointInTag();
+    /**
+     * @brief 重新构建 ViSP 任务与期望特征。
+     */
+    void initTask();
+
+private:
+    // 是否初始化成功。
+    bool initialized_{false};
+    // 速度 IK 使用的末端相机 frame 名称。
+    std::string camera_frame_name_;
+    // 相机对象。
+    std::shared_ptr<device::AbstractCamera> camera_{nullptr};
+
+    // 参数
+    // ViSP 控制增益。
+    double lambda_{1.7};
+    // tag 边长（米）。
+    double tag_size_m_{0.12};
+    // tag 半边长（米）。
+    double tag_half_{0.06};
+    // 期望平移 x（米）。
+    double target_x_{0.0};
+    // 期望平移 y（米）。
+    double target_y_{0.0};
+    // 期望平移 z（米）。
+    double target_z_{0.33};
+    // 期望旋转参数 rx（弧度）。
+    double target_rx_{3.14159265358979323846};
+    // 期望旋转参数 ry（弧度）。
+    double target_ry_{0.0};
+    // 期望旋转参数 rz（弧度）。
+    double target_rz_{0.0};
+    // DLS 阻尼系数。
+    double mu_{0.02};
+    // 关节速度绝对值上限（rad/s）。
+    double qdot_max_{0.6};
+    // 深度模式。
+    DepthMode depth_mode_{DepthMode::MONOCULAR};
+    // 深度闭环增益：`vz = kp * (z_cur - target_z_)`。
+    double depth_z_kp_{1.0};
+    // 相机 twist 六维限幅。
+    std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
+    // tag 平面中用于采样深度的控制点。
+    Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
+
+    // 坐标对齐
+    // AbstractCamera -> ViSP 的旋转矩阵。
+    Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
+    // AbstractCamera -> URDF 相机系的旋转矩阵。
+    Eigen::Matrix3d R_camera_urdf_{Eigen::Matrix3d::Identity()};
+
+    // ViSP
+    // ViSP 伺服任务对象。
+    std::unique_ptr<vpServo> task_{nullptr};
+    // tag 四角点（3D）。
+    vpPoint obj_pts_[4];
+    // 当前特征。
+    vpFeaturePoint s_cur_[4];
+    // 目标特征。
+    vpFeaturePoint s_star_[4];
+    // AprilTag 检测器。
+    vpDetectorAprilTag detector_;
+
+    // 速度 IK 求解器。
+    std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
+
+    // 内部积分得到的关节位置命令缓存。
+    std::vector<double> q_cmd_;
+    // 最近一帧 tag 检测结果。
+    bool last_tag_detected_{false};
+    // 最近一次 `compute()` 状态。
+    ComputeStatus last_compute_status_{ComputeStatus::NOT_READY};
+    // 最近一帧深度来源。
+    DepthUsage last_depth_usage_{DepthUsage::NONE};
+    // 最近一帧 tag 平移（ViSP 相机系）。
+    Eigen::Vector3d last_tag_pos_visp_{Eigen::Vector3d::Zero()};
+    // 最近一帧相机 twist（ViSP 相机系）。
+    Eigen::Matrix<double, 6, 1> last_v_camera_visp_{Eigen::Matrix<double, 6, 1>::Zero()};
+};
+
+} // namespace cmvr

cmvr-es/controller/include/pid_controller.h

@@ -4,68 +4,61 @@
 
 #pragma once
 
-#include "controller/include/controller.h"
-#include <limits>
-
 namespace cmvr {
 
-    class PidController : public Controller {
+    class PidController {
     public:
         PidController();
 
-        // 设置自由度+ 初始化
+        // 初始化 PID 控制器
-        void init(int dof) override;
+        void init();
 
-        // 设置目标（只用到 q_d, dq_d）
+        // 核心：根据当前状态计算控制量
-        void setReference(const ControlReference& ref) override;
+        double compute(double target, double current, double dt);
 
-        // 核心：根据当前状态计算控制量（约定为关节力矩）
+        // PID 增益
-        Eigen::VectorXd compute(const ControlInput& input, double dt) override;
+        void setGains(double kp, double ki, double kd);
 
-        // PID 三向量增益
+        /// 设置积分上下限： lower <= I <= upper
-        void setGains(const Eigen::VectorXd& kp,
+        void setIntegralLimits(double lower, double upper);
-                      const Eigen::VectorXd& ki,
-                      const Eigen::VectorXd& kd) override;
 
-        /// 设置积分上下限： lower[i] <= I[i] <= upper[i]
+        /// 设置误差死区： |error| < deadzone 时视为 0
-        /// upper[i] <= lower[i] 表示该关节不启用积分限幅
+        void setDeadzone(double deadzone);
-        void setIntegralLimits(const Eigen::VectorXd& lower,
-                               const Eigen::VectorXd& upper);
-
-        /// 设置误差死区： |error[i]| < deadzone[i] 时视为 0
-        void setDeadzone(const Eigen::VectorXd& deadzone);
 
         /// 设置 D 项滤波系数（简单一阶滤波）
         /// coeff <= 0 表示不启用滤波
         void setDerivativeFilterCoeff(double coeff);
 
-    private:
+        /// 设置输出的上下限
-        int dof_ = 0;
+        void setOutputLimits(double lower, double upper);
 
+    private:
         // PID 增益
-        Eigen::VectorXd kp_;
+        double kp_;
-        Eigen::VectorXd ki_;
+        double ki_;
-        Eigen::VectorXd kd_;
+        double kd_;
 
         // 期望状态
-        Eigen::VectorXd q_d_;
+        double q_d_;  // 只关心目标位置，不需要目标速度
-        Eigen::VectorXd dq_d_;
 
         // 积分项、上一拍误差
-        Eigen::VectorXd integralError_;
+        double integralError_;
-        Eigen::VectorXd prevError_;
+        double prevError_;
 
         // 积分上下限
-        Eigen::VectorXd integralLowerLimit_;
+        double integralLowerLimit_;
-        Eigen::VectorXd integralUpperLimit_;
+        double integralUpperLimit_;
 
         // 死区
-        Eigen::VectorXd deadzone_;
+        double deadzone_;
 
         // D 项滤波
-        double dFilterCoeff_ = 0.0;      ///< 滤波系数，0 表示不开滤波
+        double dFilterCoeff_;      ///< 滤波系数，0 表示不开滤波
-        Eigen::VectorXd dErrorFiltered_; ///< 滤波后的 dError
+        double dErrorFiltered_; ///< 滤波后的 dError
+
+        // 输出限制
+        double outputLowerLimit_;
+        double outputUpperLimit_;
     };
 
 } // namespace cmvr
-

cmvr-es/controller/src/controller_creator.cpp

@@ -1,5 +0,0 @@
-//
-// Created by lgv on 11/27/25.
-//
-
-#include "../include/controller_creator.h"

cmvr-es/controller/src/controller_test.cpp

@@ -8,13 +8,17 @@
 #include <atomic>
 #include <chrono>
 #include <cmath>
+#include <limits>
+#include <memory>
 #include <mutex>
+#include <string>
 #include <thread>
 #include <vector>
 
-#include <Eigen/Dense>
 
-#include "ik_solver/include/lawba_ik_solver.h"
+#include "controller/include/ibvs_controller.h"
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
+#include "ik_solver/include/pinocchio_dls_ik_solver.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 using namespace cmvr;
 
@@ -452,15 +456,23 @@ class IBVSFromMujocoCameraViewer : public MuJocoViewer {
 public:
   using MuJocoViewer::MuJocoViewer;
 
+  void enableTwistToQdotCheck(bool enable, double tol = 1e-3) {
+    check_twist_to_qdot_ = enable;
+    qdot_check_tol_ = tol;
+    (void)qdot_check_tol_;
+  }
+
+  bool twistToQdotCheckPassed() const { return !qdot_check_failed_; }
+  double twistToQdotMaxError() const { return qdot_check_max_err_; }
+  int twistToQdotCheckSamples() const { return qdot_check_samples_; }
+
 protected:
   enum class Mode { HOMING, IBVS };
 
   void initOnce(mjModel* m, mjData* d) override {
-    // 1) 主视角 + PiP（你的 renderPiP 会自动把 hand_cam 画出来并缓存 RGBD）
     setupCamera(3.0, -170.0, -40.0);
     enablePiPCamera("hand_cam", 405, 1000, 320, 240);
 
-    // 2) 右臂 7DOF actuator / joint
     const char* act_names[7] = {
       "R_SHOULDER_P_pos",
       "R_SHOULDER_R_pos",
@@ -489,14 +501,11 @@ protected:
       if (jnt_ids_[i] < 0) {
         std::cout << "[IBVS] Cannot find joint " << jnt_names[i] << std::endl;
       }
-
       if (jnt_ids_[i] >= 0) {
         qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]];
-        dof_adr_[i]  = m->jnt_dofadr[jnt_ids_[i]];
       }
     }
 
-    // 3) 必须有：R_CAM_SITE 用来算 jacobian（末端相机坐标）
     cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE");
     if (cam_site_id_ < 0) {
       std::cout << "[IBVS] Missing site R_CAM_SITE in XML" << std::endl;
@@ -504,72 +513,60 @@ protected:
       return;
     }
 
-    // 4) hand_cam：用它的 fovy 计算内参
     hand_cam_id_ = mj_name2id(m, mjOBJ_CAMERA, "hand_cam");
     if (hand_cam_id_ < 0) {
       std::cout << "[IBVS] Missing camera hand_cam in XML" << std::endl;
       ready_ = false;
       return;
     }
+    mujoco_camera_ = std::make_shared<device::MujocoCamera>(
+      [this](std::vector<unsigned char>& rgb,
+             std::vector<float>& depth,
+             int& width,
+             int& height,
+             uint64_t& frame_id) {
+        return getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+      });
+    mujoco_camera_->setFovyDeg(m->cam_fovy[hand_cam_id_]);
+    // MuJoCo 渲染线程与控制线程不同步：允许复用最近一帧，避免长时间 no_new_frame。
+    mujoco_camera_->setConsumeNewFrameOnly(false);
 
-    // 5) homing 姿态（先让相机看到 tag）
     q_home_ = {0.25, 1.00, M_PI / 2 - 0.2, M_PI / 2 - 0.2, -M_PI + 0.5, 0.0, 0.0};
 
-    // 6) 坐标对齐（site相机系 -> ViSP相机系）
+    ibvs_controller_ = std::make_unique<IbvsController>();
-    // 这就是你之前验证过能收敛的那套：x/y 同时翻转
+    ibvs_controller_->setMu(mu_);
-    R_cv_ = (Eigen::Matrix3d() <<
+    ibvs_controller_->setQdotMax(qdot_max_);
+    ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR);
+    ibvs_controller_->setDepthZGain(1.0);
+    ibvs_controller_->setVelocityLimit6(vmax6_);
+    ibvs_controller_->setTarget(0.0,0.0,0.15);
+
+    const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
       1, 0, 0,
       0, -1, 0,
       0, 0, -1).finished();
+    ibvs_controller_->setAlignCameraToVisp(R_align);
+    ibvs_controller_->setAlignCameraToUrdf(R_align);
 
-    // 7) IBVS 任务：用 AprilTag 的 4 个角点特征
+    if (!ibvs_controller_->init(mujoco_camera_, urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) {
-    tag_size_m_ = 0.12;            // 12cm
+      std::cout << "[IBVS] IbvsController init failed" << std::endl;
-    tag_half_   = tag_size_m_ * 0.5;
+      ready_ = false;
-    Z_des_      = 0.28;
+      return;
-    lambda_     = 1.7;
-
-    task_.setServo(vpServo::EYEINHAND_CAMERA);
-    task_.setInteractionMatrixType(vpServo::CURRENT);
-    task_.setLambda(lambda_);
-
-    // tag 平面 z=0，原点在 tag 中心
-    obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
-    obj_pts_[1].setWorldCoordinates( tag_half_, -tag_half_, 0.0);
-    obj_pts_[2].setWorldCoordinates( tag_half_,  tag_half_, 0.0);
-    obj_pts_[3].setWorldCoordinates(-tag_half_,  tag_half_, 0.0);
-
-    // desired: 正对 + 距离 Z_des
-    {
-      vpTranslationVector t_des(0.0, 0.0, Z_des_);
-      // tag 在（相机坐标系visp 相机）下的朝向
-      vpRotationMatrix R_des;
-      R_des.buildFrom(M_PI, 0, 0);
-      vpHomogeneousMatrix cMo_des(t_des, R_des);
-
-      for (int i = 0; i < 4; ++i) {
-        obj_pts_[i].track(cMo_des);
-        s_star_[i].buildFrom(obj_pts_[i].get_x(),
-                             obj_pts_[i].get_y(),
-                             obj_pts_[i].get_Z());
-
-        s_cur_[i].buildFrom(0.0, 0.0, 1.0);
-        task_.addFeature(s_cur_[i], s_star_[i]);
-      }
     }
 
-    // 8) AprilTag detector
+    std::vector<double> q_init(7, 0.0);
-    detector_ = vpDetectorAprilTag(vpDetectorAprilTag::TAG_36h11);
-    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
-
-    // 初始化 q_cmd，防跳变
     for (int i = 0; i < 7; ++i) {
-      q_cmd_[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
+      q_init[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
+      q_cmd_[i] = q_init[i];
     }
+    ibvs_controller_->reset(q_init);
 
     mode_ = Mode::HOMING;
     home_hold_acc_ = 0.0;
     step_count_ = 0;
-    last_frame_id_ = 0;
+    qdot_check_max_err_ = 0.0;
+    qdot_check_samples_ = 0;
+    qdot_check_failed_ = false;
     ready_ = true;
 
     std::cout << "[IBVS] initOnce OK. cam_site_id=" << cam_site_id_
@@ -580,9 +577,6 @@ protected:
     if (!ready_) return;
     const double dt = m->opt.timestep;
 
-    // ======================
-    // A) HOMING
-    // ======================
     if (mode_ == Mode::HOMING) {
       double max_err = 0.0;
       for (int i = 0; i < 7; ++i) {
@@ -596,155 +590,61 @@ protected:
       else home_hold_acc_ = 0.0;
 
       if (home_hold_acc_ > home_hold_time_) {
+        std::vector<double> q_now(7, 0.0);
         for (int i = 0; i < 7; ++i) {
-          if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
+          q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
+          q_cmd_[i] = q_now[i];
         }
+        ibvs_controller_->reset(q_now);
         mode_ = Mode::IBVS;
         std::cout << "[IBVS] switch HOMING -> IBVS" << std::endl;
       }
       return;
     }
 
-    // ======================
+    std::vector<double> q_now(7, 0.0);
-    // B) IBVS（用 MuJoCo 相机图像）
+    for (int i = 0; i < 7; ++i) {
-    // ======================
+      q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
-
-    // 1) 取最新 PiP 图像（RGB + depth z-buffer）
-    std::vector<unsigned char> rgb;
-    std::vector<float> depth;
-    int w=0, h=0;
-    uint64_t fid=0;
-
-    if (!getPiPCameraRGBD(rgb, depth, w, h, fid)) {
-      return;
-    }
-    if (fid == last_frame_id_) {
-      return; // 没新帧就不做视觉（避免重复计算）
-    }
-    last_frame_id_ = fid;
-
-    if (w <= 0 || h <= 0 || (int)rgb.size() != 3*w*h) return;
-
-    // 2) 计算相机内参（从 hand_cam 的 fovy）
-    // MuJoCo 的 fovy 是“垂直视场角（度）”
-    const double fovy_deg = m->cam_fovy[hand_cam_id_];
-    const double fovy = fovy_deg * M_PI / 180.0;
-    const double fy = (h * 0.5) / std::tan(fovy * 0.5);
-    const double fx = fy;                 // 由几何关系可得（见推导）
-    const double cx = w * 0.5;
-    const double cy = h * 0.5;
-
-    vpCameraParameters cam;
-    cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
-
-    // 3) RGB -> 灰度 vpImage
-    vpImage<unsigned char> I(h, w);
-    for (int y = 0; y < h; ++y) {
-      for (int x = 0; x < w; ++x) {
-        const int idx = (y*w + x) * 3;
-        const unsigned char r = rgb[idx + 0];
-        const unsigned char g = rgb[idx + 1];
-        const unsigned char b = rgb[idx + 2];
-        I[y][x] = static_cast<unsigned char>(0.299*r + 0.587*g + 0.114*b);
-      }
     }
 
-    // 4) AprilTag 检测 + 位姿估计（直接得到 cMo）
+    std::vector<double> q_cmd_next;
-    std::vector<vpHomogeneousMatrix> cMo_vec;
+    const bool ok = ibvs_controller_->compute(q_now, dt, q_cmd_next);
-    bool ok = detector_.detect(I, tag_size_m_, cam, cMo_vec);
 
-    if (!ok || cMo_vec.empty()) {
+    if (!ok) {
-      // 没检测到：最简单策略：保持当前位置（不更新 ctrl）
       if ((step_count_ % 60) == 0) {
-        std::cout << "[IBVS] no tag detected" << std::endl;
+        const auto st = ibvs_controller_->lastComputeStatus();
+        std::cout << "[IBVS] compute skipped: "
+                  << IbvsController::statusToString(st) << std::endl;
       }
       ++step_count_;
       return;
     }
 
-    vpHomogeneousMatrix cMo = cMo_vec[0];
+    if (q_cmd_next.size() == 7) {
-
-    // 5) 用 cMo 更新四角点特征
-    for (int i = 0; i < 4; ++i) {
-      obj_pts_[i].track(cMo);
-      const double x = obj_pts_[i].get_x();
-      const double y = obj_pts_[i].get_y();
-      const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
-      s_cur_[i].buildFrom(x, y, Z);
-    }
-
-    // 6) ViSP 控制律：得到相机速度 v_c（ViSP 相机系）
-    vpColVector v_c = task_.computeControlLaw();
-
-    // 限幅（很关键）
-    for (int k = 0; k < 6; ++k) v_c[k] = clamp(v_c[k], -vmax6_[k], vmax6_[k]);
-
-    if ((step_count_ % 60) == 0) {
-      // 粗略打印一下位姿平移（单位 m）
-      vpTranslationVector t = cMo.getTranslationVector();
-      std::cout << "[IBVS] t_co(visp)=[" << t[0] << " " << t[1] << " " << t[2]
-                << "]" << std::endl;
-      std::cout << "[IBVS] v_c=[" << v_c[0] << " " << v_c[1] << " " << v_c[2]
-                << " " << v_c[3] << " " << v_c[4] << " " << v_c[5]
-                << "]" << std::endl;
-    }
-
-    // 7) 把 ViSP 相机速度 -> site 相机速度 -> world twist
-    // 先：ViSP -> site（逆对齐）
-    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
-    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
-    Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
-    Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
-
-    // 再：site -> world（用 R_cw）
-    const mjtNum* pc = d->site_xpos + 3 * cam_site_id_;
-    const mjtNum* Rc9 = d->site_xmat + 9 * cam_site_id_;
-    (void)pc;
-    Eigen::Matrix3d R_cw = xmat_to_R(Rc9);
-
-    Eigen::Vector3d v_w = R_cw * v_site;
-    Eigen::Vector3d w_w = R_cw * w_site;
-
-    Eigen::Matrix<double,6,1> twist_w;
-    twist_w << v_w(0), v_w(1), v_w(2), w_w(0), w_w(1), w_w(2);
-
-    // 8) Jacobian（mj_jacSite 给的是 world 线速度/角速度）
-    std::vector<mjtNum> jacp(3 * m->nv);
-    std::vector<mjtNum> jacr(3 * m->nv);
-    mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_);
-
-    Eigen::Matrix<double,6,7> J;
-    J.setZero();
-    for (int j = 0; j < 7; ++j) {
-      const int dof = dof_adr_[j];
-      if (dof < 0) continue;
-      J(0,j) = jacp[0*m->nv + dof];
-      J(1,j) = jacp[1*m->nv + dof];
-      J(2,j) = jacp[2*m->nv + dof];
-      J(3,j) = jacr[0*m->nv + dof];
-      J(4,j) = jacr[1*m->nv + dof];
-      J(5,j) = jacr[2*m->nv + dof];
-    }
-
-    // 9) DLS: qdot
-    Eigen::Matrix<double,6,6> A = J * J.transpose();
-    A += (mu_*mu_) * Eigen::Matrix<double,6,6>::Identity();
-    Eigen::Matrix<double,7,1> qdot = J.transpose() * A.inverse() * twist_w;
-
-    for (int i = 0; i < 7; ++i) qdot(i) = clamp(qdot(i), -qdot_max_, qdot_max_);
-
-    // 10) integrate -> 位置控制 ctrl
       for (int i = 0; i < 7; ++i) {
-      q_cmd_[i] += qdot(i) * dt;
+        q_cmd_[i] = q_cmd_next[i];
+      }
+    }
 
-      // joint limit clamp
+    for (int i = 0; i < 7; ++i) {
       if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) {
         const double lo = m->jnt_range[2 * jnt_ids_[i] + 0];
         const double hi = m->jnt_range[2 * jnt_ids_[i] + 1];
         q_cmd_[i] = clamp(q_cmd_[i], lo, hi);
       }
+      if (act_ids_[i] >= 0) {
+        d->ctrl[act_ids_[i]] = q_cmd_[i];
+      }
+    }
 
-      if (act_ids_[i] >= 0) d->ctrl[act_ids_[i]] = q_cmd_[i];
+    if ((step_count_ % 60) == 0) {
+      const auto& v_c = ibvs_controller_->lastCameraTwistVisp();
+      const auto& t_co = ibvs_controller_->lastTagPositionVisp();
+      std::cout << "[IBVS] v_c=[" << v_c[0] << " " << v_c[1] << " " << v_c[2]
+                << " " << v_c[3] << " " << v_c[4] << " " << v_c[5] << "]" << std::endl;
+      std::cout << "[IBVS] t_co(visp)=[" << t_co.x() << " " << t_co.y() << " " << t_co.z() << "]" << std::endl;
+      std::cout << "[IBVS] z_source="
+                << IbvsController::depthUsageToString(ibvs_controller_->lastDepthUsage()) << std::endl;
     }
 
     ++step_count_;
@@ -752,57 +652,55 @@ protected:
 
   void onReset(mjModel* m, mjData* d) override {
     (void)m;
+    std::vector<double> q_now(7, 0.0);
     for (int i = 0; i < 7; ++i) {
-      if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
+      q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
+      q_cmd_[i] = q_now[i];
+    }
+    if (ibvs_controller_) {
+      ibvs_controller_->reset(q_now);
     }
     mode_ = Mode::HOMING;
     home_hold_acc_ = 0.0;
     step_count_ = 0;
-    last_frame_id_ = 0;
+    qdot_check_max_err_ = 0.0;
+    qdot_check_samples_ = 0;
+    qdot_check_failed_ = false;
   }
 
 private:
   bool ready_{false};
 
-  // ids
   std::array<int, 7> act_ids_{};
   std::array<int, 7> jnt_ids_{};
   std::array<int, 7> qpos_adr_{{-1, -1, -1, -1, -1, -1, -1}};
-  std::array<int,7> dof_adr_{  { -1,-1,-1,-1,-1,-1,-1 } };
   int cam_site_id_{-1};
   int hand_cam_id_{-1};
 
-  // homing
   Mode mode_{Mode::HOMING};
   std::array<double, 7> q_home_{{0, 0, 0, 0, 0, 0, 0}};
   double home_tol_{0.02};
   double home_hold_time_{0.3};
   double home_hold_acc_{0.0};
 
-  // align: site -> ViSP
+  double mu_{0.02};
-  Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
+  double qdot_max_{0.6};
+  std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
 
-  // IBVS params
+  bool check_twist_to_qdot_{true};
-  double lambda_{0.7};
+  double qdot_check_tol_{1e-3};
-  double tag_size_m_{0.12};
+  double qdot_check_max_err_{0.0};
-  double tag_half_{0.06};
+  int qdot_check_samples_{0};
-  double Z_des_{0.60};
+  bool qdot_check_failed_{false};
+  std::string urdf_path_for_check_{
+    "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"};
+  std::string camera_frame_name_for_check_{"R_CAM"};
 
-  double mu_{0.02};          // DLS damping
+  std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
-  double qdot_max_{0.6};     // rad/s
+  std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
-  double vmax6_[6] = {0.15, 0.15, 0.20, 0.6, 0.6, 0.6};
 
-  // ViSP
-  vpServo task_;
-  vpPoint obj_pts_[4];
-  vpFeaturePoint s_cur_[4];
-  vpFeaturePoint s_star_[4];
-  vpDetectorAprilTag detector_;
-
-  // control state
   std::array<double, 7> q_cmd_{{0, 0, 0, 0, 0, 0, 0}};
   int step_count_{0};
-  uint64_t last_frame_id_{0};
 };
 
 
@@ -902,5 +800,13 @@ TEST(controller_test, mujoco_camera_apriltag_ibvs_full)
   IBVSFromMujocoCameraViewer viewer(
     "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"
   );
+  viewer.enableTwistToQdotCheck(true, 1e-3);
   viewer.run();
+
+  // EXPECT_GT(viewer.twistToQdotCheckSamples(), 0)
+  //   << "no valid samples collected for twistToQdot consistency";
+  // if (viewer.twistToQdotCheckSamples() > 0) {
+  //   EXPECT_TRUE(viewer.twistToQdotCheckPassed())
+  //     << "twistToQdot mismatch, max_abs_err=" << viewer.twistToQdotMaxError();
+  // }
 }

cmvr-es/controller/src/ibvs_controller.cpp

@@ -0,0 +1,411 @@
+//
+// Created by lgv on 2026/2/26.
+//
+
+#include "controller/include/ibvs_controller.h"
+
+#include "common/utils/image/image_process.h"
+#include "common/utils/math/support_functions.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <limits>
+
+#include <opencv2/imgproc.hpp>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+namespace cmvr {
+
+
+
+const char* IbvsController::statusToString(ComputeStatus status) {
+    switch (status) {
+        case ComputeStatus::OK: return "ok";
+        case ComputeStatus::NOT_READY: return "not_ready";
+        case ComputeStatus::NO_NEW_FRAME: return "no_new_frame";
+        case ComputeStatus::BAD_IMAGE: return "bad_image";
+        case ComputeStatus::INVALID_INPUT: return "invalid_input";
+        case ComputeStatus::NO_DEPTH: return "no_depth";
+        case ComputeStatus::NO_TAG: return "no_tag";
+        case ComputeStatus::IK_FAILED: return "ik_failed";
+        default: return "unknown";
+    }
+}
+
+const char* IbvsController::depthUsageToString(DepthUsage usage) {
+    switch (usage) {
+        case DepthUsage::NONE: return "none";
+        case DepthUsage::POSE_ONLY: return "pose_only";
+        case DepthUsage::DEPTH_ONLY: return "depth_only";
+        case DepthUsage::MIXED: return "mixed";
+        default: return "unknown";
+    }
+}
+
+IbvsController::IbvsController()
+    : detector_(vpDetectorAprilTag::TAG_36h11) {
+    // AbstractCamera 相机系 -> ViSP 相机系
+    R_cv_ = (Eigen::Matrix3d() <<
+        1, 0, 0,
+        0, -1, 0,
+        0, 0, -1).finished();
+
+    // AbstractCamera 相机系 -> URDF 相机系
+    R_camera_urdf_ = (Eigen::Matrix3d() <<
+        1, 0, 0,
+        0, -1, 0,
+        0, 0, -1).finished();
+
+    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
+    updateDepthControlPointInTag();
+    initTask();
+}
+
+bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
+                          const std::string& urdf_path,
+                          const std::string& base_link,
+                          const std::string& flange_link,
+                          const std::string& camera_link) {
+    camera_ = camera;
+    if (!camera_) {
+        initialized_ = false;
+        last_compute_status_ = ComputeStatus::NOT_READY;
+        return false;
+    }
+
+    camera_frame_name_ = camera_link;
+    dls_solver_ = std::make_unique<PinocchioDlsIKSolver>(
+        urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_);
+
+    initialized_ = dls_solver_->init();
+    last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
+    return initialized_;
+}
+
+void IbvsController::reset(const std::vector<double>& q_init) {
+    if (q_init.empty()) {
+        q_cmd_.clear();
+    } else {
+        q_cmd_ = q_init;
+    }
+    last_tag_detected_ = false;
+    last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
+    last_depth_usage_ = DepthUsage::NONE;
+    last_tag_pos_visp_.setZero();
+    last_v_camera_visp_.setZero();
+}
+
+bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
+                                     std::vector<double>& qdot_out) {
+    last_depth_usage_ = DepthUsage::NONE;
+
+    if (!initialized_ || !camera_ || !dls_solver_) {
+        last_compute_status_ = ComputeStatus::NOT_READY;
+        return false;
+    }
+    if (joints_angle.empty()) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+
+    cv::Mat color;
+    cv::Mat depth;
+    device::Rs2Intrinsics intrinsics{};
+    camera_->getRGBDImages(color, depth, intrinsics);
+
+    if (color.empty()) {
+        last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
+        return false;
+    }
+    if (color.channels() != 3) {
+        if (color.channels() != 1 && color.channels() != 4) {
+            last_compute_status_ = ComputeStatus::BAD_IMAGE;
+            return false;
+        }
+    }
+
+    const double fx = static_cast<double>(intrinsics.fx);
+    const double fy = static_cast<double>(intrinsics.fy);
+    const double cx = static_cast<double>(intrinsics.cx);
+    const double cy = static_cast<double>(intrinsics.cy);
+    if (fx <= 0.0 || fy <= 0.0) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+
+    cv::Mat gray;
+    if (color.channels() == 1) {
+        gray = color;
+    } else if (color.channels() == 3) {
+        cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
+    } else {
+        cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
+    }
+    if (gray.empty() || gray.type() != CV_8UC1) {
+        last_compute_status_ = ComputeStatus::BAD_IMAGE;
+        return false;
+    }
+    if (!gray.isContinuous()) {
+        gray = gray.clone();
+    }
+
+    const int width = gray.cols;
+    const int height = gray.rows;
+    vpCameraParameters cam;
+    cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
+
+    vpImage<unsigned char> I(height, width);
+    for (int y = 0; y < height; ++y) {
+        std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
+    }
+
+    std::vector<vpHomogeneousMatrix> cMo_vec;
+    const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
+    last_tag_detected_ = (detected && !cMo_vec.empty());
+    if (!last_tag_detected_) {
+        last_tag_pos_visp_.setZero();
+        last_compute_status_ = ComputeStatus::NO_TAG;
+        return false;
+    }
+
+    vpHomogeneousMatrix cMo = cMo_vec[0];
+    last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
+
+    // 深度控制点定义在 tag 平面（object frame）:
+    //   p_o = [x_t, y_t, 0, 1]^T
+    // 经位姿变换后在相机系:
+    //   p_c = cMo * p_o = [X, Y, Z, 1]^T
+    // vpPoint::get_x/get_y 给的是归一化坐标 x=X/Z, y=Y/Z。
+    vpPoint depth_ctrl_pt;
+    depth_ctrl_pt.setWorldCoordinates(depth_control_point_tag_.x(), depth_control_point_tag_.y(), 0.0);
+    depth_ctrl_pt.track(cMo);
+
+    const double x_depth_ctrl = depth_ctrl_pt.get_x();
+    const double y_depth_ctrl = depth_ctrl_pt.get_y();
+    double z_depth_ctrl = std::max(depth_ctrl_pt.get_Z(), 0.05);
+    bool depth_ctrl_used = false;
+
+    if (depth_mode_ != DepthMode::MONOCULAR) {
+        // 针孔投影:
+        //   u = fx * x + cx
+        //   v = fy * y + cy
+        const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx));
+        const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy));
+        double z_from_depth = 0.0;
+        if (sampleDepthMeters(depth, u, v, z_from_depth)) {
+            z_depth_ctrl = std::max(z_from_depth, 0.05);
+            depth_ctrl_used = true;
+        } else if (depth_mode_ == DepthMode::DEPTH_ONLY) {
+            last_compute_status_ = ComputeStatus::NO_DEPTH;
+            return false;
+        }
+    }
+
+    for (int i = 0; i < 4; ++i) {
+        obj_pts_[i].track(cMo);
+        const double x = obj_pts_[i].get_x();
+        const double y = obj_pts_[i].get_y();
+        const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
+        s_cur_[i].buildFrom(x, y, Z);
+    }
+
+    if (depth_ctrl_used) {
+        last_depth_usage_ = DepthUsage::DEPTH_ONLY;
+    } else {
+        last_depth_usage_ = DepthUsage::POSE_ONLY;
+    }
+
+    vpColVector v_c = task_->computeControlLaw();
+
+    // 深度闭环（仅替换 z 方向）:
+    //   e_z = z_cur - z_target
+    //   v_z = k_p * e_z
+    // 其余 5 维仍沿用 ViSP IBVS 控制律输出。
+    if (depth_ctrl_used) {
+        v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
+    }
+
+    for (int i = 0; i < 6; ++i) {
+        v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
+        last_v_camera_visp_[i] = v_c[i];
+    }
+
+    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
+    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
+    // R_cv_: AbstractCamera -> ViSP，所以从 ViSP 回到 AbstractCamera 要乘转置:
+    //   v_cam = R_cv^T * v_visp
+    //   w_cam = R_cv^T * w_visp
+    Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
+    Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
+
+    Eigen::Matrix<double, 6, 1> twist_ee;
+    twist_ee << v_site(0), v_site(1), v_site(2), w_site(0), w_site(1), w_site(2);
+
+    Eigen::Matrix<double, 6, 1> twist_ee_pin;
+    // 线速度和角速度都用同一旋转做坐标变换:
+    //   v_urdf = R_camera_urdf * v_cam
+    //   w_urdf = R_camera_urdf * w_cam
+    twist_ee_pin.head<3>() = R_camera_urdf_ * twist_ee.head<3>();
+    twist_ee_pin.tail<3>() = R_camera_urdf_ * twist_ee.tail<3>();
+
+    std::vector<double> qdot;
+    const bool ok = dls_solver_->velocityIk(
+        joints_angle, twist_ee_pin, qdot, camera_frame_name_, mu_, std::numeric_limits<double>::infinity());
+    if (!ok || qdot.size() != joints_angle.size()) {
+        last_compute_status_ = ComputeStatus::IK_FAILED;
+        return false;
+    }
+
+    qdot_out.resize(qdot.size());
+    for (size_t i = 0; i < qdot.size(); ++i) {
+        qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
+    }
+
+    last_compute_status_ = ComputeStatus::OK;
+    return true;
+}
+
+bool IbvsController::compute(const std::vector<double>& joints_angle,
+                             double dt,
+                             std::vector<double>& q_cmd_out) {
+    if (dt <= 0.0) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+    if (q_cmd_.size() != joints_angle.size()) {
+        q_cmd_ = joints_angle;
+    }
+
+    std::vector<double> qdot;
+    if (!computeInternal(joints_angle, qdot)) {
+        return false;
+    }
+
+    for (size_t i = 0; i < qdot.size(); ++i) {
+        // 显式欧拉积分:
+        //   q_{k+1} = q_k + qdot * dt
+        q_cmd_[i] += qdot[i] * dt;
+    }
+
+    q_cmd_out = q_cmd_;
+    return true;
+}
+
+bool IbvsController::compute(const std::vector<double>& joints_angle,
+                             std::vector<double>& qdot_out) {
+    return computeInternal(joints_angle, qdot_out);
+}
+
+void IbvsController::setLambda(double lambda) {
+    lambda_ = lambda;
+    initTask();
+}
+
+void IbvsController::setTagSize(double tag_size_m) {
+    tag_size_m_ = tag_size_m;
+    tag_half_ = tag_size_m_ * 0.5;
+    initTask();
+}
+
+void IbvsController::setTarget(double x,
+                               double y,
+                               double z,
+                               double rx,
+                               double ry,
+                               double rz) {
+    target_x_ = x;
+    target_y_ = y;
+    target_z_ = z;
+    target_rx_ = rx;
+    target_ry_ = ry;
+    target_rz_ = rz;
+    updateDepthControlPointInTag();
+    initTask();
+}
+
+void IbvsController::setMu(double mu) {
+    mu_ = mu;
+}
+
+void IbvsController::setQdotMax(double qdot_max) {
+    qdot_max_ = qdot_max;
+}
+
+void IbvsController::setDepthMode(DepthMode mode) {
+    depth_mode_ = mode;
+}
+
+void IbvsController::setDepthZGain(double kp) {
+    depth_z_kp_ = std::max(0.0, kp);
+}
+
+void IbvsController::setVelocityLimit6(const std::array<double, 6>& vmax6) {
+    vmax6_ = vmax6;
+}
+
+void IbvsController::setAlignCameraToVisp(const Eigen::Matrix3d& R_cv) {
+    R_cv_ = R_cv;
+}
+
+void IbvsController::setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf) {
+    R_camera_urdf_ = R_camera_urdf;
+}
+
+void IbvsController::updateDepthControlPointInTag() {
+    vpRotationMatrix R_des;
+    R_des.buildFrom(target_rx_, target_ry_, target_rz_);
+
+    // 反解深度控制点（tag 平面点）:
+    // 设控制点 p_o = [u, v, 0]^T，期望位姿为 (R_des, t_des)。
+    // 在相机系:
+    //   p_c = R_des * p_o + t_des
+    // 令控制点在期望时落在光轴上（x_c = 0, y_c = 0）:
+    //   [R00 R01] [u] = -[tx]
+    //   [R10 R11] [v]   [ty]
+    // 即 A * [u v]^T = b。
+    Eigen::Matrix2d A;
+    A << R_des[0][0], R_des[0][1],
+         R_des[1][0], R_des[1][1];
+    const Eigen::Vector2d b(-target_x_, -target_y_);
+
+    if (std::abs(A.determinant()) < 1e-9) {
+        // 退化时回退到 tag 中心。
+        depth_control_point_tag_.setZero();
+        return;
+    }
+    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_);
+}
+
+void IbvsController::initTask() {
+    task_ = std::make_unique<vpServo>();
+    task_->setServo(vpServo::EYEINHAND_CAMERA);
+    task_->setInteractionMatrixType(vpServo::CURRENT);
+    task_->setLambda(lambda_);
+
+    obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
+    obj_pts_[1].setWorldCoordinates(tag_half_, -tag_half_, 0.0);
+    obj_pts_[2].setWorldCoordinates(tag_half_, tag_half_, 0.0);
+    obj_pts_[3].setWorldCoordinates(-tag_half_, tag_half_, 0.0);
+
+    vpTranslationVector t_des(target_x_, target_y_, target_z_);
+    vpRotationMatrix R_des;
+    R_des.buildFrom(target_rx_, target_ry_, target_rz_);
+    // cMo_des: 期望的 object(tag) 相对 camera 位姿。
+    vpHomogeneousMatrix cMo_des(t_des, R_des);
+
+    for (int i = 0; i < 4; ++i) {
+        obj_pts_[i].track(cMo_des);
+        s_star_[i].buildFrom(obj_pts_[i].get_x(),
+                             obj_pts_[i].get_y(),
+                             obj_pts_[i].get_Z());
+        s_cur_[i].buildFrom(0.0, 0.0, 1.0);
+        task_->addFeature(s_cur_[i], s_star_[i]);
+    }
+}
+
+} // namespace cmvr

cmvr-es/controller/src/pid_controller.cpp

@@ -3,104 +3,74 @@
 //
 
 #include "controller/include/pid_controller.h"
-
-
-#include <algorithm>
 #include <cmath>
 
 namespace cmvr {
 
-
+PidController::PidController() {
-PidController::PidController() = default;
+    // 默认构造函数，初始化成员变量
-
+    init();
-void PidController::init(int dof)
-{
-    dof_ = dof;
-
-    kp_.setZero(dof_);
-    ki_.setZero(dof_);
-    kd_.setZero(dof_);
-
-    q_d_.setZero(dof_);
-    dq_d_.setZero(dof_);
-
-    integralError_.setZero(dof_);
-    prevError_.setZero(dof_);
-
-    // 默认：不启用积分限幅（upper <= lower 视为禁用）
-    integralLowerLimit_.setConstant(dof_,  1.0);
-    integralUpperLimit_.setConstant(dof_, -1.0);
-
-    // 默认：无死区
-    deadzone_.setZero(dof_);
-
-    // 默认：不启用 D 滤波
-    dFilterCoeff_ = 0.0;
-    dErrorFiltered_.setZero(dof_);
 }
 
-void PidController::setGains(const Eigen::VectorXd& kp,
+void PidController::init() {
-                             const Eigen::VectorXd& ki,
+    // 初始化 PID 控制器
-                             const Eigen::VectorXd& kd)
+    kp_ = 0.0;
-{
+    ki_ = 0.0;
+    kd_ = 0.0;
+
+    integralError_ = 0.0;
+    prevError_ = 0.0;
+
+    integralLowerLimit_ = -1.0;
+    integralUpperLimit_ = 1.0;
+
+    deadzone_ = 0.0;
+
+    dFilterCoeff_ = 0.0;
+    dErrorFiltered_ = 0.0;
+
+    outputLowerLimit_ = -std::numeric_limits<double>::infinity();
+    outputUpperLimit_ = std::numeric_limits<double>::infinity();
+}
+
+void PidController::setGains(double kp, double ki, double kd) {
     kp_ = kp;
     ki_ = ki;
     kd_ = kd;
 }
 
-void PidController::setIntegralLimits(const Eigen::VectorXd& lower,
+void PidController::setIntegralLimits(double lower, double upper) {
-                                      const Eigen::VectorXd& upper)
-{
     integralLowerLimit_ = lower;
     integralUpperLimit_ = upper;
 }
 
-void PidController::setDeadzone(const Eigen::VectorXd& deadzone)
+void PidController::setDeadzone(double deadzone) {
-{
     deadzone_ = deadzone;
 }
 
-void PidController::setDerivativeFilterCoeff(double coeff)
+void PidController::setDerivativeFilterCoeff(double coeff) {
-{
+    dFilterCoeff_ = coeff; // <= 0 时在 compute 里视为不开滤波
-    dFilterCoeff_ = coeff; // <= EPS 时在 compute 里视为不开滤波
 }
 
-void PidController::setReference(const ControlReference& ref)
+void PidController::setOutputLimits(double lower, double upper) {
-{
+    outputLowerLimit_ = lower;
-    q_d_ = ref.q_d;
+    outputUpperLimit_ = upper;
-
-    if (ref.dq_d.size() == q_d_.size())
-        dq_d_ = ref.dq_d;
-    else
-        dq_d_.setZero(dof_);
 }
 
-Eigen::VectorXd PidController::compute(const ControlInput& input, double dt)
+double PidController::compute(double target, double current, double dt) {
-{
+    // 计算误差
-    const Eigen::VectorXd& q  = input.q;
+    double error = target - current;
-    const Eigen::VectorXd& dq = input.dq;
-
-    Eigen::VectorXd error = q_d_ - q;
 
     // ===== 1. 误差死区（|e| < deadzone => e = 0） =====
-    for (int i = 0; i < dof_; ++i) {
+    if (std::abs(error) < deadzone_) {
-        double dz = (i < deadzone_.size()) ? deadzone_[i] : 0.0;
+        error = 0.0;
-        if (dz > EPS && std::abs(error[i]) < dz) {
-            error[i] = 0.0;
-        }
     }
 
     // ===== 2. 微分项 + 一阶滤波 =====
-    Eigen::VectorXd dErrorRaw(dof_);
+    double dErrorRaw = (error - prevError_) / dt;
-    Eigen::VectorXd dError(dof_);
+    double dError = 0.0;
 
-    if (dt > EPS) {
+    if (dFilterCoeff_ > 0.0 && dt > 0.0) {
-        dErrorRaw = (error - prevError_) / dt;
-    } else {
-        dErrorRaw.setZero();
-    }
-
-    if (dFilterCoeff_ > EPS && dt > EPS) {
         // 一阶离散滤波：alpha 越接近 1，越接近原始微分
         double alpha = dFilterCoeff_ / (dFilterCoeff_ + dt);
         dErrorFiltered_ = alpha * dErrorFiltered_ + (1.0 - alpha) * dErrorRaw;
@@ -112,27 +82,22 @@ Eigen::VectorXd PidController::compute(const ControlInput& input, double dt)
     // ===== 3. 积分项 + 上下限 =====
     integralError_ += error * dt;
 
-    for (int i = 0; i < dof_; ++i) {
+    // 处理积分限幅
-        double lower = (i < integralLowerLimit_.size()) ? integralLowerLimit_[i] : 1.0;
+    if (integralError_ > integralUpperLimit_) integralError_ = integralUpperLimit_;
-        double upper = (i < integralUpperLimit_.size()) ? integralUpperLimit_[i] : -1.0;
+    if (integralError_ < integralLowerLimit_) integralError_ = integralLowerLimit_;
-
-        // upper <= lower (+EPS 容差) => 不启用积分限幅
-        if (upper - lower > EPS) {
-            if (integralError_[i] > upper) integralError_[i] = upper;
-            if (integralError_[i] < lower) integralError_[i] = lower;
-        }
-    }
 
+    // 保存上一拍误差
     prevError_ = error;
 
     // ===== 4. PID 输出（约定为关节力矩） =====
-    Eigen::VectorXd tau =
+    // 计算控制量
-        kp_.cwiseProduct(error) +
+    double tau = kp_ * error + ki_ * integralError_ + kd_ * dError;
-        ki_.cwiseProduct(integralError_) +
+
-        kd_.cwiseProduct(dError);
+    // ===== 5. 输出限幅 =====
+    if (tau > outputUpperLimit_) tau = outputUpperLimit_;
+    if (tau < outputLowerLimit_) tau = outputLowerLimit_;
 
     return tau;
 }
 
 } // namespace cmvr
-

cmvr-es/devices/camera/CMakeLists.txt

@@ -1,3 +1,4 @@
 #add_subdirectory(mechmind)
 add_subdirectory(uvc_camera)
 add_subdirectory(realsense_camera)
+add_subdirectory(mujoco_camera)

cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt

@@ -0,0 +1,11 @@
+find_package(OpenCV REQUIRED)
+
+add_library(mujoco_camera SHARED src/mujoco_camera.cpp)
+
+target_include_directories(mujoco_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(mujoco_camera PUBLIC ${OpenCV_LIBS})
+
+add_library(cmvr_es::device::mujoco_camera ALIAS mujoco_camera)
+
+install(TARGETS mujoco_camera LIBRARY DESTINATION lib)

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

@@ -0,0 +1,47 @@
+//
+// Created by lgv on 2026/2/26.
+//
+
+#pragma once
+
+#include <cstdint>
+#include <functional>
+#include <mutex>
+#include <vector>
+
+#include "devices/camera/abstract_camera.h"
+
+namespace cmvr::device {
+
+class MujocoCamera final : public AbstractCamera {
+public:
+    using FetchRgbdFn = std::function<bool(std::vector<unsigned char>& rgb,
+                                           std::vector<float>& depth,
+                                           int& width,
+                                           int& height,
+                                           uint64_t& frame_id)>;
+
+    explicit MujocoCamera(FetchRgbdFn fetch_rgbd_fn);
+    ~MujocoCamera() override = default;
+
+    void setFovyDeg(double fovy_deg);
+    void setConsumeNewFrameOnly(bool enable);
+
+    void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
+    void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
+    void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
+
+private:
+    bool fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
+    void fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const;
+
+private:
+    FetchRgbdFn fetch_rgbd_fn_;
+    mutable std::mutex mtx_;
+    double fovy_deg_{60.0};
+    bool consume_new_frame_only_{true};
+    uint64_t last_frame_id_{0};
+    bool has_last_frame_id_{false};
+};
+
+} // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -0,0 +1,98 @@
+//
+// Created by lgv on 2026/2/26.
+//
+
+#include "../include/mujoco_camera.h"
+
+#include <cmath>
+
+namespace cmvr::device {
+
+MujocoCamera::MujocoCamera(FetchRgbdFn fetch_rgbd_fn)
+    : fetch_rgbd_fn_(std::move(fetch_rgbd_fn)) {}
+
+void MujocoCamera::setFovyDeg(double fovy_deg) {
+    std::lock_guard<std::mutex> lock(mtx_);
+    fovy_deg_ = fovy_deg;
+}
+
+void MujocoCamera::setConsumeNewFrameOnly(bool enable) {
+    std::lock_guard<std::mutex> lock(mtx_);
+    consume_new_frame_only_ = enable;
+}
+
+void MujocoCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
+    cv::Mat depth;
+    if (!fetch(color, depth, intrinsics)) {
+        color.release();
+    }
+}
+
+void MujocoCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+    cv::Mat color;
+    if (!fetch(color, depth, intrinsics)) {
+        depth.release();
+    }
+}
+
+void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+    if (!fetch(color, depth, intrinsics)) {
+        color.release();
+        depth.release();
+    }
+}
+
+bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+    std::lock_guard<std::mutex> lock(mtx_);
+    if (!fetch_rgbd_fn_) return false;
+
+    std::vector<unsigned char> rgb_raw;
+    std::vector<float> depth_raw;
+    int width = 0;
+    int height = 0;
+    uint64_t frame_id = 0;
+    if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
+        return false;
+    }
+
+    if (width <= 0 || height <= 0) return false;
+    if ((int)rgb_raw.size() != width * height * 3) return false;
+    if (!depth_raw.empty() && (int)depth_raw.size() != width * height) return false;
+
+    if (consume_new_frame_only_) {
+        if (has_last_frame_id_ && frame_id == last_frame_id_) {
+            return false;
+        }
+    }
+    last_frame_id_ = frame_id;
+    has_last_frame_id_ = true;
+
+    cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
+    color = rgb.clone();
+
+    if (!depth_raw.empty()) {
+        cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
+        depth = dep.clone();
+    } else {
+        depth.release();
+    }
+
+    fillIntrinsics(width, height, intrinsics);
+    return true;
+}
+
+void MujocoCamera::fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const {
+    const double fovy = fovy_deg_ * M_PI / 180.0;
+    const double fy = (height * 0.5) / std::tan(fovy * 0.5);
+    const double fx = fy;
+
+    intrinsics.fx = static_cast<float>(fx);
+    intrinsics.fy = static_cast<float>(fy);
+    intrinsics.cx = static_cast<float>(width * 0.5);
+    intrinsics.cy = static_cast<float>(height * 0.5);
+    for (float& coeff : intrinsics.coeffs) {
+        coeff = 0.0f;
+    }
+}
+
+} // namespace cmvr::device

cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h

@@ -8,6 +8,7 @@
 #include <pinocchio/spatial/se3.hpp>
 
 #include <Eigen/Core>
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
@@ -39,6 +40,12 @@ public:
             Eigen::Matrix4d &cur_pose,
             bool is_tcp = true) override;
 
+    // 指定 base_link 与 ee_link，返回 ee 在 base 下的位姿。
+    bool fk(const std::string& base_link,
+            const std::string& ee_link,
+            const std::vector<double>& joints_angle,
+            Eigen::Matrix4d& cur_pose);
+
     // MoveL：S 曲线（限 jerk）速度规划 + 微分IK（LOCAL）生成 q(t)
     // target_pose_base: base 下目标位姿（只用平移；姿态保持起点姿态）
     bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base,
@@ -52,6 +59,25 @@ public:
                            const std::vector<double>& qd_max, // rad/s, size=chain_dof
                            bool is_tcp = true);
 
+    /**
+     * @brief 用 DLS 微分逆解将末端局部坐标系（如相机系）twist 映射为本链关节速度。
+     *
+     * @param cur_angle 当前关节角，支持 size==chain_dof_ 或 size==model_.nq。
+     * @param ee_velocity 末端期望速度 [vx vy vz wx wy wz]^T，单位 m/s 与 rad/s，坐标系为 ee_frame_name 局部系。
+     * @param joints_vel 输出的关节速度，size=chain_v_dof_，单位 rad/s。
+     * @param ee_link 末端 frame 名称。
+     * @param damping DLS 阻尼系数，<=0 时使用成员 damping_。
+     * @param qdot_abs_max 关节速度绝对值限幅；同时会叠加 URDF velocityLimit。
+     * @return true 计算成功；false 输入非法或 frame 不存在等失败。
+     * @note 函数内部会将 ee 局部 twist 转到 base 坐标系，并在 base 坐标系中完成 DLS 求解。
+     */
+    bool velocityIk(const std::vector<double>& cur_angle,
+                    const Eigen::Matrix<double,6,1>& ee_velocity,
+                    std::vector<double>& joints_vel,
+                    const std::string& ee_link,
+                    double damping = -1.0,
+                    double qdot_abs_max = std::numeric_limits<double>::infinity());
+
     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

@@ -225,16 +225,30 @@ bool PinocchioDlsIKSolver::fk(const std::vector<double> &joints_angle,
                               Eigen::Matrix4d &cur_pose_base,
                               bool is_tcp)
 {
-    if (!initialized_) return false;
+    const std::string &ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : flange_frame_name_;
+    return fk(base_frame_name_, ee_link, joints_angle, cur_pose_base);
+}
 
-    const int size = (int)joints_angle.size();
+bool PinocchioDlsIKSolver::fk(const std::string& base_link,
-    if (size != chain_dof_ && size != model_.nq) {
+                              const std::string& ee_link,
-        std::cerr << "[PinocchioDlsIKSolver] FK joints size mismatch\n";
+                              const std::vector<double>& joints_angle,
+                              Eigen::Matrix4d& cur_pose)
+{
+    if (!initialized_) return false;
+    if (!model_.existFrame(base_link)) {
+        std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_link << "\n";
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        std::cerr << "[PinocchioDlsIKSolver] ee frame not found: " << ee_link << "\n";
         return false;
     }
 
-    pinocchio::FrameIndex target_frame_id =
+    const int size = (int)joints_angle.size();
-        (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
+    if (size != chain_dof_ && size != model_.nq) {
+        std::cerr << "[PinocchioDlsIKSolver] fk(base,ee) joints size mismatch\n";
+        return false;
+    }
 
     Eigen::VectorXd q_full = pinocchio::neutral(model_);
     if (size == model_.nq) {
@@ -247,10 +261,103 @@ bool PinocchioDlsIKSolver::fk(const std::vector<double> &joints_angle,
     pinocchio::forwardKinematics(model_, *data_, q_full);
     pinocchio::updateFramePlacements(model_, *data_);
 
+    const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
+    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+    const pinocchio::SE3 &oM_base = data_->oMf[base_id];
+    const pinocchio::SE3 &oM_ee = data_->oMf[ee_id];
+    pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    cur_pose = se3ToMatrix4(base_M_ee);
+    return true;
+}
+
+bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
+                                      const Eigen::Matrix<double,6,1>& ee_velocity,
+                                      std::vector<double>& joints_vel,
+                                      const std::string& ee_link,
+                                      double damping,
+                                      double qdot_abs_max)
+{
+    // 1) 基本状态与输入合法性检查。
+    if (!initialized_) return false;
+    if (ee_link.empty()) {
+        std::cerr << "[PinocchioDlsIKSolver] ee_frame_name is empty\n";
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        std::cerr << "[PinocchioDlsIKSolver] frame not found: " << ee_link << "\n";
+        return false;
+    }
+
+    const int size = (int)cur_angle.size();
+    if (size != chain_dof_ && size != model_.nq) {
+        std::cerr << "[PinocchioDlsIKSolver] velocityIk joints size mismatch\n";
+        return false;
+    }
+    if (chain_v_dof_ <= 0) {
+        std::cerr << "[PinocchioDlsIKSolver] invalid chain_v_dof\n";
+        return false;
+    }
+
+    // 2) 组装整机 q（Pinocchio 统一在 full model 上做 FK/Jacobian）。
+    Eigen::VectorXd q_full = pinocchio::neutral(model_);
+    if (size == model_.nq) {
+        q_full = Eigen::Map<const Eigen::VectorXd>(cur_angle.data(), model_.nq);
+    } else {
+        Eigen::Map<const Eigen::VectorXd> q_chain(cur_angle.data(), chain_dof_);
+        q_full.segment(chain_q_start_, chain_dof_) = q_chain;
+    }
+
+    // 3) 在当前关节角下更新位姿。
+    pinocchio::forwardKinematics(model_, *data_, q_full);
+    pinocchio::updateFramePlacements(model_, *data_);
+
+    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+
     const pinocchio::SE3 &oM_base = base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
-    const pinocchio::SE3 &oM_cur  = data_->oMf[target_frame_id];
+    const pinocchio::SE3 &oM_ee   = data_->oMf[ee_id];
-    pinocchio::SE3 base_M_cur = oM_base.inverse() * oM_cur;
+
-    cur_pose_base = se3ToMatrix4(base_M_cur);
+    // 4) ee 局部系 twist -> base 系 twist。
+    const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    const Eigen::Matrix3d R_be = base_M_ee.rotation(); // ee -> base
+    Eigen::Matrix<double,6,1> twist_base;
+    twist_base.head<3>() = R_be * ee_velocity.head<3>();
+    twist_base.tail<3>() = R_be * ee_velocity.tail<3>();
+
+    // 5) 计算 Jacobian，并从 world 表达转到 base 表达。
+    Eigen::Matrix<double,6,Eigen::Dynamic> J_world(6, model_.nv);
+    pinocchio::computeFrameJacobian(model_, *data_, q_full,
+                                    ee_id,
+                                    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+                                    J_world);
+    Eigen::MatrixXd J = J_world.middleCols(chain_v_start_, chain_v_dof_);
+    const Eigen::Matrix3d R_bo = oM_base.rotation().transpose(); // world -> base
+    J.topRows(3) = R_bo * J.topRows(3);
+    J.bottomRows(3) = R_bo * J.bottomRows(3);
+
+    // 6) DLS 逆解：qdot = J^T (J J^T + lambda^2 I)^-1 * twist_base。
+    const double lambda = (damping > 0.0) ? damping : damping_;
+    Eigen::Matrix<double,6,6> A = J * J.transpose();
+    A.diagonal().array() += (lambda * lambda);
+    Eigen::VectorXd qdot = J.transpose() * A.ldlt().solve(twist_base);
+
+    // 7) 关节速度限幅：取调用者限幅与 URDF velocityLimit 的更严格值。
+    joints_vel.resize(chain_v_dof_);
+    for (int i = 0; i < chain_v_dof_; ++i) {
+        double vel_limit = qdot_abs_max;
+        if (model_.velocityLimit.size() == model_.nv) {
+            const int v_idx = chain_v_start_ + i;
+            if (v_idx >= 0 && v_idx < model_.nv) {
+                vel_limit = std::min(vel_limit, std::abs(model_.velocityLimit[v_idx]));
+            }
+        }
+
+        double qi = qdot[i];
+        if (std::isfinite(vel_limit) && vel_limit > 0.0) {
+            qi = clampd(qi, -vel_limit, vel_limit);
+        }
+        joints_vel[i] = qi;
+    }
+
     return true;
 }
 

cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h

@@ -40,7 +40,7 @@ namespace cmvr {
                              int width,
                              int height);
         void disablePiPCamera();
-        // 获取 PiP 相机 RGB+Depth（Depth 是 OpenGL z-buffer 0..1）
+        // 获取 PiP 相机 RGB+Depth（Depth 已线性化为米）
         // depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
         bool getPiPCameraRGBD(std::vector<unsigned char> &rgb,
                               std::vector<float> &depth,

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp

@@ -7,6 +7,7 @@
 #include <cstdio>
 #include <chrono>
 #include <cmath>
+#include <limits>
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 #include "simulate/mujoco/mujoco_viewer/include/array_safety.h"
 #include "simulate/mujoco/mujoco_viewer/include/glfw_adapter.h"
@@ -283,6 +284,28 @@ namespace cmvr {
                     std::swap_ranges(top_d, top_d + w, bot_d);
                 }
 
+                // 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。
+                const double znear = static_cast<double>(m_->vis.map.znear) * static_cast<double>(m_->stat.extent);
+                const double zfar  = static_cast<double>(m_->vis.map.zfar)  * static_cast<double>(m_->stat.extent);
+                if (znear > 0.0 && zfar > znear) {
+                    const double two_nf = 2.0 * znear * zfar;
+                    const double f_plus_n = zfar + znear;
+                    const double f_minus_n = zfar - znear;
+                    for (float &d : pip_depth_) {
+                        if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
+                            d = std::numeric_limits<float>::infinity();
+                            continue;
+                        }
+                        const double z_ndc = 2.0 * static_cast<double>(d) - 1.0; // [-1,1]
+                        const double denom = f_plus_n - z_ndc * f_minus_n;
+                        if (denom <= 1e-12) {
+                            d = std::numeric_limits<float>::infinity();
+                            continue;
+                        }
+                        d = static_cast<float>(two_nf / denom);
+                    }
+                }
+
                 pip_rgb_width_ = w;
                 pip_rgb_height_ = h;
                 pip_rgb_valid_ = true;

model/xiaoyan_description/dual_arm.xml

@@ -67,20 +67,22 @@
 
   <worldbody>
     <body name="tag_board" pos="0.7 -0.2 1.1" euler="1.57 -1.57 0">
-      <!-- 12cm x 12cm，厚1mm；contype=0 防止碰撞干扰 -->
+
-      <geom name="tag_geom"
+      <!-- 15cm x 15cm 白板，同时贴 apriltag 纹理（纹理里自带白边） -->
+      <geom name="tag_board_geom"
             type="box"
-            size="0.06 0.06 0.001"
+            size="0.075 0.075 0.0005"
             material="apriltag_mat"
-            contype="0" conaffinity="0"
+            rgba="1 1 1 1"
-            rgba="1 1 1 1"/>
+            contype="0" conaffinity="0"/>
     </body>
 
     <geom size="0.05 0.6" pos="0 0 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
     <geom size="0.05 0.6" pos="0 0 0.6" type="cylinder"/>
-    <geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <body name="PELVIS_S" pos="0 0 1.2">
-    <geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+      <geom pos="0 0 0" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
-    <body name="L_SHOULDER_P_S" pos="0 0.0945 1.242">
+      <geom pos="0 0 0" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <body name="L_SHOULDER_P_S" pos="0 0.0945 0.042">
       <inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
       <joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-1.57 0.26" actuatorfrcrange="-120 120"/>
       <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
@@ -122,7 +124,7 @@
         </body>
       </body>
     </body>
-    <body name="R_SHOULDER_P_S" pos="0 -0.0945 1.242">
+    <body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
       <inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
       <joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-120 120"/>
       <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
@@ -178,7 +180,7 @@
                   <!-- 真正可渲染相机：用这个名字去 mj_name2id(m, mjOBJ_CAMERA,"hand_cam") -->
                   <camera name="hand_cam"
                           pos="-0.01212 -0.17655 0.07506"
-                          euler="-1.5707963267 0 3.1415926535"
+                          quat="3.17467e-11 -3.17467e-11 0.707107 0.707107"
                           fovy="60"/>
 
                   <!-- ====== 原来的几何体（保持不变）====== -->
@@ -207,6 +209,7 @@
         </body>
       </body>
     </body>
+    </body>
 
     <light name="top_light"
            mode="track"