Panthera HT Demo

这是一个基于 Panthera-HT 机械臂的开源实验仓库，主要维护官方 SDK 之外的视觉伺服、手眼标定、目标跟踪、GraspNet 抓取等扩展功能。仓库会持续更新，后续新增的实验脚本、标定流程和抓取流程都会尽量统一放在这里维护。

当前代码以 Panthera-HT + RealSense D405 + Python 控制为主。SDK 本体在 Panthera-HT_SDK/，视觉伺服和抓取相关功能是基于 SDK 做的应用扩展，不属于 SDK 的基础接口示例。

Directory

Panthera_HT_demo/
├── Panthera-HT_SDK/
│   └── panthera_python/scripts/              # 官方 SDK Python 控制接口和基础示例
├── Panthera-HT_vision_servo_sdk/
│   └── panthera_python/scripts/              # 手眼标定、视觉伺服、蓝球/ArUco 跟踪
└── Panthera-HT_graspnet/
    ├── grasp_app/                            # 鸭子动态抓取: YOLO + PBVS + GraspNet
    ├── graspnet-baseline/                    # GraspNet baseline
    └── weights/                              # 检测模型权重

Main Features

• Panthera-HT Python SDK 调用和基础运动控制。
• RealSense D405 Eye-in-Hand 手眼标定。
• 蓝色小球检测、跟随、IBVS/3D 视觉伺服。
• 单 ArUco marker 连续跟踪，使用 PBVS 位姿误差和速度雅可比控制。
• 鸭子动态抓取流程：YOLO 检测、视觉伺服跟踪、GraspNet 抓取规划、夹爪执行。
• 所有视觉伺服和抓取脚本统一使用同一份手眼标定文件：

Panthera-HT_vision_servo_sdk/panthera_python/scripts/hand_eye_calibration.json

Hardware

当前主要验证硬件：

• Panthera-HT 六轴机械臂
• RealSense D405 深度相机
• Eye-in-Hand 安装方式
• 标定板：默认 8x8 棋盘格，内部角点 7x7，方格尺寸 20mm
• 可选 ArUco marker，用于跟踪或替代棋盘格标定

运行任何会控制机械臂的脚本前，请确认急停可用，工作空间内无人，机械臂不会碰撞桌面、相机线和周围物体。

Environment

推荐根据功能分开环境：

# SDK / 视觉伺服 / 手眼标定
conda activate panthera

# GraspNet / 鸭子抓取
conda activate graspnet

常用依赖包括：

numpy
opencv-contrib-python
pyrealsense2
pinocchio
pynput
ultralytics
open3d
torch

视觉伺服脚本不需要走 ROS。如果系统里 source 过 ROS，可能会让 Python 误加载 /opt/ros/.../pinocchio，出现 NumPy ABI 或 _ARRAY_API 相关错误。遇到这类问题时，优先使用当前 conda 环境里的 pinocchio，并避免让 ROS 的 PYTHONPATH 排在 conda 前面。

Hand-Eye Calibration

手眼标定脚本：

Panthera-HT_vision_servo_sdk/panthera_python/scripts/9_hand_eye_calibration.py

这个脚本是 Eye-in-Hand 手动拖动模式。机械臂进入重力补偿后，可以手动拖动相机到不同姿态，采集机械臂 TCP 位姿和相机看到的标定板位姿，然后调用 OpenCV calibrateHandEye 求解 T_tcp_camera。

1. Check Calibration Parameters

打开脚本顶部参数：

CALIBRATION_PATTERN = "chessboard"
CHESSBOARD_SIZE = (7, 7)
SQUARE_SIZE = 0.020

默认使用 8x8 棋盘格，内部角点为 7x7，每个方格 20mm。如果使用 ArUco，需要把 CALIBRATION_PATTERN 改成 "aruco"，并确认：

ARUCO_DICT = cv2.aruco.DICT_6X6_250
MARKER_SIZE = 0.05

MARKER_SIZE 必须按实际打印的 marker 黑边边长填写，单位是米。

2. Run Calibration

cd ~/Panthera_HT_demo/Panthera-HT_vision_servo_sdk/panthera_python/scripts
python 9_hand_eye_calibration.py

程序启动后会初始化 RealSense D405 和机械臂，并启用重力补偿。按提示手动拖动机械臂，让相机在不同位置和角度稳定看到标定板。

3. Collect Samples

按键：

Key	Action
S or Space	采集当前姿态数据
C	使用已采集数据执行手眼标定
R	重置已采集数据
Esc	退出

建议采集 15-20 组数据。姿态要有多样性：不要只平移，也要改变相机角度。D405 建议距离标定板 7-50cm，并保持标定板固定不动，只移动机械臂和相机。

如果棋盘格原点跳变，脚本会拒绝采集并提示警告。这种情况下按 R 重置后重新采集，或者改用 ArUco 标定。

4. Save Result

标定成功后，结果会保存到当前目录：

Panthera-HT_vision_servo_sdk/panthera_python/scripts/hand_eye_calibration.json

文件里核心字段是：

{
  "T_tcp_camera": [[...], [...], [...], [...]]
}

后续蓝球视觉伺服、ArUco 跟踪和鸭子 GraspNet 抓取都会使用这同一份 hand_eye_calibration.json。如果重新安装相机、改变相机支架或 TCP 定义，需要重新标定。

Vision Servo Scripts

视觉伺服脚本统一放在：

cd ~/Panthera_HT_demo/Panthera-HT_vision_servo_sdk/panthera_python/scripts

常用脚本：

Script	Function
11_camera_click_to_move.py	点击图像点，反投影到 3D 后移动机械臂
12_blue_circle_approach.py	检测蓝色圆形后靠近目标
13_blue_circle_follow.py	持续跟随蓝色目标
14_blue_circle_grasp.py	蓝色目标抓取流程
15_blue_circle_ibvs.py	蓝色目标 IBVS 视觉伺服
16_blue_circle_ibvs_fixed_orientation.py	蓝色目标视觉伺服，固定末端姿态
19_aruco_marker_tracking_velocity_jacobian.py	单 ArUco marker PBVS + 速度雅可比连续跟踪

Blue Ball Tracking

蓝色目标识别不使用模型权重，核心逻辑在：

blue_circle_detection.py

它使用 OpenCV HSV 蓝色阈值、圆形轮廓筛选、跨帧追踪和 RealSense 深度反投影得到相机系三维坐标。

推荐先跑固定姿态版本：

python 16_blue_circle_ibvs_fixed_orientation.py

按键：

Key	Action
g or Space	启动/停止跟踪
q	退出，停止并返回零位

这版控制链路是：蓝球像素和深度 -> 相机系 3D 目标点 -> TCP 世界系速度 -> Jacobian 伪逆 -> Joint_Vel。脚本已经加入三维误差死区、速度上限和平滑，适合做蓝色小球实时跟踪测试。

蓝色阈值可以在 blue_circle_detection.py 里调整：

BLUE_HSV_LOWER = np.array([100, 60, 40])
BLUE_HSV_UPPER = np.array([135, 255, 255])

ArUco Tracking

运行：

python 19_aruco_marker_tracking_velocity_jacobian.py

默认配置：

DICT_7X7_100
marker id: 59
marker 黑边长: 102mm
跟踪安全距离: marker 中心法向前方 150mm

按键：

Key	Action
Space	启动/停止连续跟踪
R	回安全位置
Esc	退出

这个脚本用于验证 PBVS 位姿伺服和速度雅可比控制。第一次运行时请留足空间，确认 marker 位姿稳定后再开始跟踪。

GraspNet Duck Grasping

鸭子动态抓取在：

Panthera-HT_graspnet/grasp_app/

运行：

cd ~/Panthera_HT_demo/Panthera-HT_graspnet/grasp_app
python duck_dynamic_grasp.py

功能流程：

初始化机器人、相机、YOLO、GraspNet
-> YOLO 检测鸭子
-> PBVS 视觉伺服跟踪鸭子上方安全位置
-> 手动触发 GraspNet
-> 点云裁剪和抓取候选生成
-> 碰撞/工作空间/关节限位检查
-> pre-grasp -> final approach -> close gripper -> retreat

按键：

Key	Action
Space	未跟踪时开始跟踪；跟踪中触发 GraspNet 抓取
G	手动触发 GraspNet 抓取
R	回安全位置
Esc	退出并返回零位

鸭子抓取和视觉伺服脚本使用同一份手眼标定：

../Panthera-HT_vision_servo_sdk/panthera_python/scripts/hand_eye_calibration.json

GraspNet 相关依赖需要额外准备：

• Panthera-HT_graspnet/graspnet-baseline/checkpoint-rs.tar
• Panthera-HT_graspnet/weights/duck_yolo_best.pt
• GraspNet pointnet2 CUDA 扩展
• 可用的 NVIDIA 驱动和 CUDA 环境

如果出现 No module named 'pointnet2._ext'，说明 GraspNet 的 CUDA 扩展没有编译或当前环境找不到编译产物。如果 torch.cuda.is_available() 为 False，GraspNet 推理通常不能正常跑。

Development Notes

• 最外层仓库是总入口，SDK、视觉伺服、GraspNet 分别在子目录维护。
• Panthera-HT_SDK 是基础 SDK，尽量不要把实验逻辑塞回 SDK 本体。
• 视觉伺服扩展统一放在 Panthera-HT_vision_servo_sdk/panthera_python/scripts/。
• GraspNet 抓取扩展统一放在 Panthera-HT_graspnet/grasp_app/。
• 手眼标定统一使用 scripts/hand_eye_calibration.json，不要在多个目录维护不同版本，避免坐标系混乱。
• 这个仓库会持续更新，新增脚本请在 README 中补上运行方式、按键说明和安全注意事项。

Safety

这些脚本会真实控制机械臂，不是仿真。第一次运行任何新脚本时：

• 先确认急停按钮可用。
• 降低速度、力矩和控制增益。
• 保证相机线、桌面、标定板和人都不在运动路径内。
• 非跟踪状态下确认 JointSpeed 为 0.000 后再启动视觉伺服。
• 一旦方向不对、关节乱扭或目标丢失，立即停止脚本或按急停。

License

本项目基于 Panthera-HT 开源生态做扩展维护。各子仓库和第三方组件的许可证以对应目录中的 LICENSE 或 README 为准。