SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add ibvs class and test pass

Browse Source
This commit is contained in:
lgv 2026-02-26 14:45:07 +08:00
parent 61bbd33be7
commit b218fdccf8
20 changed files with 1316 additions and 457 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
cmvr-es/common/utils/image/image_process.h Normal file
View File

@ -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

11
cmvr-es/controller/CMakeLists.txt
View File

@ -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

81
cmvr-es/controller/include/controller.h
View File

@ -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

13
cmvr-es/controller/include/controller_creator.h
View File

@ -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

300
cmvr-es/controller/include/ibvs_controller.h Normal file
View File

@ -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

65
cmvr-es/controller/include/pid_controller.h
View File

@ -4,68 +4,61 @@
#pragma once
#include "controller/include/controller.h"
#include <limits>
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

5
cmvr-es/controller/src/controller_creator.cpp
View File

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

330
cmvr-es/controller/src/controller_test.cpp
View File

@ -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 };
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<IbvsController>();
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, -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<double> 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<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;
}
// ======================
// B) IBVS（用 MuJoCo 相机图像）
// ======================
// 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);
}
std::vector<double> 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<vpHomogeneousMatrix> cMo_vec;
bool ok = detector_.detect(I, tag_size_m_, cam, cMo_vec);
std::vector<double> 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);
}
// 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
if (q_cmd_next.size() == 7) {
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];
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 } };
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}};
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} };
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
Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
double mu_{0.02};
double qdot_max_{0.6};
std::array<double, 6> 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<IbvsController> ibvs_controller_{nullptr};
std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
// 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} };
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();
// }
}

411
cmvr-es/controller/src/ibvs_controller.cpp Normal file
View File

@ -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

135
cmvr-es/controller/src/pid_controller.cpp
View File

@ -3,104 +3,74 @@
//
#include "controller/include/pid_controller.h"
#include <algorithm>
#include <cmath>
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<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,
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

1
cmvr-es/devices/camera/CMakeLists.txt
View File

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

11
cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt Normal file
View File

@ -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)

47
cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h Normal file
View File

@ -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

98
cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp Normal file
View File

@ -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

26
cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h
View File

@ -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; }

125
cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp
View File

@ -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();
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<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 =
(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<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];
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<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;
}

2
cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h
View File

@ -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,

23
cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp
View File

@ -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;

BIN
model/april_tag/tag36_11.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.8 KiB

After

Width:  |  Height:  |  Size: 20 KiB

23
model/xiaoyan_description/dual_arm.xml
View File

@ -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"/>
<geom pos="0 0 1.2" 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 1.242">
<body name="PELVIS_S" pos="0 0 1.2">
<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"/>
<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"