diff --git a/cmvr-es/common/utils/image/image_process.h b/cmvr-es/common/utils/image/image_process.h new file mode 100644 index 00000000..03e9dce4 --- /dev/null +++ b/cmvr-es/common/utils/image/image_process.h @@ -0,0 +1,60 @@ +// +// Created by lgv on 2026/2/26. +// + +#pragma once + +#include +#include +#include + +#include + +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 valid; + valid.reserve(static_cast((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(depth.at(vv, uu)); + } else if (depth.type() == CV_16UC1) { + z = static_cast(depth.at(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 + diff --git a/cmvr-es/controller/CMakeLists.txt b/cmvr-es/controller/CMakeLists.txt index 34d7c50a..d11e0bc7 100644 --- a/cmvr-es/controller/CMakeLists.txt +++ b/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,10 +57,12 @@ 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 glog ${VISP_LIBRARIES} realsense2::realsense2 -) \ No newline at end of file +) diff --git a/cmvr-es/controller/include/controller.h b/cmvr-es/controller/include/controller.h deleted file mode 100644 index f1d2ba41..00000000 --- a/cmvr-es/controller/include/controller.h +++ /dev/null @@ -1,81 +0,0 @@ -// -// Created by lgv on 11/27/25. -// - -#pragma once - -#include -#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 - - - diff --git a/cmvr-es/controller/include/controller_creator.h b/cmvr-es/controller/include/controller_creator.h deleted file mode 100644 index 976d70c4..00000000 --- a/cmvr-es/controller/include/controller_creator.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h new file mode 100644 index 00000000..7997cf06 --- /dev/null +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -0,0 +1,300 @@ +// +// Created by lgv on 2026/2/26. +// + +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#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& 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& q_init = {}); + + /** + * @brief 根据当前图像与关节角计算下一拍关节位置命令。 + * @param joints_angle 当前关节角。 + * @param dt 控制周期（秒）。 + * @param q_cmd_out 输出的下一拍关节位置命令。 + * @return 计算成功返回 `true`。 + */ + bool compute(const std::vector& joints_angle, + double dt, + std::vector& q_cmd_out); + /** + * @brief 根据当前图像与关节角计算目标关节速度命令。 + * @param joints_angle 当前关节角。 + * @param qdot_out 输出的关节速度命令（rad/s）。 + * @return 计算成功返回 `true`。 + */ + bool compute(const std::vector& joints_angle, + std::vector& 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& 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& 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& joints_angle, + std::vector& qdot_out); + /** + * @brief 根据当前目标位姿反解 tag 平面的深度控制点。 + */ + void updateDepthControlPointInTag(); + /** + * @brief 重新构建 ViSP 任务与期望特征。 + */ + void initTask(); + +private: + // 是否初始化成功。 + bool initialized_{false}; + // 速度 IK 使用的末端相机 frame 名称。 + std::string camera_frame_name_; + // 相机对象。 + std::shared_ptr 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 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 task_{nullptr}; + // tag 四角点（3D）。 + vpPoint obj_pts_[4]; + // 当前特征。 + vpFeaturePoint s_cur_[4]; + // 目标特征。 + vpFeaturePoint s_star_[4]; + // AprilTag 检测器。 + vpDetectorAprilTag detector_; + + // 速度 IK 求解器。 + std::unique_ptr dls_solver_{nullptr}; + + // 内部积分得到的关节位置命令缓存。 + std::vector 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 last_v_camera_visp_{Eigen::Matrix::Zero()}; +}; + +} // namespace cmvr diff --git a/cmvr-es/controller/include/pid_controller.h b/cmvr-es/controller/include/pid_controller.h index e9e44e0e..4a67e62f 100644 --- a/cmvr-es/controller/include/pid_controller.h +++ b/cmvr-es/controller/include/pid_controller.h @@ -4,68 +4,61 @@ #pragma once -#include "controller/include/controller.h" -#include - namespace cmvr { - class PidController : public Controller { + class PidController { public: PidController(); - // 设置自由度+ 初始化 - void init(int dof) override; + // 初始化 PID 控制器 + void init(); - // 设置目标（只用到 q_d, dq_d） - void setReference(const ControlReference& ref) override; + // 核心：根据当前状态计算控制量 + double compute(double target, double current, double dt); - // 核心：根据当前状态计算控制量（约定为关节力矩） - Eigen::VectorXd compute(const ControlInput& input, double dt) override; + // PID 增益 + void setGains(double kp, double ki, double kd); - // PID 三向量增益 - void setGains(const Eigen::VectorXd& kp, - const Eigen::VectorXd& ki, - const Eigen::VectorXd& kd) override; + /// 设置积分上下限： lower <= I <= upper + void setIntegralLimits(double lower, double upper); - /// 设置积分上下限： lower[i] <= I[i] <= upper[i] - /// upper[i] <= lower[i] 表示该关节不启用积分限幅 - void setIntegralLimits(const Eigen::VectorXd& lower, - const Eigen::VectorXd& upper); - - /// 设置误差死区： |error[i]| < deadzone[i] 时视为 0 - void setDeadzone(const Eigen::VectorXd& deadzone); + /// 设置误差死区： |error| < deadzone 时视为 0 + void setDeadzone(double deadzone); /// 设置 D 项滤波系数（简单一阶滤波） /// coeff <= 0 表示不启用滤波 void setDerivativeFilterCoeff(double coeff); - private: - int dof_ = 0; + /// 设置输出的上下限 + void setOutputLimits(double lower, double upper); + private: // PID 增益 - Eigen::VectorXd kp_; - Eigen::VectorXd ki_; - Eigen::VectorXd kd_; + double kp_; + double ki_; + double kd_; // 期望状态 - Eigen::VectorXd q_d_; - Eigen::VectorXd dq_d_; + double q_d_; // 只关心目标位置，不需要目标速度 // 积分项、上一拍误差 - Eigen::VectorXd integralError_; - Eigen::VectorXd prevError_; + double integralError_; + double prevError_; // 积分上下限 - Eigen::VectorXd integralLowerLimit_; - Eigen::VectorXd integralUpperLimit_; + double integralLowerLimit_; + double integralUpperLimit_; // 死区 - Eigen::VectorXd deadzone_; + double deadzone_; // D 项滤波 - double dFilterCoeff_ = 0.0; ///< 滤波系数，0 表示不开滤波 - Eigen::VectorXd dErrorFiltered_; ///< 滤波后的 dError + double dFilterCoeff_; ///< 滤波系数，0 表示不开滤波 + double dErrorFiltered_; ///< 滤波后的 dError + + // 输出限制 + double outputLowerLimit_; + double outputUpperLimit_; }; -} // namespace cmvr - +} // namespace cmvr \ No newline at end of file diff --git a/cmvr-es/controller/src/controller_creator.cpp b/cmvr-es/controller/src/controller_creator.cpp deleted file mode 100644 index a5de345d..00000000 --- a/cmvr-es/controller/src/controller_creator.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// -// Created by lgv on 11/27/25. -// - -#include "../include/controller_creator.h" diff --git a/cmvr-es/controller/src/controller_test.cpp b/cmvr-es/controller/src/controller_test.cpp index 902f584d..8d2cc4eb 100644 --- a/cmvr-es/controller/src/controller_test.cpp +++ b/cmvr-es/controller/src/controller_test.cpp @@ -8,13 +8,17 @@ #include #include #include +#include +#include #include +#include #include #include -#include -#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", @@ -482,21 +494,18 @@ protected: for (int i = 0; i < 7; ++i) { act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]); - jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]); + jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]); if (act_ids_[i] < 0) { std::cout << "[IBVS] Cannot find actuator " << act_names[i] << std::endl; } 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( + [this](std::vector& rgb, + std::vector& 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 }; + 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相机系） - // 这就是你之前验证过能收敛的那套：x/y 同时翻转 - R_cv_ = (Eigen::Matrix3d() << + ibvs_controller_ = std::make_unique(); + ibvs_controller_->setMu(mu_); + 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(); + 0, -1, 0, + 0, 0, -1).finished(); + ibvs_controller_->setAlignCameraToVisp(R_align); + ibvs_controller_->setAlignCameraToUrdf(R_align); - // 7) IBVS 任务：用 AprilTag 的 4 个角点特征 - tag_size_m_ = 0.12; // 12cm - tag_half_ = tag_size_m_ * 0.5; - Z_des_ = 0.28; - 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]); - } + if (!ibvs_controller_->init(mujoco_camera_, urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) { + std::cout << "[IBVS] IbvsController init failed" << std::endl; + ready_ = false; + return; } - // 8) AprilTag detector - detector_ = vpDetectorAprilTag(vpDetectorAprilTag::TAG_36h11); - detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); - - // 初始化 q_cmd，防跳变 + std::vector q_init(7, 0.0); 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 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; } - // ====================== - // B) IBVS（用 MuJoCo 相机图像） - // ====================== - - // 1) 取最新 PiP 图像（RGB + depth z-buffer） - std::vector rgb; - std::vector 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 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(0.299*r + 0.587*g + 0.114*b); - } + std::vector q_now(7, 0.0); + for (int i = 0; i < 7; ++i) { + q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0; } - // 4) AprilTag 检测 + 位姿估计（直接得到 cMo） - std::vector cMo_vec; - bool ok = detector_.detect(I, tag_size_m_, cam, cMo_vec); + std::vector q_cmd_next; + const bool ok = ibvs_controller_->compute(q_now, dt, q_cmd_next); - if (!ok || cMo_vec.empty()) { - // 没检测到：最简单策略：保持当前位置（不更新 ctrl） + if (!ok) { 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]; - - // 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); + if (q_cmd_next.size() == 7) { + for (int i = 0; i < 7; ++i) { + q_cmd_[i] = q_cmd_next[i]; + } } - // 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 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 jacp(3 * m->nv); - std::vector jacr(3 * m->nv); - mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_); - - Eigen::Matrix 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 A = J * J.transpose(); - A += (mu_*mu_) * Eigen::Matrix::Identity(); - Eigen::Matrix 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; - - // joint limit clamp 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]; + 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 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 act_ids_{}; - std::array jnt_ids_{}; - std::array qpos_adr_{ { -1,-1,-1,-1,-1,-1,-1 } }; - std::array dof_adr_{ { -1,-1,-1,-1,-1,-1,-1 } }; + std::array act_ids_{}; + std::array jnt_ids_{}; + std::array qpos_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 q_home_{ {0,0,0,0,0,0,0} }; + std::array 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 - Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()}; + double mu_{0.02}; + double qdot_max_{0.6}; + std::array vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; - // IBVS params - double lambda_{0.7}; - double tag_size_m_{0.12}; - double tag_half_{0.06}; - double Z_des_{0.60}; + bool check_twist_to_qdot_{true}; + double qdot_check_tol_{1e-3}; + double qdot_check_max_err_{0.0}; + int qdot_check_samples_{0}; + 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 - double qdot_max_{0.6}; // rad/s - double vmax6_[6] = {0.15, 0.15, 0.20, 0.6, 0.6, 0.6}; + std::unique_ptr ibvs_controller_{nullptr}; + std::shared_ptr mujoco_camera_{nullptr}; - // ViSP - vpServo task_; - vpPoint obj_pts_[4]; - vpFeaturePoint s_cur_[4]; - vpFeaturePoint s_star_[4]; - vpDetectorAprilTag detector_; - - // control state - std::array q_cmd_{ {0,0,0,0,0,0,0} }; + std::array 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(); + // } } diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp new file mode 100644 index 00000000..ed8f1fca --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include + +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& 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( + 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& 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& joints_angle, + std::vector& 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(intrinsics.fx); + const double fy = static_cast(intrinsics.fy); + const double cx = static_cast(intrinsics.cx); + const double cy = static_cast(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 I(height, width); + for (int y = 0; y < height; ++y) { + std::memcpy(I[y], gray.ptr(y), static_cast(width)); + } + + std::vector 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(std::lround(fx * x_depth_ctrl + cx)); + const int v = static_cast(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 twist_ee; + twist_ee << v_site(0), v_site(1), v_site(2), w_site(0), w_site(1), w_site(2); + + Eigen::Matrix 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 qdot; + const bool ok = dls_solver_->velocityIk( + joints_angle, twist_ee_pin, qdot, camera_frame_name_, mu_, std::numeric_limits::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& joints_angle, + double dt, + std::vector& 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 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& joints_angle, + std::vector& 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& 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(); + 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 diff --git a/cmvr-es/controller/src/pid_controller.cpp b/cmvr-es/controller/src/pid_controller.cpp index f577bc2e..eb0cc0b0 100644 --- a/cmvr-es/controller/src/pid_controller.cpp +++ b/cmvr-es/controller/src/pid_controller.cpp @@ -3,104 +3,74 @@ // #include "controller/include/pid_controller.h" - - -#include #include namespace cmvr { - -PidController::PidController() = default; - -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_); +PidController::PidController() { + // 默认构造函数，初始化成员变量 + init(); } -void PidController::setGains(const Eigen::VectorXd& kp, - const Eigen::VectorXd& ki, - const Eigen::VectorXd& kd) -{ +void PidController::init() { + // 初始化 PID 控制器 + 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::infinity(); + outputUpperLimit_ = std::numeric_limits::infinity(); +} + +void PidController::setGains(double kp, double ki, double kd) { kp_ = kp; ki_ = ki; kd_ = kd; } -void PidController::setIntegralLimits(const Eigen::VectorXd& lower, - const Eigen::VectorXd& upper) -{ +void PidController::setIntegralLimits(double lower, double upper) { integralLowerLimit_ = lower; integralUpperLimit_ = upper; } -void PidController::setDeadzone(const Eigen::VectorXd& deadzone) -{ +void PidController::setDeadzone(double deadzone) { deadzone_ = deadzone; } -void PidController::setDerivativeFilterCoeff(double coeff) -{ - dFilterCoeff_ = coeff; // <= EPS 时在 compute 里视为不开滤波 +void PidController::setDerivativeFilterCoeff(double coeff) { + dFilterCoeff_ = coeff; // <= 0 时在 compute 里视为不开滤波 } -void PidController::setReference(const ControlReference& ref) -{ - q_d_ = ref.q_d; - - if (ref.dq_d.size() == q_d_.size()) - dq_d_ = ref.dq_d; - else - dq_d_.setZero(dof_); +void PidController::setOutputLimits(double lower, double upper) { + outputLowerLimit_ = lower; + outputUpperLimit_ = upper; } -Eigen::VectorXd PidController::compute(const ControlInput& input, double dt) -{ - const Eigen::VectorXd& q = input.q; - const Eigen::VectorXd& dq = input.dq; - - Eigen::VectorXd error = q_d_ - q; +double PidController::compute(double target, double current, double dt) { + // 计算误差 + double error = target - current; // ===== 1. 误差死区（|e| < deadzone => e = 0） ===== - for (int i = 0; i < dof_; ++i) { - double dz = (i < deadzone_.size()) ? deadzone_[i] : 0.0; - if (dz > EPS && std::abs(error[i]) < dz) { - error[i] = 0.0; - } + if (std::abs(error) < deadzone_) { + error = 0.0; } // ===== 2. 微分项 + 一阶滤波 ===== - Eigen::VectorXd dErrorRaw(dof_); - Eigen::VectorXd dError(dof_); + double dErrorRaw = (error - prevError_) / dt; + double dError = 0.0; - if (dt > EPS) { - dErrorRaw = (error - prevError_) / dt; - } else { - dErrorRaw.setZero(); - } - - if (dFilterCoeff_ > EPS && dt > EPS) { + if (dFilterCoeff_ > 0.0 && dt > 0.0) { // 一阶离散滤波：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; - double upper = (i < integralUpperLimit_.size()) ? integralUpperLimit_[i] : -1.0; - - // upper <= lower (+EPS 容差) => 不启用积分限幅 - if (upper - lower > EPS) { - if (integralError_[i] > upper) integralError_[i] = upper; - if (integralError_[i] < lower) integralError_[i] = lower; - } - } + // 处理积分限幅 + if (integralError_ > integralUpperLimit_) integralError_ = integralUpperLimit_; + if (integralError_ < integralLowerLimit_) integralError_ = integralLowerLimit_; + // 保存上一拍误差 prevError_ = error; // ===== 4. PID 输出（约定为关节力矩） ===== - Eigen::VectorXd tau = - kp_.cwiseProduct(error) + - ki_.cwiseProduct(integralError_) + - kd_.cwiseProduct(dError); + // 计算控制量 + double tau = kp_ * error + ki_ * integralError_ + kd_ * dError; + + // ===== 5. 输出限幅 ===== + if (tau > outputUpperLimit_) tau = outputUpperLimit_; + if (tau < outputLowerLimit_) tau = outputLowerLimit_; return tau; } } // namespace cmvr - diff --git a/cmvr-es/devices/camera/CMakeLists.txt b/cmvr-es/devices/camera/CMakeLists.txt index 19a56cc0..30677cdd 100644 --- a/cmvr-es/devices/camera/CMakeLists.txt +++ b/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) diff --git a/cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt b/cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt new file mode 100644 index 00000000..a78953f4 --- /dev/null +++ b/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) diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h new file mode 100644 index 00000000..fe0076d0 --- /dev/null +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -0,0 +1,47 @@ +// +// Created by lgv on 2026/2/26. +// + +#pragma once + +#include +#include +#include +#include + +#include "devices/camera/abstract_camera.h" + +namespace cmvr::device { + +class MujocoCamera final : public AbstractCamera { +public: + using FetchRgbdFn = std::function& rgb, + std::vector& 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 diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp new file mode 100644 index 00000000..05b44f91 --- /dev/null +++ b/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 + +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 lock(mtx_); + fovy_deg_ = fovy_deg; +} + +void MujocoCamera::setConsumeNewFrameOnly(bool enable) { + std::lock_guard 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 lock(mtx_); + if (!fetch_rgbd_fn_) return false; + + std::vector rgb_raw; + std::vector 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(fx); + intrinsics.fy = static_cast(fy); + intrinsics.cx = static_cast(width * 0.5); + intrinsics.cy = static_cast(height * 0.5); + for (float& coeff : intrinsics.coeffs) { + coeff = 0.0f; + } +} + +} // namespace cmvr::device 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 696b3c80..dafbeb4e 100644 --- a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h +++ b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -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& 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& 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& cur_angle, + const Eigen::Matrix& ee_velocity, + std::vector& joints_vel, + const std::string& ee_link, + double damping = -1.0, + double qdot_abs_max = std::numeric_limits::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; } 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 4467dce7..daf51972 100644 --- a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp +++ b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp @@ -225,16 +225,30 @@ bool PinocchioDlsIKSolver::fk(const std::vector &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(); - if (size != chain_dof_ && size != model_.nq) { - std::cerr << "[PinocchioDlsIKSolver] FK joints size mismatch\n"; +bool PinocchioDlsIKSolver::fk(const std::string& base_link, + const std::string& ee_link, + const std::vector& 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 = - (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_; + const int size = (int)joints_angle.size(); + 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 &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& cur_angle, + const Eigen::Matrix& ee_velocity, + std::vector& 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(cur_angle.data(), model_.nq); + } else { + Eigen::Map 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]; - pinocchio::SE3 base_M_cur = oM_base.inverse() * oM_cur; - cur_pose_base = se3ToMatrix4(base_M_cur); + const pinocchio::SE3 &oM_ee = data_->oMf[ee_id]; + + // 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 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 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 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; } diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h index fc64a4ab..7ad37d93 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h +++ b/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 &rgb, std::vector &depth, diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp index fd2b05be..40ccf705 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #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(m_->vis.map.znear) * static_cast(m_->stat.extent); + const double zfar = static_cast(m_->vis.map.zfar) * static_cast(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::infinity(); + continue; + } + const double z_ndc = 2.0 * static_cast(d) - 1.0; // [-1,1] + const double denom = f_plus_n - z_ndc * f_minus_n; + if (denom <= 1e-12) { + d = std::numeric_limits::infinity(); + continue; + } + d = static_cast(two_nf / denom); + } + } + pip_rgb_width_ = w; pip_rgb_height_ = h; pip_rgb_valid_ = true; diff --git a/model/april_tag/tag36_11.png b/model/april_tag/tag36_11.png index 21f23cfd..446911d0 100644 Binary files a/model/april_tag/tag36_11.png and b/model/april_tag/tag36_11.png differ diff --git a/model/xiaoyan_description/dual_arm.xml b/model/xiaoyan_description/dual_arm.xml index 6db2ff15..79a2c3b0 100644 --- a/model/xiaoyan_description/dual_arm.xml +++ b/model/xiaoyan_description/dual_arm.xml @@ -67,20 +67,22 @@ - - + + rgba="1 1 1 1" + contype="0" conaffinity="0"/> - - - + + + + @@ -122,7 +124,7 @@ - + @@ -178,7 +180,7 @@ @@ -207,6 +209,7 @@ +