301 lines
10 KiB
C++
301 lines
10 KiB
C++
//
|
||
// 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
|