orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: 框架科学优化前的完整基线

Browse Source 
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态，
使后续任何删除/重构都可回退。

本基线包含：
- 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等）
- 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接）
- 设备契约层 src/arm_interfaces（新增，46 项单测通过）
- devices/ 设备选型与品牌清单（新增）
- scripts/gen_site_example.py 与由它生成的 config/site.example.yaml

按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
This commit is contained in:
arm_dev
2026-09-23 10:53:06 +08:00
commit 6edb8d39ab
635 changed files with 1165230 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+44
View File
@@ -0,0 +1,44 @@
# 本机现场配置（含 IP / 序列号；从 site.example.yaml 复制）
config/site.yaml
# YOLO 二进制
models/yolo/*.pt
models/yolo/*.onnx
models/yolo/*.engine
models/yolo/*.trt
models/yolo/active.pt
models/hole_insert/*.pt
models/hole_insert/*.onnx
models/hole_insert/*.engine
models/hole_insert/*.trt
# 编译产物
build/
install/
log/
.colcon/
# Python
*.pyc
__pycache__/
*.egg-info/
.pytest_cache/
# Editor / OS
.vscode/
.idea/
.DS_Store
*.swp
# ROS bags
*.bag
*.db3
# Hand-eye backups
*.bak
*.bak.*
# 可选离线 RDK / Isaac（体积大）
thirdparty/
vendor/
README.md
+82
View File
@@ -0,0 +1,82 @@
# Flexiv Rizon 视觉伺服（`~/ros2`）
独立工作空间：腕部 D405 眼在手上、手眼标定、YOLO 找工件、法兰悬停跟踪、DH 夹爪、入孔检测。
**一份仓库即可跑完整跟踪与检测**，不依赖 `camera_test` 或装配 HMI。
工作目录必须是 ASCII 路径（推荐克隆到 `~/ros2`）。
| 项 | 值 |
|----|-----|
| 配置 | [`config/site.yaml`](config/site.example.yaml)（从 example 复制，不进 git） |
| 操作 | [`scripts/`](scripts/README.md) |
| 目录说明 | **[docs/WORKSPACE.md](docs/WORKSPACE.md)**（每个文件夹做什么） |
| Conda | `ros_flexiv` |
| ROS | Humble |
## 克隆后
```bash
cd ~/ros2
cp config/site.example.yaml config/site.yaml
# 编辑 site.yaml：robot.sn / robots[].ip / camera.serial / paths.yolo_weights
# 权重文件放到 models/yolo/（二进制不在 git 里）
conda activate ros_flexiv
source scripts/env.sh
./scripts/build_all.sh
source scripts/env.sh
```
示教器：**AUTO + Remote Ethernet**，关掉 Flexiv Studio（RDK 独占）。
```bash
# 三个终端必须带同一个 --arm（89=左 91=右）
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
```
改 `site.yaml`：锁高 / YOLO / 伺服 → 重启第三终端；分辨率 / 曝光 → 重启相机。
## 功能入口
| 功能 | 命令 |
|------|------|
| 实机 | `scripts/run_real.sh --arm 89\|91` |
| 腕部相机 | `scripts/run_camera_d405.sh --arm … --preview` |
| 顶视 D435 | `scripts/run_camera_d435.sh --preview` |
| 列出相机 | `scripts/list_realsense.sh` |
| 跟踪 | `scripts/run_visual_servo_rt.sh --arm … [--control]` |
| 回 home | `scripts/go_home.sh --arm …` |
| 手眼 | `scripts/run_handeye.sh --arm … teach` → `merge` → `calibrate` |
| PGEA / RGI 夹爪 | `scripts/run_gripper.sh --arm 89\|91 --cli …` |
| 双爪 ROS | `scripts/run_dual_gripper.sh` |
| 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` |
| Orisys 触觉 | `scripts/run_tactile.sh [--cli]` |
| 看配置 | `scripts/show_site_config.sh` |
89 与 91 **各有** 手眼、相机序列号、锁高、named poses，禁止混用。
## 文档
| 文档 | 内容 |
|------|------|
| [docs/WORKSPACE.md](docs/WORKSPACE.md) | 每个目录 / 脚本 / 包 |
| [docs/QUICKSTART.md](docs/QUICKSTART.md) | 环境与第一次跑通 |
| [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md) | 进程与几何 |
| [docs/SITE_CONFIG.md](docs/SITE_CONFIG.md) | `site.yaml` 字段 |
| [docs/HANDEYE.md](docs/HANDEYE.md) | 手眼标定 |
| [docs/TRACKING.md](docs/TRACKING.md) | 视觉伺服 |
| [docs/GRIPPER.md](docs/GRIPPER.md) | PGEA + RGI |
| [docs/HOLE_DETECT.md](docs/HOLE_DETECT.md) | 入孔检测 |
| [docs/TACTILE.md](docs/TACTILE.md) | Orisys 视触觉 |
| [docs/INTERFACES.md](docs/INTERFACES.md) | ROS / 脚本接口 |
| [docs/TRAJECTORIES.md](docs/TRAJECTORIES.md) | 轨迹回放 |
## 上传 git 时
- **要提交**：`src/` `scripts/` `docs/` `config/site.example.yaml` `models/yolo/README.md` `models/hole_insert/README.md`
- **不要提交**：`config/site.yaml`（含本机 IP/序列号）、`build/` `install/` `log/`、YOLO `.pt/.onnx/.engine`、`thirdparty/` `vendor/`
- 规则已写在 `.gitignore`
别人克隆后必须自己 `cp site.example.yaml site.yaml` 并重新手眼标定（不要直接用仓库里某台现场的 `T_4x4` 当真值）。
config/README.md
+9
View File
@@ -0,0 +1,9 @@
# config/
| 文件 | git | 说明 |
|------|-----|------|
| `site.example.yaml` | 提交 | 模板。克隆后复制为 `site.yaml` |
| `site.yaml` | 不提交 | 本机 SN、IP、相机、锁高、权重路径 |
| `fastdds_visual_servo.xml` | 提交 | 可选 DDS；默认 `site.yaml` 里 `fastdds_profile: ""` |
改跟踪 / 手眼 / 相机：**只改 `site.yaml`**。字段说明见 [docs/SITE_CONFIG.md](../docs/SITE_CONFIG.md)。
config/fastdds_visual_servo.xml
+84
View File
@@ -0,0 +1,84 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!--
FastDDS profile for visual-servo hard paths (Ubuntu 22.04 / Humble default RMW).
Use:
export FASTRTPS_DEFAULT_PROFILES_FILE=~/ros2/config/fastdds_visual_servo.xml
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
Goals:
- Prefer UDP unicast for low-latency control topics
- Shrink discovery / participant lease so late joiners do not stall 100 Hz
- Keep history shallow so subscribers never chase a backlog
-->
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<transport_descriptors>
<transport_descriptor>
<transport_id>udp_transport</transport_id>
<type>UDPv4</type>
<!-- Avoid shared-memory backlog stalls between heavy perception and servo -->
<maxMessageSize>65500</maxMessageSize>
</transport_descriptor>
</transport_descriptors>
<participant profile_name="visual_servo_participant" is_default_profile="true">
<rtps>
<userTransports>
<transport_id>udp_transport</transport_id>
</userTransports>
<useBuiltinTransports>false</useBuiltinTransports>
<builtin>
<discovery_config>
<!-- Faster discovery; reduce lease so dead peers drop quickly -->
<leaseDuration>
<sec>10</sec>
<nanosec>0</nanosec>
</leaseDuration>
<leaseAnnouncement>
<sec>3</sec>
<nanosec>0</nanosec>
</leaseAnnouncement>
<initialAnnouncements>
<count>5</count>
<period>
<sec>0</sec>
<nanosec>100000000</nanosec>
</period>
</initialAnnouncements>
</discovery_config>
</builtin>
</rtps>
</participant>
<!-- Perception image topics: BEST_EFFORT + KEEP_LAST (drop old frames) -->
<data_reader profile_name="sensor_best_effort">
<qos>
<reliability>
<kind>BEST_EFFORT</kind>
</reliability>
<history>
<kind>KEEP_LAST</kind>
<depth>5</depth>
</history>
<durability>
<kind>VOLATILE</kind>
</durability>
</qos>
</data_reader>
<!-- /visual_servo/cmd_vel: always only the newest Twist -->
<data_writer profile_name="cmd_vel_latest">
<qos>
<reliability>
<kind>RELIABLE</kind>
</reliability>
<history>
<kind>KEEP_LAST</kind>
<depth>1</depth>
</history>
<durability>
<kind>VOLATILE</kind>
</durability>
</qos>
</data_writer>
</profiles>
config/site.example.yaml
+483
View File
@@ -0,0 +1,483 @@
# =============================================================================
# site.example.yaml — 新环境部署模板
#
# ！本文件由 scripts/gen_site_example.py 从 config/site.yaml 自动生成，请勿手改。
# 要改模板内容，请改 config/site.yaml（它是唯一权威）后重跑：
# python3 scripts/gen_site_example.py
# 或在改完 site.yaml 后跑一次以确认模板同步：
# python3 scripts/gen_site_example.py --check
#
# 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
# 现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
#
# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
# 部署时请改成实际用户的 home 目录。
# =============================================================================
# =============================================================================
# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制而来）
# 路径：~/ros2/config/site.yaml
#
# 89 与 91 的手眼 / 腕部相机 / 夹爪 / 点位互相独立，禁止混用。
# 切换机械臂：改 robot.sn，或所有终端加同一 --arm 89|91。
# =============================================================================
robot:
# >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
sn: "Rizon4-XXXXXX"
# Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
tool_name: ""
# 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
robots:
- sn: "Rizon4-XXXXXX"
arm: left
ip: "192.168.2.XXX"
pc_ip: "192.168.2.XXX"
iface: "enp4s0"
label: "左臂89"
camera:
name: d405_left # 话题 /d405_left/... 与 91 分开
serial: "<序列号>" # 左腕 D405（一直在用的那台）
auto_exposure: true
ae_limit_us: 4000
emitter_enabled: false
gripper:
model: "PGEA-100-20-W"
sn: "<序列号>"
port: "/dev/ttyUSB0"
stroke_mm: 20.0
named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
paths:
handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_89.yaml"
handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_89.yaml"
handeye_workdir: "/tmp/handeye_calib_89"
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
servo:
ee_z_m: 0.35 # 跟踪锁高：法兰相对基座 Z [m]
min_ee_z_m: 0.20
safe_z_m: 0.45
z_min_m: 0.22
xy_track: hover # 法兰(夹爪中心) XY 压在工件上，不是画面中心
ee_vertical_down: true # 法兰 +Z 朝世界 −Z
hover_offset_ee_xy_m: [0.0, 0.0] # 爪心相对法兰 XY；平行爪同轴保持 0
# >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
# 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
# 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
hole_hover_z_m: 0.23 # ← 二阶段悬停高度调这一行
- sn: "Rizon4-XXXXXX"
arm: right
ip: "192.168.2.XXX"
pc_ip: "192.168.2.XXX"
iface: "enp4s0"
label: "右臂91"
camera:
name: d405_right # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
serial: "<序列号>" # 右腕新 D405
auto_exposure: true
ae_limit_us: 0 # 0=不封顶，让固件自己调亮
emitter_enabled: false
laser_power: 0
gripper:
model: "RGI-100-30-O-S-M1" # 旋转爪：run_gripper.sh --arm 91
sn: ""
port: "/dev/ttyUSB1"
stroke_mm: 30.0
auto_init: false
named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
paths:
handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_91.yaml"
handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_91.yaml"
handeye_workdir: "/tmp/handeye_calib_91"
# >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
# 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
servo:
ee_z_m: 0.25 # 跟踪锁高：法兰相对基座 Z [m] ← 日常调这一行
min_ee_z_m: 0.20
safe_z_m: 0.45
z_min_m: 0.22
xy_track: hover # 用法兰系手眼 T：夹爪中心(法兰原点)压在工件正上方
ee_vertical_down: true # 法兰/RGI 轴线朝下；相机有 29° 倾角，绿框不必对准十字
hover_offset_ee_xy_m: [0.0, 0.0] # RGI 与法兰同轴；爪心偏了再改这两项 [m]
# 二阶段孔定位悬停锁高（91 装了传感器后同样可用）
hole_hover_z_m: 0.22
# --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方
paths:
# >>> YOLO 权重（标准放法，不必改代码）<<<
# 1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
# bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
# 4) yolo.imgsz 必须等于 ONNX 输入边长（本文件 640；旧 hc90 才是 416）
# 5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
# 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
# yolo_weights: "/home/USER/ros2/models/yolo/detect_best.onnx"
# yolo_weights: "/home/USER/ros2/models/yolo/detect_hc90.pt"
# yolo_weights: "/home/USER/ros2/models/yolo/detect_yolo26_gongjian.pt"
# 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用上面的工件权重。
hole_weights: "/home/USER/ros2/models/hole_insert/best.pt"
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_89.yaml"
handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_89.yaml"
handeye_calibrate: "/home/USER/ros2/src/arm_application/config/handeye_calibrate.yaml"
handeye_workdir: "/tmp/handeye_calib_89"
# 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
fastdds_profile: ""
handeye:
mode: "eye_in_hand"
# >>> 标定板尺寸（内角点数 + 方格边长 m）；改这里即可 <<<
# 例：A5 板 12×17 方格 → 内角点 11×16，方格 11 mm
board_cols: 8 # 内角点列数 (方格列数 - 1)
board_rows: 11 # 内角点行数 (方格行数 - 1)
square_size_m: 0.015 # 方格边长 [m]
# 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照
velocity_scale: 0.05
move_time_sec: 8.0
settle_sec: 1.5
settle_qd_max: 0.015 # |qd|_max 低于此值才算静
settle_stable_sec: 0.6
settle_timeout_sec: 8.0
pin_holds: 2
pin_hold_sec: 0.45
skip_unstable_poses: true # 某点看不到棋盘则跳过，不整段中止
# 点位：paths.handeye_waypoints
camera:
name: "d405"
serial: "<序列号>" # 全局缺省=左腕；真机以 robots[].camera.serial 为准
# USB3 推荐 848x480@30；USB2 自动降到 640x480@15，深度/右目仍开
width: 848
height: 480 # 改宽高/帧率必须重启相机：bash ~/ros2/scripts/run_camera_d405.sh --preview
fps: 30
enable_depth: true
enable_infra: true # 同一 SDK 进程开左右红外；失败则自动退回彩图+深度
preview: true
preview_stereo: true # 预览同时显示 彩图 | 深度 | 左目 | 右目（不另开相机、不抢 USB）
overlay_hold_sec: 0.10 # 预览只留很短；长残留会看起来像绿框不跟
use_site_resolution: true
auto_exposure: true
ae_limit_us: 4000 # D405 腕部近距；91 在 robots[] 里更严
emitter_enabled: false # 腕部关红外投射，否则彩图发白
laser_power: 0
# 现场三台 RealSense（按序列号钉死，禁止按「第一台 D405」乱开）
cameras:
d405_left:
model: d405
name: d405_left
serial: "<序列号>"
role: left_wrist
arm: left
label: 左腕D405（89 平行夹爪）
width: 848 # 相机分辨率以本段为准（全局 camera: 段为旧路径，P2 收敛）
height: 480
emitter_enabled: false # 腕部必须关投射器，红外点阵会污染跟踪图像
d405_right:
model: d405
name: d405_right
serial: "<序列号>"
role: right_wrist
arm: right
label: 右腕D405（91 旋转夹爪）
width: 848
height: 480
emitter_enabled: false
d435:
model: d435
serial: "<序列号>"
role: overhead_eye_to_hand
label: 双臂中间手外D435
name: d435
width: 1280
height: 720
fps: 30
auto_exposure: true
ae_limit_us: 8000
emitter_enabled: true # 顶视深度可用投射器；只看彩图可改 false
yolo:
conf: 0.10 # 检测器门槛：略低于 min_conf，让运动模糊框也能进跟踪
iou: 0.45
imgsz: 640 # detect_best.onnx 固定 640；hc90.engine 才是 416
device: "0"
half_precision: true
backend: "tensorrt" # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh
mp_infer: true # YOLO 独立进程，不和 ROS 抢 GIL
min_conf: 0.12 # 新锁门槛；已锁定的框可略低于此
vision_hz: 30.0
infer_max_width: 640 # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵
box_ema_alpha: 1.00 # 绿框用预测补偿后的框，不再 EMA
miss_clear_frames: 10 # ~0.33 s @30 Hz；期间绿框按像素速度滑行
overlay_hold_on_miss: true # 滑行失败时仍续发上一框
hold_point_on_miss: false # 眼在手上：丢检续发旧点会让臂追错位置
miss_hold_point_frames: 1
jump_px: 400.0
jump_z_m: 0.10
z_ref_m: 0.22
min_depth_m: 0.04
max_depth_m: 0.55
work_z_max_m: 0.50
meas_blend: 0.85 # 3D 跟手；抖则略降
z_alpha_min: 0.10
z_alpha_max: 0.40
z_alpha_gain: 1.0
camera_frame: "d405_color_optical_frame"
object_frame: "tracked_object"
depth_topic: "/d405/aligned_depth_to_color/image_raw"
feature_mode: "plane" # plane=框内近表面立体点（工件本体）；box=单目射线
plane_inlier_m: 0.008 # 拟合平面内点阈值 [m]
plane_band_m: 0.06 # 近表面深度带宽 [m]
servo:
control_hz: 100.0
# --- 基座坐标系下的末端约束（法兰，单位 m）---
ee_x_m: 0.25
ee_y_m: 0.00
ee_z_m: 0.35 # 全局缺省锁高；真机以 robots[].servo.ee_z_m 为准（89/91 分开调）
ee_hold_xy: false # false=跟踪工件 XY；true=锁死 ee_x/ee_y
xy_track: "hover" # hover=法兰悬停在目标正上方（任意锁高） lookat=画面中心对准目标
use_static_handeye_tf: false # 只用 handeye_result.yaml，避免 TF 树抢父子帧
ee_vertical_down: true
# --- 启动回初始位（已停用：--control 不再调用 goto_ee_plane）---
safe_z_m: 0.45 # >>> 防撞安全高度：先纯 Z 抬到这里再回 home，避免扫到料框/壁
home_pose_name: "home" # 非夕初始点位（/set_named_pose：home | ready | inspect | pos_left）
home_pose_speed: 0.30 # 回初始位的速度比例 (0,1]，越小越稳
ee_hold_kp: 2.0
ee_w_max: 0.60
z_ref_m: 0.22
z_min_m: 0.22
approach_xy_gate_m: 0.040 # --control 先降到锁高，|ee_z-ee_z_m| 小于此值才开始 XY hover
v_approach_z: 0.025
watchdog_timeout_sec: 1.2
lost_timeout_sec: 1.2
stale_hold_sec: 0.35 # 丢检空窗内用基座 KF 预测继续跟（自运动安全），超过才钉住
max_extrapolate_sec: 0.35 # 预测时间上限（图像延迟 + 前瞻）
predict_enable: true # 基座系恒速预测，补视觉延迟
predict_sec: 0.16 # 额外前瞻 [s]
predict_v_max: 0.80 # 工件速度上限 [m/s]，防乱飞
predict_jump_m: 0.08 # 基座 XY 跳变大于此则重锁速度
kp_xy: 4.20 # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5
ki_xy: 0.00
kd_xy: 0.20
kp_z: 0.80
ki_z: 0.00
kd_z: 0.35
i_sep_pos_m: 0.008
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
deadzone_z_m: 0.012
lock_hold_sec: 0.7 # 死区驻留多久才发 /visual_servo/locked（串联 0.6s 已够）
e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速
v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s
v_max_z: 0.04
min_ee_z_m: 0.20
workspace_xyz_min: [0.08, -0.55, 0.18] # 法兰工作盒；盒外只允许往回走，不再卡死 no traj
workspace_xyz_max: [0.85, 0.55, 0.65]
a_max: 3.00 # 加速度上限（真正作用在 hover 指令上）
vel_lpf_alpha: 0.80 # 速度低通：越大越跟手（1=不滤波）
dq_window_size: 1 # 1=不做滑窗平均，去掉 ~20ms 滞后
traj_stream_hz: 50.0 # 轨迹点流率：JTC open_loop+样条，吃得起
exec_horizon_sec: 0.08 # 每个轨迹点的执行窗口
max_qdot: 2.00 # 关节速度上限 [rad/s]
min_cmd_delta_rad: 0.001
point_topic: "visual_servo/filtered_point"
traj_topic: "rizon_arm_controller/joint_trajectory"
# 大寰夹爪：官方 Modbus RTU，USB–RS485 插 IPC，不走 Studio/RDK。
# 左 89 PGEA → /dev/ttyUSB0；右 91 RGI → /dev/ttyUSB1。
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open|close|pos
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|close|rotate 90
# bash ~/ros2/scripts/run_dual_gripper.sh
# ROS：/gripper/open|close（PGEA 单爪）或 /dual_gripper_driver/{left,right}/execute
gripper:
backend: "usb" # usb=工控机官方Modbus rdk=Studio设备（不用）
vendor: "dh"
model: "PGEA-100-20-W"
sn: "<序列号>" # 铭牌，不进协议
device_name: "GripperDahuanModbus"
port: "/dev/ttyUSB0" # 本机 MACROSILICON/pl2303
baud: 115200 # 官方默认 115200 8N1
slave_id: 1 # 官方默认从站 1
rs485_rts: "" # 空=不拉RTS；scan 若 OK 在 tx/tx_inv 再填
stroke_mm: 20.0
# --- 开合 / 力 / 速（大寰官方寄存器，超出会被驱动钳位）---
force_pct: 20 # 夹持力 区间 20–100 %（低于 20 会被钳到 20；PGEA-100 约 30–100 N）
speed_pct: 50 # 开合速度 区间 1–100 %
open_position: 1000 # 张开目标 区间 0–1000 ‰（1000=全开）
close_position: 0 # 闭合目标 区间 0–1000 ‰（0=全合；留缝改 50–200）
auto_init: true
state_hz: 20.0
# 双爪共用参数（run_dual_gripper.sh / load_from_site）
parallel:
stroke_min: 0.0
stroke_max: 20.0
default_speed: 50
default_force: 20
force_min: 20
force_max: 100
auto_initialize: true
rotary:
auto_initialize: false
angle_min: -32767.0
angle_max: 32767.0
max_step_deg: 30000.0
finger_open_per_mille: 1000
finger_close_per_mille: 0
default_speed: 50
default_torque: 50
default_finger_speed: 50
default_finger_force: 50
torque_min: 20
torque_max: 100
# RDK 回退字段（backend: rdk 才用）
force_n: 50.0
vel_mps: 0.05
# 入孔检测。算法参数写在这里（覆盖包内 hole_detect.yaml 默认值）。
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
hole_detect:
backend: "two_stage" # two_stage | yolo | hough
weights: "/home/USER/ros2/models/hole_insert/best.pt"
# 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
conf: 0.25
iou: 0.7
imgsz: 640
center_gate_enable: true
center_max_offset_ratio_x: 0.15
center_max_offset_ratio_y: 0.18
center_gate_draw: true
refine_hole_centers: true
bbox_padding: 80
gaussian_ksize: 5
gaussian_sigma: 1.5
hough_dp: 1.2
hough_min_dist: 50
hough_param1: 100
hough_param2: 26
hough_min_radius: 6 # 悬停较高时螺纹孔半径只有几像素
hough_max_radius: 30
min_radius: 6
max_radius: 30
hole_distance_min: 90 # 悬停 0.23m 两螺纹孔约 170px；过小会配到中心孔
hole_distance_max: 700
expected_holes: 2
min_hole_sep_px: 20
max_hole_sep_px: 2000
stabilize_hold_frames: 15
stabilize_ema_alpha: 0.35
stabilize_max_jump: 60
default_depth_m: 0.10
camera_intrinsics:
fx: 425.0
fy: 425.0
cx: 640.0
cy: 360.0
# ===== 二阶段一次性拍照定位（stage2_locate_node，不做实时推理）=====
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只悬停调高度
stage2:
target_hole: "k1" # 对准目标孔：k1=图像左孔 / k2=右孔（见 k1_label）
k1_label: "left" # k1 是画面里的左孔；现场相反就改 right
align_tol_m: 0.0007 # 对准到位判据 [m]
align_max_iters: 8 # 拍照→修正→复拍 迭代上限
coarse_max_moves: 2 # 工件粗居中（bbox 中心）移动上限
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
final_gap_mm: 2.0 # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
final_descent_enable: true # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
settle_sec: 0.45 # 到位后静止多久再拍照
snap_frames: 5 # 每次精拍取中值的帧数
min_depth_m: 0.06 # 有效深度范围（防孔底/桌面拉偏 XY）
max_depth_m: 0.60
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
# ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
# 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入
stage3:
# —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
# 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
touch_delta: 250.0 # 触面判定：fnormal > μ0 + max(4σ0, 本值)
press_delta: 200.0 # 轻触目标按压量（触面阈值之上再加这么多）
max_press_delta: 900.0 # 螺旋中允许的最大按压（超过→力闭环抬起）
safe_press_delta: 2000.0 # 安全上限：fnormal > μ0+本值 立即抬升中止
shear_delta: 600.0 # 切向拖拽保护：shear > 静止切向均值+本值 → 贴面时微抬 0.15mm（Z 钉在触面）
insert_press_delta: 1200.0 # 插入力上限（触面阈值之上；超过回退微抖重试）
kp_z_mm_per_unit: 0.001 # Z 力闭环增益：每单位读数偏差调整的毫米数
approach_step_mm: 0.8 # 触面前每步下降量（驱动死区已修，0.5mm 微步可跟踪；小步=触面冲击小）
enter_probe_mm: 2.0 # 触面深于此值→疑似直接入孔，四向微探验证后跳过螺旋
mouth_lift_mm: 2.0 # 直接入孔后抬到孔口倒角段再定心（深插横探会别死孔壁）
baseline_min: 8.0 # 触觉基线健康窗下限（<此值=几乎全 0，tare/断流）
baseline_max: 1500.0 # 上限（>此值=近饱和，接触余量不足）——出窗即中止并提示重启触觉
approach_ft_touch_n: 6.0 # approach 腕部 |F| 旁路触面判据（标定牛顿；自由空气≈2-3N）
approach_max_mm: 60.0 # 从当前位姿最多下降这么多（找不到面就中止；悬停被抬高后仍有余量）
spiral_pitch_mm: 1.0 # 螺旋螺距（< 通止规直径，保证重叠覆盖）
spiral_step_mm: 0.5 # 沿螺旋的弧长步长
spiral_r_max_mm: 4.0 # 最大搜索半径（覆盖二阶段残差 1.5mm + 余量）
spiral_expand_max: 1 # 找不到孔时扩大半径的次数（残差≤1.8mm 时代码不再扩大）
spiral_expand_step_mm: 2.0
drop_ratio: 0.45 # 入孔判据：fnormal 相对接触均值跌落比例
drop_frames: 2 # 跌落需持续的帧数（防毛刺）
center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
center_scan_mm: 0.3
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
insert_retry: 3
screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
search_speed: 0.03 # 搜孔笛卡尔速度 [m/s]
settle_sec: 0.25 # 每个微步后的静止时间
tactile_timeout_s: 3.0 # 触觉话题断流多久中止
ft_watchdog: true # 订阅 /<sn>/flexiv_robot_states 做腕部力看门狗
ft_safe_n: 35.0 # 腕部 |F| 安全上限（真牛顿，标定过的；连续3拍超限才中止）
linger_sec: 8.0 # 完成后继续发布结果的时长
# Orisys 视触觉（夹爪左右指摄像机，不是腕部 D405）。SDK 单独 pip 安装。
# bash ~/ros2/scripts/run_tactile.sh --cli
# bash ~/ros2/scripts/run_tactile.sh
tactile:
backend: "orisys" # orisys | mock
model: "dd03" # 传感器型号（设备契约层用的键；须与 devices/sensors/orisys/models/ 下的目录同名）
config: "dd03" # 旧键，与 model 同义。仅为兼容现有节点保留，P2 会移除
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json"
# 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
# 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
cuda: false # 两路进程抢 GPU 上下文会互相拖慢；CPU 解算仅 ~9ms/帧，足够
isstitch: true
infer_backend: "cpu" # cupy | opencl | cpu；双路并行用 cpu 最稳
baseline_seconds: 2.0
filter_alpha: 0.25
motion_threshold: 0.0
target_fps: 30.0
publish_hz: 20.0
rotation_config: "" # 左右共用；有分指文件则用下面两项
left_rotation: "" # 例：orisys SDK 的 rotation_config_0.json
right_rotation: ""
go_home:
pose: "home"
speed: 0.25
network:
# User1=enp4s0 本机 <PC_IP> ↔ <左臂SN>=.101 / <右臂SN>=.100
prefer_iface: "enp4s0"
# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
sim:
platform: "gazebo"
world: "" # 世界文件路径；留空用 Gazebo 默认空场景
physics_enable: true
joint_prefix: "sim_" # 仿真关节前缀，必须与真实臂前缀区分
devices/README.md
+62
View File
@@ -0,0 +1,62 @@
# devices/ —— 设备与外部文件总目录
这个目录存在的唯一目的：**把「换个硬件」从改代码变成改配置**。
## 设计原则（务必先读）
### 1. 选型与数值分离
| 关注点 | 放在哪 | 谁改 |
|--------|--------|------|
| **用哪个品牌的哪个驱动** | `devices/registry.yaml` | 集成工程师（很少改） |
| **设备号 / 速度 / 阈值 / 悬高** | `config/site.yaml` | 现场操作人员（经常改） |
**`devices/` 下不允许出现任何现场数值。** 数值只有 `config/site.yaml` 一个来源。
这样可以避免现在这种「同一个值三处定义、改了不生效」的问题。
### 2. 目录约定
```
devices/
├── registry.yaml # 选型开关（唯一入口）
├── robots/ # 机械臂：RDK + 适配器
├── grippers/ # 夹爪
├── sensors/
│ └── orisys/ # ★ 视触觉核心，长期迭代，单独占一个大文件夹
├── cameras/ # 相机
└── sim/ # 仿真平台（当前 Gazebo）
```
每个品牌一个子目录，子目录内固定结构：
```
<类别>/<品牌>/
├── manifest.yaml # 我是谁 / 支持什么能力 / 需要哪些配置 / 外部文件放哪
├── driver/ # 适配器（把厂商 API 翻译成框架接口）
└── <厂商物料>/ # 如 rdk/、sdk/，原样放置，不要改名
```
### 3. 一个新品牌要接进来，只需要四步
1. **丢物料**：把厂商的 RDK / SDK / 驱动包放进 `<类别>/<品牌>/` 下对应文件夹
（格式和命名要求写在该类别的 `README.md` 里，**必须按它要求放**）。
2. **填 manifest**：复制同类别的 `manifest.yaml` 改四个字段（id / label / driver.package / config_schema）。
3. **写适配器**：在 `driver/` 下实现框架的接口契约（见 `src/arm_interfaces/`）。
这一步的工作量取决于厂商 —— 如果厂商本来就提供符合契约的 ROS 2 驱动，这一步几乎为零。
4. **改一行选型**：把 `registry.yaml` 里对应类别的值改成新品牌 id。
> **诚实说明**：第 3 步无法完全避免。Flexiv 的 RDK 是 C++/Python 库而不是 ROS 2 驱动，
> 所以它外面永远需要一层薄适配器。框架能做的是把这层适配器写到最薄（只做 API 翻译，
> 不含任何业务逻辑），而不是让你零代码接入。
### 4. 启动时的校验
框架启动会依次读 `registry.yaml` → 各 `manifest.yaml` → `config/site.yaml`，
校验三件事：
- 选中的品牌目录存在，且 `manifest.yaml` 可解析；
- manifest 里 `assets` 声明的必需物料文件都在；
- `config_schema` 里标记 `required: true` 的配置键，在 `site.yaml` 里都有值。
任何一项不满足**立即报错并明确指出该去哪个文件补什么**，不允许静默降级。
这是替代现在「`run_handeye.sh` 传了 5 个节点没声明的参数、调参静默失效」那类问题的机制。
devices/cameras/README.md
+51
View File
@@ -0,0 +1,51 @@
# devices/cameras/ —— 相机
## 厂商物料放置要求
| 项 | 要求 |
|----|------|
| 格式 | 厂商 SDK / 驱动包原始格式。RealSense 用 `pyrealsense2` 或官方 ROS 2 包 |
| 命名 | 保持原样，不要改名 |
| 序列号 | **不要写在这里**。序列号在 `config/site.yaml` 的 `cameras.<名字>.serial` |
| 生效 | 放好物料 → 改 `registry.yaml` 选型 → 改 `site.yaml` 的 serial / 分辨率 / 曝光 → 重启对应相机脚本 |
## 目录结构
```
cameras/
├── README.md
├── realsense/ # Intel RealSense（当前在用）
│ ├── manifest.yaml
│ └── driver/
└── <新品牌>/
```
## 相机角色与命名（重要）
框架里每台相机必须有一个**角色**，因为不同角色参与完全不同的流程：
| 角色 | 当前相机 | 参与流程 | 是否参与手眼标定 |
|------|----------|----------|------------------|
| `wrist_eye_in_hand` | D405（左腕 / 右腕各一台） | 视觉跟踪 k0、拍照推理 k1/k2 | 是（眼在手上） |
| `overhead_eye_to_hand` | D435（双臂中间） | 粗定位兜底（S03） | 是（眼在手外） |
**注意**：D435 当前**还没有做手眼标定**，所以 S03 粗定位链路暂时不可用。
但这不影响 S04 跟踪链路，两条链路用的是同一套权重、同一个跟踪目标 k0。
## 已知的坑（迁移时必须保留的处理）
1. **USB 独占**：RealSense 同时只能被一个进程打开。启动前必须 `pkill` 掉旧进程，
现有脚本里那句 `sleep 2` 是必要的，不要删。
2. **USB2 vs USB3 会自动降分辨率**：现有脚本检测到 USB2 会降到 `640x480@15` 并告警，
这个降级逻辑要保留，否则会在弱链路上直接起不来。
3. **`run_camera_d435.sh` 曾把分辨率/曝光/emitter 硬编码**，与 `site.yaml` 的
`cameras.d435` 字段脱节。迁移后必须全部从 `site.yaml` 读，不允许硬编码。
## 当前状态（迁移参考）
| 现有位置 | 内容 | 迁移去向 |
|----------|------|----------|
| `src/arm_application/arm_application/realsense_color_pub.py` | 采集 + 发布彩图/对齐深度/红外 | `driver/` |
| `src/arm_application/arm_application/realsense_view.py` | 独立预览进程（与采集分离） | `driver/` 或 tools/ |
| `src/arm_application/arm_application/realsense_sdk_view.py` | Qt 四宫格窗口（被上面两者共用） | 保留 |
| `src/arm_application/arm_application/handeye_realsense.py` | 内参查询、话题解析、曝光设置 | 拆开：相机相关的进 `driver/`，标定相关的进 `handeye/` |
devices/cameras/realsense/driver/.gitkeep
View File
devices/cameras/realsense/manifest.yaml
+141
View File
@@ -0,0 +1,141 @@
# =============================================================================
# devices/cameras/realsense/manifest.yaml
#
# Intel RealSense 相机驱动清单。覆盖两种型号：
# D405 —— 腕部相机，眼在手上，参与跟踪与手眼标定
# D435 —— 手外相机，顶视，参与粗定位兜底与手眼标定
#
# 这里【不放现场数值】。序列号 / 分辨率 / 曝光一律在 config/site.yaml。
# =============================================================================
id: realsense
label: Intel RealSense D405 / D435
kind: camera
vendor: intel
version: "pyrealsense2"
# 从哪里读出「当前用哪个型号」。值是 config_schema 里的 field key。
model_selector: model
description: >
RealSense 深度相机。D405 装在法兰/夹爪上做腕部视觉（眼在手上），
D435 架在双臂中间做顶视（眼在手外）。USB 独占，同一时刻只能一个进程打开。
# --- 厂商物料放置要求 ------------------------------------------------------
assets:
- path: driver
required: false
what: 厂商驱动（若使用 pyrealsense2 则无需放置，由 conda 环境提供）
format: 厂商原始格式
naming: 保持原样
where_used: driver 通过 pyrealsense2 或官方 ROS 2 包访问相机
# --- 适配器 ----------------------------------------------------------------
driver:
package: arm_driver_realsense
executable: realsense_publisher
build: colcon
# --- 能力声明 --------------------------------------------------------------
capabilities:
- color
- depth
- aligned_depth_to_color
- infrared_left
- infrared_right
- query_intrinsics # 手眼标定必需
- set_exposure
# --- 型号与角色 ------------------------------------------------------------
models:
- id: d405
label: D405 腕部相机
roles: [wrist_eye_in_hand]
default_resolution: "848x480"
default_fps: 30
notes: >
腕部使用，必须关闭红外投射器（emitter_enabled: false），
否则投射的红外点阵会污染跟踪图像。
- id: d435
label: D435 手外顶视相机
roles: [overhead_eye_to_hand]
default_resolution: "1280x720"
default_fps: 30
notes: >
手外使用，通常需要开启红外投射器以提升无纹理表面的深度质量。
当前尚未完成手眼标定。
# --- 配置项声明 ------------------------------------------------------------
config_schema:
- key: model
site_path: cameras[].model
type: str
required: true
doc: >
相机型号。必须与上面 models 列表里的某个 id 一致（d405 或 d435）。
型号决定默认分辨率与是否应开启红外投射器。
- key: serial
site_path: cameras[].serial
type: str
required: true
doc: >
相机序列号。用 scripts/list_realsense.sh 查看当前接入的全部序列号并映射角色。
每个角色必须是不同的序列号，否则两台相机抢同一台设备。
- key: role
site_path: cameras[].role
type: str
required: true
doc: >
相机角色，合法值: left_wrist | right_wrist | overhead_eye_to_hand。
以 _wrist 结尾表示装在法兰上（眼在手上，手眼标定用 eye_in_hand）；
overhead_eye_to_hand 表示固定安装（眼在手外，手眼标定用 eye_to_hand）。
角色决定该相机参与哪条流程，所以必须与物理安装位置一致。
- key: width
site_path: cameras[].width
type: int
unit: px
required: true
doc: 彩色流宽度。注意 USB2 链路会自动降到 640 并告警。
- key: height
site_path: cameras[].height
type: int
unit: px
required: true
doc: 彩色流高度。注意 USB2 链路会自动降到 480 并告警。
- key: fps
site_path: cameras[].fps
type: int
unit: Hz
required: false
default: 30
doc: 帧率。
- key: auto_exposure
site_path: cameras[].auto_exposure
type: bool
required: false
default: true
doc: 自动曝光。
- key: ae_limit_us
site_path: cameras[].ae_limit_us
type: float
unit: us
required: false
doc: >
曝光上限。腕部 D405 建议限到 4000us 以内 —— 跟踪时曝光过长会导致运动拖影。
0 表示不封顶。
- key: emitter_enabled
site_path: cameras[].emitter_enabled
type: bool
required: true
doc: >
红外投射器。腕部必须 false（投射点阵污染跟踪图像），
手外通常 true（提升无纹理表面深度质量）。
devices/grippers/README.md
+52
View File
@@ -0,0 +1,52 @@
# devices/grippers/ —— 夹爪
## 厂商物料放置要求
| 项 | 要求 |
|----|------|
| 格式 | 厂商原始发布的驱动/SDK/协议文档，不要转换 |
| 命名 | 保持原文件名。若厂商只提供 PDF 协议手册，把它放进来即可（适配器按手册实现） |
| 通讯参数 | **不要写在这里**。串口、波特率、从站号、行程全部在 `config/site.yaml` |
| 生效 | 放好物料 → 改 `devices/registry.yaml` 选型 → 改 `site.yaml` 里的串口与行程 → 重启 `scripts/run_gripper.sh` |
## 目录结构
```
grippers/
├── README.md
├── dh_pgea/ # 大寰（当前在用）
│ ├── manifest.yaml
│ ├── protocol/ # 厂商协议手册 / 寄存器表
│ └── driver/
└── <新品牌>/
```
## 适配器必须实现的接口契约
夹爪的接口刻意做成和机械臂对称 —— 上层只调接口，不关心是哪个品牌、走 Modbus 还是走 RDK。
| 能力 | 用途 |
|------|------|
| `open` | 张开 |
| `close` | 夹紧（可带力/速度参数） |
| `move` | 移动到指定开度 |
| `rotate` | 旋转到指定角度（旋转爪） |
| `get_status` | 读开度、力、是否夹持到位 |
**关键设计：`rotate` 与 `open/close` 解耦。** 通止规装配的 S08 循环是
「转 180° → 松爪 → 快回转 → 夹紧」四步反复，这四步必须是四个独立可调用的原语，
不能封装成一个「拧螺丝」复合动作 —— 否则中途掉件时无法精确恢复。
## 当前状态（迁移参考）
大寰现有实现有两套互不复用的 Modbus 栈，P1 之后会合并到 `dh_pgea/driver/` 下：
| 文件 | 内容 | 处置 |
|------|------|------|
| `src/arm_application/arm_application/dh_pgea.py` | PGEA Modbus 客户端 + 自带 CRC 表 + CLI | 保留，收敛为驱动 |
| `src/arm_application/arm_application/dh_official.py` | 另一份寄存器级客户端（含 RGI 旋转寄存器） | 合并进来，消除重复 |
| `src/arm_application/arm_application/dual_gripper.py` | PGEA/RGI 统一抽象 | 改为调用新驱动接口 |
| `src/arm_application/arm_application/dh_pgea_node.py` | ROS 节点封装 | 改为通用夹爪节点的品牌无关实现 |
**注意**：`site.yaml` 里 `gripper.backend: rdk` 这条路径已被 `run_gripper.sh` 明确拒绝，
`scripts/rdk_gripper.py` 是孤儿脚本。这条「走控制器 RS485」的分支在 P2 会被清理。
devices/grippers/dh_pgea/driver/.gitkeep
View File
devices/grippers/dh_pgea/manifest.yaml
+135
View File
@@ -0,0 +1,135 @@
# =============================================================================
# devices/grippers/dh_pgea/manifest.yaml
#
# 大寰（DH）夹爪驱动清单。覆盖两种型号：
# PGEA —— 平行夹爪（左臂 89 使用）
# RGI —— 旋转夹爪（右臂 91 使用）
#
# 这里【不放现场数值】。串口 / 波特率 / 从站号 / 行程一律在 config/site.yaml。
# =============================================================================
id: dh_pgea
label: 大寰 PGEA 平行爪 / RGI 旋转爪
kind: gripper
vendor: dh
version: "Modbus RTU（工控机 USB-RS485 直连）"
# 从哪里读出「当前用哪个型号」。值是 config_schema 里的 field key。
# RGI 型号会在此基础上叠加 rotate 能力。
model_selector: model
description: >
大寰夹爪。PGEA 提供开合，RGI 在开合之外提供绕轴旋转。
通讯走工控机 USB 转 RS485，不经过机械臂控制器。
# --- 厂商物料放置要求 ------------------------------------------------------
assets:
- path: protocol
required: false
what: 大寰 Modbus 协议手册与寄存器表
format: 厂商原始 PDF / 文档
naming: 保持原文件名
where_used: 适配器实现寄存器读写时的依据
notes: >
寄存器表已固化进驱动代码（见 driver/）。此目录只作为排查时的对照资料，
缺失不影响运行。
# --- 适配器 ----------------------------------------------------------------
driver:
package: arm_driver_dh_gripper
executable: dh_gripper_driver
build: colcon
# --- 能力声明 --------------------------------------------------------------
capabilities:
- open
- close
- move
- get_status
# RGI 型号额外支持旋转。框架按 manifest 的型号分支决定是否暴露 rotate。
models:
- id: PGEA-100-20-W
label: PGEA 平行爪（左臂）
capabilities: [open, close, move, get_status]
stroke_mm: 20.0
- id: RGI-100-30-O-S-M1
label: RGI 旋转爪（右臂）
capabilities: [open, close, move, rotate, get_status]
stroke_mm: 30.0
notes: >
rotate 是通止规装配 S08 循环的核心原语，必须可独立调用且可随时中止。
单次转角默认 180°（配置项 system.screw_step_deg），因为不确定基于工具系
能转多少度是极限。
# --- 配置项声明 ------------------------------------------------------------
config_schema:
- key: model
site_path: robot.robots[].gripper.model
type: str
required: true
doc: >
当前使用的夹爪型号。必须与上面 models 列表里的某个 id 一致。
型号决定是否具备 rotate 能力（PGEA 没有，RGI 有）。
- key: port
site_path: robot.robots[].gripper.port
type: str
required: true
doc: 串口设备路径（如 /dev/ttyUSB0）。建议用 /dev/serial/by-id/ 下的稳定路径，
因为 ttyUSB 编号会随插拔漂移。
- key: baud
site_path: gripper.baud
type: int
required: false
default: 115200
doc: 波特率。
- key: slave_id
site_path: gripper.slave_id
type: int
required: false
default: 1
doc: Modbus 从站号。
- key: stroke_mm
site_path: robot.robots[].gripper.stroke_mm
type: float
unit: mm
required: true
doc: 该爪的总行程。用于把 0–1000‰ 开度换算成毫米。
- key: force_pct
site_path: gripper.force_pct
type: int
unit: '%'
required: false
default: 20
doc: 夹紧力百分比。低于 20 会被硬件钳位到 20。
- key: speed_pct
site_path: gripper.speed_pct
type: int
unit: '%'
required: false
default: 50
doc: 动作速度百分比。
- key: open_position
site_path: gripper.open_position
type: int
unit: 千分比
required: false
default: 1000
doc: 「张开」对应的开度。1000 = 全开。
- key: close_position
site_path: gripper.close_position
type: int
unit: 千分比
required: false
default: 0
doc: 「夹紧」对应的开度。0 = 全合。
注意：通止规是异形件，全合可能压不到，实际夹紧位需要按件标定后填这里。
devices/grippers/dh_pgea/protocol/README.md
+31
View File
@@ -0,0 +1,31 @@
# devices/grippers/dh_pgea/protocol/ —— 大寰 Modbus 协议资料
## 放什么
大寰（DH）夹爪的 Modbus RTU 协议手册、寄存器表、型号规格书。
## 格式与命名要求
| 项 | 要求 |
|----|------|
| 格式 | 厂商原始 PDF / 文档，不要转换 |
| 命名 | 保持原文件名，或改为 `<型号>_<文档类型>.pdf`，如 `PGEA-100-20-W_寄存器表.pdf` |
| 是否必需 | **不必需**。寄存器表已固化进驱动代码，此目录仅供人工排查时对照 |
## 为什么放这里
驱动里硬编码了寄存器地址与 CRC 表。当现场出现「某个动作没反应」时，
需要拿手册逐条核对寄存器定义与从站号，所以把手册放在驱动旁边最方便。
## 已知的两处不一致（迁移时要处理）
1. 现在代码里有**两套独立的 Modbus 实现**，各自定义了一份寄存器常量：
- `src/arm_application/arm_application/dh_pgea.py`（PGEA，自带 CRC 表）
- `src/arm_application/arm_application/dh_official.py`（含 RGI 旋转寄存器）
两份功能重叠，P1 之后合并成一份。
2. `site.yaml` 里 `gripper.backend: rdk` 这条「走机械臂控制器 RS485」的分支
已被 `run_gripper.sh` 明确拒绝，对应的 `scripts/rdk_gripper.py` 是孤儿脚本。
如果你其实需要走控制器 RS485（而不是工控机 USB），请明确告知 ——
这会影响驱动是挂在 `devices/grippers/` 还是 `devices/robots/flexiv/` 下。
devices/registry.yaml
+30
View File
@@ -0,0 +1,30 @@
# =============================================================================
# devices/registry.yaml —— 设备选型开关
#
# 这个文件只回答一个问题：每一类设备当前由哪个驱动实现来接管。
# 这里【不放任何现场数值】。数值一律在 config/site.yaml。
#
# 改动任何一个值之后：重启对应的启动脚本即生效，不需要重新编译。
# =============================================================================
version: 1
# 机械臂驱动实现。可选值 = devices/robots/ 下的目录名
# flexiv -> devices/robots/flexiv/ （非夕 Rizon / AICO）
robots: flexiv
# 夹爪驱动实现。可选值 = devices/grippers/ 下的目录名
# dh_pgea -> devices/grippers/dh_pgea/ （大寰 PGEA 平行爪 / RGI 旋转爪）
grippers: dh_pgea
# 传感器驱动实现。可选值 = devices/sensors/ 下的目录名
# orisys -> devices/sensors/orisys/ （视触觉：3 切向力 + 1 法向力 + 深度图 + 力矢量图）
sensors: orisys
# 相机驱动实现。可选值 = devices/cameras/ 下的目录名
# realsense -> devices/cameras/realsense/ （D405 腕部 / D435 手外）
cameras: realsense
# 仿真平台。可选值 = devices/sim/ 下的目录名
# gazebo -> devices/sim/gazebo/
sim: gazebo
devices/robots/README.md
+61
View File
@@ -0,0 +1,61 @@
# devices/robots/ —— 机械臂
## 厂商物料放置要求
每个品牌一个子目录，物料放在该子目录下的 `rdk/`（或厂商自定的物料目录，须在 `manifest.yaml`
的 `assets` 里声明）。
**通用要求：**
| 项 | 要求 |
|----|------|
| 格式 | 厂商**原始发布格式**，不要自行转换、不要重新打包成 zip/tar 后不展开 |
| 命名 | **保持厂商原目录名和原文件名，一律不要改名**。改名会导致 RDK 内部的 include 路径、CMake 查找路径、Python 包名全部失效 |
| 版本 | 在 `manifest.yaml` 的 `version` 字段里记录版本号，方便日后排查 |
| 编译产物 | 不要把编译产物（`.a` `.so` `build/`）提交进 git；离线库放 `vendor/` |
| 生效 | 放好物料 → 编译一次 → 改 `devices/registry.yaml` 选型 → 重启 `scripts/run_real.sh` |
## 目录结构
```
robots/
├── README.md
├── flexiv/ # 非夕（当前在用）
│ ├── manifest.yaml
│ ├── rdk/ # 厂商 RDK 源码或安装包，原样放置
│ └── driver/ # 适配器：把 RDK API 翻译成框架接口
└── <新品牌>/
├── manifest.yaml
├── rdk/ 或 sdk/
└── driver/
```
## 适配器必须实现的接口契约
框架上层（伺服、跟踪、装配状态机、手眼标定）**只调用下面这组接口，不认识任何厂商 SDK**。
契约定义在 `src/arm_interfaces/`，每个方法对应的 ROS 2 话题/服务/action 也在那里声明。
| 能力 | 用途 | 谁在用 |
|------|------|--------|
| `get_arm_state` | 读关节角 + 末端位姿 | 手眼标定、状态记录 |
| `follow_joint_trajectory` | 关节轨迹（点到点、多路点） | 手眼标定走点、装配移动 |
| `servo_joint` | 高频关节伺服 | 视觉伺服 `servo_node` |
| `servo_cartesian` | 笛卡尔位姿伺服 | 跟踪、悬停 |
| `compliant_descent` | 柔顺下臂（接触力约束） | 装配接触阶段 |
| `set_tool` / `switch_tool` | 设置/切换工具坐标系（如 tool1） | 全局工具系锁定 |
| `home` | 回到初始位姿 | 每个循环开始、异常复位 |
| `enable` / `clear_fault` | 使能、清故障 | 启动自检、异常恢复 |
**缺哪个能力就在 manifest 的 `capabilities` 里不写哪个**，框架启动时会检查上层要用的能力是否都被支持，
不支持就明确报错，而不是运行时莫名其妙失败。
## 当前状态（迁移参考）
非夕臂的现有实现分散在这些地方，P1 之后会收敛到 `flexiv/driver/` 下：
- 硬件接口：`src/flexiv_hardware/`（ros2_control 插件，上游官方）
- bringup：`src/arm_bringup/stack.launch.py` 的 `backend:=real` 分支
- 轨迹适配：`src/arm_control/arm_control/real_trajectory_adapter.py`
- 几何 IK 与手眼加载：`src/arm_application/arm_application/geom_ik.py`
- 软拖拽：`scripts/rdk_drag_bridge.py`
- 工具系切换：`scripts/rdk_switch_tool.py`
devices/robots/flexiv/driver/.gitkeep
View File
devices/robots/flexiv/manifest.yaml
+116
View File
@@ -0,0 +1,116 @@
# =============================================================================
# devices/robots/flexiv/manifest.yaml
#
# 非夕机械臂（Flexiv Rizon / AICO）驱动清单。
# 框架启动时读这个文件，决定加载哪个适配器、校验哪些配置、检查哪些外部物料。
#
# 这里【不放现场数值】。SN / IP / 速度 / 悬高一律在 config/site.yaml。
# =============================================================================
id: flexiv
label: 非夕 Flexiv Rizon / AICO
kind: robot
vendor: flexiv
version: "RDK 1.9.x（与 flexiv_ros2 humble-v1 v1.9.1 配套）"
description: >
非夕七轴力控机械臂。当前项目使用 Rizon4 双臂（SN 尾号 89 / 91）。
上层通过 ros2_control + rizon_arm_controller 下发关节轨迹，柔顺下臂走 RDK 力控原语。
# --- 厂商物料放置要求 ------------------------------------------------------
# 每个条目会在框架启动时被检查；required: true 且文件缺失时直接报错。
assets:
- path: rdk
required: true
what: Flexiv RDK 源码或安装包
format: 厂商原始发布格式，不要转换、不要重新打包
naming: 保持厂商原目录名与文件名，一律不要改名
where_used: driver 通过此路径加载 RDK
notes: >
与 flexiv_ros2 humble-v1 配套的是 RDK 1.9.x。仓库外的已编译安装位于
~/flexiv_install（由 scripts/env.sh 与 run_real.sh 注入 LD_LIBRARY_PATH）。
离线安装库在 vendor/rdk_libs/。
# --- 适配器 ----------------------------------------------------------------
driver:
package: arm_driver_flexiv
executable: flexiv_arm_driver
build: colcon
# --- 能力声明 --------------------------------------------------------------
# 框架上层只会调用这里列出的能力。少写一项，上层用到时会启动即报错。
capabilities:
- get_arm_state
- follow_joint_trajectory
- set_named_pose
- servo_joint
- servo_cartesian
- compliant_descent
- set_tool
- switch_tool
- home
- enable
- clear_fault
- soft_drag # 软拖拽，手眼标定走点用
# --- 配置项声明 ------------------------------------------------------------
# 框架启动时按 site_path 去 config/site.yaml 取值，required 的键缺失即报错。
# unit 会打印在人读的错误信息里，避免「填了毫米但代码按米」这类问题。
config_schema:
- key: sn
site_path: robot.robots[].sn
type: str
required: true
doc: 机械臂完整 SN（如 Rizon4-063989）。必须与示教器上显示的一致。
- key: ip
site_path: robot.robots[].ip
type: str
required: true
doc: 控制柜 IP。同一网段的工控机地址填 pc_ip。
- key: pc_ip
site_path: robot.robots[].pc_ip
type: str
required: true
doc: 工控机在本网段的地址，RDK 白名单用。
- key: iface
site_path: robot.robots[].iface
type: str
required: true
doc: 连接控制柜的网卡名（如 enp4s0）。
- key: tool_name
site_path: robot.tool_name
type: str
required: false
default: ""
doc: 示教器 Studio 里的工具坐标系名。留空表示不切换。
本项目要求 tool1 全程保持垂直向下且位姿锁定，需在此指定。
- key: tool_z_extend_m
site_path: robot.tool_z_extend_m
type: float
unit: m
required: false
default: 0.0
doc: >
工具坐标系沿 Z 的延长量。用于补偿「夹具 + 夹持物」超出原质心的距离。
当前 tool1 质心在未装夹具的平行夹爪末端，装上夹具并夹持通止规后需沿 Z 延长。
标定方法见 docs/TOOL_FRAME.md。
- key: ee_z_m
site_path: robot.robots[].servo.ee_z_m
type: float
unit: m
required: true
doc: 视觉跟踪锁高 —— 法兰相对基座系的 Z。
注意与悬高（相对工具系）区分，两者是不同概念。
- key: safe_z_m
site_path: robot.robots[].servo.safe_z_m
type: float
unit: m
required: true
doc: 防撞安全高度。启动归位时先抬到这个高度再水平移动。
devices/robots/flexiv/rdk/README.md
+53
View File
@@ -0,0 +1,53 @@
# devices/robots/flexiv/rdk/ —— 放置非夕 RDK
## 放什么
Flexiv RDK（Robotic Development Kit）的源码或安装包。
## 格式与命名要求（必须遵守）
| 项 | 要求 | 违反的后果 |
|----|------|-----------|
| 格式 | 厂商原始发布格式（`.tar.gz` 需展开，不要留着压缩包） | 驱动加载不到 |
| 目录名 | 保持厂商原目录名（通常是 `flexiv_rdk`） | CMake 的 include 路径失效 |
| 文件名 | 一律不改名 | CMake 查找 `libflexiv_rdk.*.a` 时按原名匹配 |
| 版本 | 在 `../manifest.yaml` 的 `version` 字段记录 | 日后无法排查版本不匹配 |
| 编译产物 | 不要把 `build/` 提交进 git | 仓库膨胀 |
## 版本必须配对（这是已踩过的坑）
项目里同时存在两个 RDK 版本，**不能混用**：
| 用途 | 需要的 RDK 版本 | 位置 |
|------|----------------|------|
| 编译 `flexiv_ros2`（humble-v1，v1.9.1） | **1.9.x** | 已编译安装于 `~/flexiv_install` |
| 单独跑 RDK Python 脚本（软拖拽、切工具） | 2.1.x | `src/flexiv_rdk`（符号链接到仓库外） |
`flexiv_ros2` 是 humble-v1 版本，它期望的 RDK 是 1.9.x。当前 `src/flexiv_rdk` 指向的是
**2.1.x**，两者靠 `~/flexiv_install` 里已编译好的 1.9.1 解耦。
离线安装 1.9.1 需要的静态库在 `vendor/rdk_libs/`，由
`scripts/install_rdk_offline.sh` 校验 sha256 后使用。
## 放好之后怎么让它生效
```bash
# 1. 若 RDK 未编译，先编译并安装到 ~/flexiv_install
bash scripts/build_flexiv_rdk.sh
# 2. 重新编译依赖 RDK 的 ROS 包
source scripts/env.sh
./scripts/build_all.sh
# 3. 确认选型
cat devices/registry.yaml # robots: flexiv
# 4. 实机启动
bash scripts/run_real.sh --arm 89
```
## 排查
- `run_real.sh` 会先调 `scripts/rdk_switch_tool.py` 做预检；失败会直接 `exit 1` 并打印原因。
- 网络不通先跑 `scripts/check_robot_network.sh`。
- RDK Python 冒烟测试：`scripts/test_rdk_python.sh <SN>`。
devices/sensors/orisys/README.md
+80
View File
@@ -0,0 +1,80 @@
# devices/sensors/orisys/ —— Orisys 视触觉（框架核心）
**这是整个框架的核心。** 后续所有 ROS 2 项目都会围绕它开发，它也会持续迭代优化更新，
所以它单独占一个文件夹，不与其它传感器混放。
## 先搞清楚它输出什么
这是上层算法真正依赖的数据契约。**注意单位与坐标系的定义，这是最容易出错的地方。**
| 数据 | 维度 | 说明 | 框架内部话题 |
|------|------|------|--------------|
| 切向力 1 | 标量 | 任意单位（非牛顿），且**基线会漂移** | `~/tangential_1` |
| 切向力 2 | 标量 | 同上，另一正交方向 | `~/tangential_2` |
| 法向力 | 标量 | 同上 | `~/normal` |
| 深度图 | 图像 | 接触面深度 | `~/depth` |
| 力矢量图 | 图像 | 接触点的力矢量可视化 | `~/force_vector` |
### 两个必须记住的坑
**1. 力是任意单位，不是牛顿。** 现有 `site.yaml` 里所有力阈值（`touch_delta`、`press_delta`、
`safe_press_delta` 等）全部是「相对静止均值的**增量**」，不是绝对值。节点启动后会先自动测基线。
改阈值时改的也是增量，不是绝对读数。
**2. 基线会漂移，所以阈值必须相对化。** 写任何新的力判据时，不允许直接比较绝对读数。
**3. 切向力的 X / Y 通道归属尚未确认。** 通止规装配里「接触到面时某一路切向力突变」「拧紧判定时
某一路切向力超阈值」都需要知道是哪个通道。在确认之前，框架按**两个切向通道都监控、任一满足即判定**
来实现，避免猜错通道导致整段流程静默失效。确认后会把配置项收敛为指定通道。
## 厂商物料放置要求
| 项 | 要求 |
|----|------|
| SDK 格式 | 厂商**原始发布格式**，不要转换、不要重新打包 |
| SDK 命名 | **保持厂商原目录名与文件名**，一律不要改名。SDK 内部常按自己包名做导入与资源查找 |
| SDK 版本 | 在 `manifest.yaml` 的 `version` 记录，并写进 `docs/CHANGELOG_ORISYS.md` |
| 型号配置 | 每个型号一份配置放 `models/<型号名>/`，不要混在一起 |
| 相机路径 | **必须用 `/dev/v4l/by-path/` 稳定路径**，不要写 `/dev/video0` —— video 编号会随插拔漂移，这是现有框架已经踩过的坑 |
| 生效 | 放好 SDK → `pip install -e` 或加进 `PYTHONPATH` → 改 `registry.yaml` 选 `orisys` → 改 `site.yaml` 的 `tactile.models.<型号>` → 重启 `scripts/run_tactile.sh` |
## 目录结构
```
sensors/orisys/
├── README.md # 本文件
├── manifest.yaml # 型号清单 + 能力 + 配置项声明
├── sdk/ # ★ 厂商 SDK 原样放置
│ └── README.md # 放进来之后怎么让它生效（见下）
├── models/ # ★ 多个传感器型号
│ ├── README.md
│ └── <型号名>/ # 每个型号一份配置
│ └── config.json
└── driver/ # 适配器：统一成上面的数据契约
```
## 多型号怎么切换
传感器有多个型号，切换只改两处，**不改任何代码**：
1. `devices/registry.yaml` → `sensors: orisys`（选厂商，通常不用改）
2. `config/site.yaml` → `tactile.model: "<型号名>"`，指向 `models/<型号名>/config.json`
型号之间的差异（通道数、内参、标定参数、拼接参数）全部由 `models/<型号名>/` 承载。
上层只订阅统一话题，换型号不影响上层代码。
## 当前状态（迁移参考）
| 现有位置 | 内容 | 迁移去向 |
|----------|------|----------|
| `src/arm_application/arm_application/orisys_tactile.py` | SDK 适配 + 多进程 worker + `load_orisys_from_site` | `driver/` |
| `src/arm_application/arm_application/tactile_node.py` | ROS 节点，双指各一个进程 | `driver/` |
| `src/arm_application/arm_application/tactile_cli.py` | 读数量级 CLI（调试阈值用） | 保留为调试工具 |
| `src/arm_application/arm_application/tactile_interface.py` | 数据类 | `driver/`，其中的 `MockTactile` 类已死，P2 删除 |
| `src/arm_application/config/orisys/dd03.json` | 当前型号 dd03 的厂商配置 | `models/dd03/config.json` |
**已解决的历史坑，迁移时必须保留这些做法：**
- 左右指各起一个独立进程，**不要合并成一个进程双路并行** —— 会抢 CUDA 上下文互相拖慢。
- `tactile.cuda` 保持 `false`，`infer_backend` 用 `cpu`。
- 退出时残留的孤儿 worker 会占死 `/dev/video`，`run_tactile.sh` 里那段 `pkill` 清理必须保留。
devices/sensors/orisys/driver/.gitkeep
View File
devices/sensors/orisys/manifest.yaml
+162
View File
@@ -0,0 +1,162 @@
# =============================================================================
# devices/sensors/orisys/manifest.yaml
#
# Orisys 视触觉传感器驱动清单（框架核心）。
#
# 这里【不放现场数值】。video 路径 / 型号 / 发布率一律在 config/site.yaml。
# =============================================================================
id: orisys
label: Orisys 视触觉传感器
kind: sensor
vendor: orisys
version: "待填 —— 放入 SDK 后在此记录实际版本号"
# 从哪里读出「当前用哪个型号」。值是 config_schema 里的 field key。
# 框架据此叠加该型号的额外能力（型号能力与顶层能力取并集）。
model_selector: model
description: >
视触觉传感器，输出两路切向力 + 一路法向力，以及深度图与力矢量图。
采用双指方案，左右指各一个独立进程（防止抢 CUDA 上下文）。
是本框架的长期核心，后续所有项目围绕它开发。
# 重要：上层算法依赖的数据契约
outputs:
- topic: tangential_1
type: scalar
unit: 任意单位（非牛顿）
notes: 基线会漂移，所有阈值必须按「相对静止均值的增量」使用
- topic: tangential_2
type: scalar
unit: 任意单位（非牛顿）
notes: 与 tangential_1 正交。通止规装配需要确认哪一路对应哪个方向，见 README
- topic: normal
type: scalar
unit: 任意单位（非牛顿）
notes: 法向力。拧紧判定需要「法向力突增」这一条件
- topic: depth
type: image
unit: null
notes: 接触面深度图
- topic: force_vector
type: image
unit: null
notes: 接触点力矢量可视化
# --- 厂商物料放置要求 ------------------------------------------------------
assets:
- path: sdk
required: true
what: Orisys 厂商 SDK（Python 包或原生库）
format: 厂商原始发布格式，不要转换、不要重新打包
naming: 保持厂商原目录名与文件名，一律不要改名
where_used: driver 通过 PYTHONPATH 或 pip install -e 加载
notes: >
SDK 内部常按自己的包名做导入与资源查找，改名会直接导致 ImportError。
- path: models
required: true
what: 各传感器型号的配置
format: 每个型号一个子目录，内含 config.json
naming: 子目录名即型号名，会被 site.yaml 的 tactile.model 引用
where_used: driver 按 site.yaml 选中的型号加载对应配置
# --- 适配器 ----------------------------------------------------------------
driver:
package: arm_driver_orisys
executable: orisys_tactile_driver
build: colcon
notes: >
必须保持「左右指各一个独立进程」的架构，不要合并。
必须保留退出时清理孤儿 worker 的逻辑（否则占死 /dev/video）。
# --- 能力声明 --------------------------------------------------------------
capabilities:
- read_tangential
- read_normal
- read_depth
- read_force_vector
- measure_baseline
# --- 配置项声明 ------------------------------------------------------------
config_schema:
- key: model
site_path: tactile.model
type: str
required: true
doc: 当前使用的传感器型号名。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
- key: left_video
site_path: tactile.left_video
type: str
required: true
doc: >
左指相机路径。必须用 /dev/v4l/by-path/ 下的稳定路径，
不要写 /dev/video0（编号会随插拔漂移）。
- key: right_video
site_path: tactile.right_video
type: str
required: true
doc: 右指相机路径，同样用 by-path 稳定路径。
- key: baseline_seconds
site_path: tactile.baseline_seconds
type: float
unit: s
required: false
default: 2.0
doc: 启动后采集静止基线的时间。所有力阈值都是相对这个基线的增量。
- key: publish_hz
site_path: tactile.publish_hz
type: float
unit: Hz
required: false
default: 20.0
doc: 数据发布率。
- key: cuda
site_path: tactile.cuda
type: bool
required: false
default: false
doc: >
双指并行时保持 false —— 开 GPU 会让两路互相抢 CUDA 上下文，反而更慢。
- key: infer_backend
site_path: tactile.infer_backend
type: str
required: false
default: cpu
doc: 推理后端。cpu / cupy / opencl。双指场景建议 cpu。
# --- 力阈值（通止规装配用）-------------------------------------------------
# 全部是「相对静止均值的增量」，不是绝对值。见 README 的坑点说明。
force_thresholds:
- key: touch_delta
site_path: hole_detect.stage3.touch_delta
unit: 增量
doc: 触面判定 —— 超过这个增量认为接触到了工件表面
- key: screw_done_shear_delta
site_path: system.screw_done_shear_delta
unit: 增量
doc: >
拧紧判定条件之一 —— 切向力超过这个增量。
必须与 screw_done_normal_delta 同时满足才判定拧紧到位。
- key: screw_done_normal_delta
site_path: system.screw_done_normal_delta
unit: 增量
doc: 拧紧判定条件之二 —— 法向力突增。
- key: safe_delta
site_path: hole_detect.stage3.safe_press_delta
unit: 增量
doc: 安全上限。任何时候超过立即抬升并报错。
devices/sensors/orisys/models/README.md
+59
View File
@@ -0,0 +1,59 @@
# devices/sensors/orisys/models/ —— 传感器型号配置
## 放什么
每个传感器型号一个子目录，子目录内存放该型号的厂商配置。
## 格式与命名要求（必须遵守）
| 项 | 要求 | 违反的后果 |
|----|------|-----------|
| 子目录名 | **就是型号名**，会被 `config/site.yaml` 的 `tactile.model` 引用 | 选型找不到配置 |
| 配置文件名 | 固定为 `config.json` | 驱动按固定名加载 |
| 内容格式 | 厂商原始 JSON 结构，不要自己重排字段 | 解析失败 |
| 新增型号 | 从现有型号复制整个目录再改，不要在原型号上改 | 污染在用型号 |
| 生效 | 改 `site.yaml` 的 `tactile.model` → 重启 `scripts/run_tactile.sh` | — |
## 目录结构
```
models/
├── README.md # 本文件
├── dd03/ # 当前在用型号
│ └── config.json # 从 src/arm_application/config/orisys/dd03.json 迁来
└── <新型号>/
└── config.json
```
## 型号之间的差异由这里承载
不同型号可能在以下方面不同，**全部通过这个目录承载，上层代码不感知**：
- 相机分辨率与内参（影响深度图与力矢量图的标定）
- 力的通道数（有的型号只有 1 路切向力）
- 基线与噪声特性（影响阈值标定）
- 左右指的拼接参数
- 是否需要旋转配置（现有 `site.yaml` 里的 `left_rotation` / `right_rotation`）
## 新增一个型号的完整步骤
```bash
# 1. 复制现有型号作为模板
cp -r devices/sensors/orisys/models/dd03 devices/sensors/orisys/models/<新型号>
# 2. 按厂商资料修改 config.json 里的分辨率/内参/通道数
# 3. 在 site.yaml 里切换型号
# tactile:
# model: "<新型号>"
# 4. 重启并对比读数
bash scripts/run_tactile.sh --cli
```
## 关键提醒
换型号之后，**必须重新测基线并重新标定所有力阈值**。
`site.yaml` 里的 `touch_delta` / `screw_done_shear_delta` / `screw_done_normal_delta` /
`safe_delta` 都是「相对静止均值的增量」，不同型号的噪声水平不同，直接沿用旧阈值会导致
接触判定误触发或完全不触发。
devices/sensors/orisys/models/dd03/config.json
+35
View File
@@ -0,0 +1,35 @@
{
"name": "dd03",
"type": "single_camera",
"version": "1.0",
"camera": {
"resolution_raw": [1280, 720],
"fps": 30,
"resolution_stitch": [420, 450],
"source_scale": 0.4,
"old": [672, 728, 1, 0.5],
"work": [504, 546, 1],
"work2": [420, 450, 0.8],
"work3": [350, 375, 4, 0.7],
"work4": [280, 300, 4, 0.4, 0.2]
},
"tracker": {
"adaptive": true,
"lower_threshold": 12.0,
"upper_threshold": 18.0,
"loop_closure_interval": 1,
"algorithm": "dis",
"dis_preset": "medium",
"dis_mode": 3,
"algorithm_list": ["raft", "dis"],
"hhd_downsample": 3
},
"contact": {
"min_contour_area": 3000,
"flow_threshold": 0.2,
"div_threshold": 0.022,
"expansion_threshold": 0.01,
"kernel_size": [5, 5],
"denoise_sigma": 12
}
}
devices/sensors/orisys/sdk/README.md
+70
View File
@@ -0,0 +1,70 @@
# devices/sensors/orisys/sdk/ —— 放置 Orisys 厂商 SDK
## 放什么
Orisys 视触觉传感器的厂商 SDK（Python 包、原生库、或 ROS 包）。
## 格式与命名要求（必须遵守）
| 项 | 要求 | 违反的后果 |
|----|------|-----------|
| 格式 | 厂商原始发布格式。若是 `.whl`/`.tar.gz`，展开后放进来，不要留压缩包 | 无法 import |
| 目录名 | 保持厂商原包名（如 `orisys_sdk/`），**一律不要改名** | Python 内部按包名做相对导入与资源查找，改名直接 `ImportError` |
| 文件名 | 一律不改名 | 原生库按 `.so` 原名加载 |
| 版本 | 记录进 `../manifest.yaml` 的 `version` | 日后无法排查版本问题 |
| 原生库 | 若含 `.so`，确认架构是 x86_64 | 加载报错 |
| 编译产物 | 不要把 `build/` 提交进 git | 仓库膨胀 |
## 放好之后怎么让它生效
```bash
# 方式 A：可编辑安装（推荐，方便日后迭代 SDK）
conda activate ros_flexiv
pip install -e devices/sensors/orisys/sdk
# 方式 B：加入 PYTHONPATH（不改环境，只在当前 shell 生效）
export PYTHONPATH="$PWD/devices/sensors/orisys/sdk:$PYTHONPATH"
# 冒烟验证
python3 -c "import orisys_sdk; print(orisys_sdk.__file__)"
```
然后：
```bash
# 1. 确认选型
cat devices/registry.yaml # sensors: orisys
# 2. 在 site.yaml 里选中型号
# tactile:
# model: "dd03" # 必须与 models/ 下的子目录名一致
# left_video: "..." # 必须用 /dev/v4l/by-path/ 稳定路径
# right_video: "..."
# 3. 确认型号配置存在
ls devices/sensors/orisys/models/dd03/config.json
# 4. 启动
bash scripts/run_tactile.sh # 图传 + 状态话题
bash scripts/run_tactile.sh --cli # 只读数值，用来调阈值
```
## 迭代 SDK 时的注意事项
这个 SDK 会持续迭代更新，升级时按下面顺序做：
1. 把新版本放在原目录名旁边（如 `sdk_new/`），**不要直接覆盖旧版本**；
2. 用 `--cli` 对比新旧版本的读数（量纲、基线漂移、噪声水平是否变化）；
3. 读数一致后再切换；
4. 更新 `../manifest.yaml` 的 `version` 字段；
5. **如果量纲变了**，`site.yaml` 里所有力阈值（`touch_delta` / `screw_done_shear_delta` /
`screw_done_normal_delta` / `safe_delta`）都需要重新标定 —— 这些阈值是相对基线的增量，
单位一变全部失效。
## 已踩过的坑（不要重犯）
1. **左右指必须各一个独立进程**，不要合并成单进程双路 —— 会抢 CUDA 上下文互相拖慢。
2. **`cuda` 保持 `false`**，`infer_backend` 用 `cpu`。
3. **退出时残留的孤儿 worker 会占死 `/dev/video`**，导致下次启动失败。
`run_tactile.sh` 里那段 `pkill` 清理逻辑必须保留。
4. **不要用 `/dev/video0`**，用 `/dev/v4l/by-path/pci-...-video-index0`。
devices/sim/gazebo/README.md
+95
View File
@@ -0,0 +1,95 @@
# devices/sim/gazebo/ —— Gazebo 仿真平台
## 先读这段：当前仿真是假的
**现在项目里的「仿真」并不是 Gazebo。** 实际情况是：
- `src/arm_description/urdf/arm.gazebo.xacro` 是零引用的死文件；
- `arm_bringup/stack.launch.py` 的 `isaac` 分支只有一行日志，没有任何实际动作；
- `run_sim.sh` 走的是 `sim.launch.py`，底层是 **mock 驱动**（`mock_arm_driver.py`），
即用假数据模拟机器人状态，不是物理仿真；
- Gazebo 从未真正接线。
所以这份文档分两部分：**第一部分是怎么把 Gazebo 真正跑起来**（当前必须做的前置工作），
**第二部分才是日常怎么用**。不要跳过第一部分。
---
## 第一部分：把 Gazebo 真正接起来（前置工作）
要让 Gazebo 跑起来，缺的是这几块，按顺序补齐：
### 1. URDF 加 Gazebo 标签
现有 `arm_description/urdf/arm.gazebo.xacro` 是死文件，需要重写。每个 link 必须补：
```xml
<!-- 视觉碰撞体 -->
<gazebo reference="${prefix}link1">
<material>Gazebo/Grey</material>
</gazebo>
```
并在 `arm.ros2_control.xacro` 里把 `ros2_control` 标签的 hardware 插件从
`flexiv_hardware/FlexivHardware` 换成 `gazebo_ros2_control/GazeboSystem`。
### 2. 关节传动与控制器
Gazebo 需要 `transmission` 标签把关节映射到 `ros2_control`。控制器配置可以直接复用
`arm_control/config/controllers.yaml` 里的 `rizon_arm_controller`，只需把关节名前缀
从真实 SN（如 `Rizon4-063989_`）换成仿真专用的前缀（如 `sim_`）。
### 3. 传感器插件
| 仿真对象 | 插件 | 对应真实设备 |
|----------|------|--------------|
| 腕部相机 | `libgazebo_ros_camera.so` | D405 |
| 手外相机 | `libgazebo_ros_camera.so` | D435 |
| 深度 | `libgazebo_ros_depth_camera.so` | 对齐深度流 |
| 触觉力 | **需要自己写插件** | Orisys |
**最关键的一条：Orisys 视触觉没有现成的 Gazebo 插件。**
3 路切向力 + 1 路法向力 + 深度图 + 力矢量图 这套数据需要自定义 Gazebo 插件来模拟。
在它做出来之前，仿真里**无法验证通止规装配的力控部分**（S07 接触、S08 拧紧判定）。
建议的自定义插件做法：订阅 Gazebo 的接触传感器数据，按接触深度和相对滑动速度合成
出一组「类触觉」读数，再按真实传感器的量纲（任意单位增量）发布到同样的内部话题上。
### 4. 工件与通止规模型
需要给工件（含 k0 中心孔、k1/k2 螺纹孔）和通止规建简化网格，并保证：
- 工件的 k0/k1/k2 三个点在模型坐标系里的位置与实际一致 —— 否则推理出的坐标映射到仿真里会对不上；
- 螺纹孔要能表达「拧入」的几何阻力，否则 S08 的力反馈无法触发。
---
## 第二部分：日常怎么用
### 放置格式与命名要求
| 类型 | 放哪 | 格式 | 命名要求 |
|------|------|------|----------|
| 世界文件 | `worlds/` | `.world` 或 `.sdf` | `<场景名>.world`，如 `assembly.world` |
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 必须以 `sim_` 为关节前缀，与真实臂区分 |
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | 单位必须是米。用毫米建模的必须先缩放到米 |
### 导入一个新场景
1. 把 `.world` 放进 `worlds/`，网格放进 `models/<模型名>/`
2. 改 `config/site.yaml` 的 `sim.world` 指向该文件
3. 改 `devices/registry.yaml` 的 `sim: gazebo`（通常已是）
4. 启动 `scripts/run_sim.sh`
### 修改哪个配置才会生效
| 想改什么 | 改哪里 |
|----------|--------|
| 用哪个场景 | `site.yaml` → `sim.world` |
| 仿真是否启用物理 | `site.yaml` → `sim.physics_enable` |
| 仿真里的悬高/速度 | **复用真实的那套参数**（`servo.*` / `hole_detect.stage2.*`），不要另建一套，否则仿真调好的值搬到真机不生效 |
| 关节前缀 | `site.yaml` → `sim.joint_prefix`（默认 `sim_`） |
> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何「仿真专用参数」都是未来
> 仿真调参搬到真机失效的隐患。只允许物理引擎本身的参数（如重力、步长）放在 `sim.*` 下。
devices/sim/gazebo/manifest.yaml
+91
View File
@@ -0,0 +1,91 @@
# =============================================================================
# devices/sim/gazebo/manifest.yaml
#
# Gazebo 仿真平台清单。
#
# 重要：本项目的 Gazebo 目前【尚未接线】。详见本目录 README.md。
# 这份清单声明的是「接线完成之后」应有的配置项，并把 world 设为非必填，
# 这样在仿真还没搭建好时不会阻塞 device_check。
# =============================================================================
id: gazebo
label: Gazebo 仿真
kind: sim
vendor: openrobotics
version: "待填 —— 接线后记录 Gazebo 版本"
description: >
物理仿真平台。当前项目的「仿真」实际是 mock 驱动（假数据），Gazebo 尚未接线：
arm.gazebo.xacro 零引用、stack.launch.py 的 isaac 分支只有日志、没有 world 文件。
接线步骤见本目录 README.md 的第一部分。
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
# --- 物料放置要求 ----------------------------------------------------------
assets:
- path: worlds
required: false
what: Gazebo 世界文件（.world / .sdf）
format: Gazebo 原生格式
naming: 场景名.world，如 assembly.world
where_used: 由 site.yaml 的 sim.world 指向
- path: models
required: false
what: 工件 / 夹具 / 通止规模型
format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/
naming: >
模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标
与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。
where_used: Gazebo 场景加载
- path: urdf
required: false
what: 仿真版机器人描述
format: .urdf / .xacro，需含 gazebo 与 ros2_control 标签
naming: 关节前缀必须以 sim_ 开头，与真实臂（Rizon4-xxxxxx_）区分
where_used: 仿真机器人加载
- path: sensor_plugins
required: false
what: 传感器插件（含 Orisys 触觉自定义插件）
format: 插件源码 + CMakeLists.txt
naming: 插件类名以 Orisys 开头
where_used: 仿真传感器数据发布
# --- 适配器 ----------------------------------------------------------------
driver:
package: arm_sim_gazebo
executable: gazebo_sim_launcher
build: colcon
notes: >
当前尚未实现。接线前 run_sim.sh 走的是 arm_bringup 的 mock 驱动路径。
# --- 配置项声明 ------------------------------------------------------------
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
# 避免「仿真调好的值搬到真机不生效」。只有物理引擎自身的参数放在 sim.* 下。
config_schema:
- key: world
site_path: sim.world
type: str
required: false
default: ""
doc: >
仿真世界文件路径。留空表示使用 Gazebo 默认空场景。
Gazebo 接线完成后填真实路径，如 devices/sim/gazebo/worlds/assembly.world。
- key: physics_enable
site_path: sim.physics_enable
type: bool
required: false
default: true
doc: 是否启用物理引擎。关闭可用于纯运动学验证。
- key: joint_prefix
site_path: sim.joint_prefix
type: str
required: false
default: sim_
doc: >
仿真关节名前缀。必须与真实臂的前缀（Rizon4-xxxxxx_）不同，
否则仿真与真机的 TF 会互相污染。
devices/sim/gazebo/models/.gitkeep
View File
devices/sim/gazebo/sensor_plugins/.gitkeep
View File
devices/sim/gazebo/urdf/.gitkeep
View File
devices/sim/gazebo/worlds/.gitkeep
View File
docs/ARCHITECTURE.md
+54
View File
@@ -0,0 +1,54 @@
# 架构
## 进程（实机跟踪）
```
run_real.sh Flexiv RDK + ros2_control + named_pose
run_camera_d405.sh 该臂 D405 → /d405_left 或 /d405_right（彩图+对齐深度+预览）
run_visual_servo_rt.sh
perception_node YOLO + 平面中心 → /visual_servo/filtered_point
servo_node 100 Hz，法兰 hover / 锁高 → JointTrajectory
static TF flange → camera（手眼 T）
run_gripper.sh 可选，IPC Modbus PGEA 或 RGI
run_dual_gripper.sh 双爪同一节点
run_hole_detect.sh 入孔检测（当前臂 D405）
run_tactile.sh Orisys 左右指视触觉
```
RDK 独占：`run_real.sh` 与 `run_handeye.sh teach`（RDK 拖拽）不能同时开。
D405 USB 独占：相机节点与示教/标定自己开会互斥；wrapper 会先停旧相机。
## 目录
```
~/ros2/
├── README.md # 克隆后怎么跑
├── config/
│ ├── site.example.yaml # 模板（入库）
│ └── site.yaml # 本机配置（不入库）
├── docs/ # 说明；总表见 WORKSPACE.md
├── models/yolo/ # 工件权重二进制不入库
├── models/hole_insert/ # 入孔权重二进制不入库
├── scripts/ # 唯一操作入口
├── src/ # ROS 2 包
├── build/ install/ log/ # colcon，不入库
```
## 配置流
```
config/site.yaml
→ scripts/lib/export_site_env.py SITE_* 环境变量 + ROS 覆盖字典
→ run_*.sh / visual_servo.launch.py
→ 节点参数（覆盖包内 perception.yaml / servo.yaml）
```
包内 yaml 只是默认值，日常不要改。
## 跟踪几何
手眼 `T_cam_to_gripper`（`handeye_result.yaml`）把相机点变到法兰。
`servo.xy_track`：
- `hover`：基座系法兰 XY = 工件 3D 点 XY，任意 `ee_z_m`
- `lookat`：光影像中心对准工件（倾斜相机时只有某一高度法兰才在正上方）
docs/GRIPPER.md
+68
View File
@@ -0,0 +1,68 @@
# 大寰夹爪（PGEA + RGI）
两只爪都走工控机 **USB–RS485 + 官方 Modbus RTU**，不走 Flexiv `Gripper` / Studio `GripperDahuanModbus`。示教器里关掉对应设备，避免抢总线。`run_real.sh` 保持 `load_gripper:=false`。
| 臂 | 型号 | 串口 | 脚本 |
|----|------|------|------|
| 左 89 | PGEA-100-20-W 平行 | `/dev/ttyUSB0` | `run_gripper.sh --arm 89` |
| 右 91 | RGI-100-30-O-S-M1 旋转 | `/dev/ttyUSB1` | `run_gripper.sh --arm 91` |
端口、行程、力/速写在 `site.yaml`：`robots[].gripper`（每臂）+ 全局 `gripper` / `gripper.parallel` / `gripper.rotary`。双爪 ROS 节点读 `robots[]` 两侧。不要改包内 Python 默认值。
源码：`src/arm_application/arm_application/dh_official.py`、`dual_gripper.py`、`dh_pgea.py`。消息：`arm_msgs`（**不要**往官方 `flexiv_msgs` 塞自定义夹爪类型）。
## 命令行
```bash
# 左臂平行爪
bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe
bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init # 上电第一次
bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open
bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli close
bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli pos 500 # 0=合 1000=开（‰）
# 右臂旋转爪
bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open
bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
```
PGEA 力/速：`gripper.force_pct`（最低 20）`speed_pct` `open_position` `close_position`。RGI 不要 `auto_init: true`（`robots[]` 里已关）。
## ROS 节点
单爪：
```bash
bash ~/ros2/scripts/run_gripper.sh --arm 89 # /gripper/open /gripper/close
bash ~/ros2/scripts/run_gripper.sh --arm 91 # dual_gripper 仅 right
```
双爪同时：
```bash
bash ~/ros2/scripts/run_dual_gripper.sh
```
| 名称 | 类型 |
|------|------|
| `/dual_gripper_driver/left/command` | `arm_msgs/msg/GripperCommand` |
| `/dual_gripper_driver/right/command` | 同上 |
| `/dual_gripper_driver/left/status` | `arm_msgs/msg/GripperStatus` |
| `/dual_gripper_driver/right/status` | 同上 |
| `/dual_gripper_driver/left/execute` | `arm_msgs/srv/GripperExecute` |
| `/dual_gripper_driver/right/execute` | 同上 |
`command` 字段：`init` `open` `close` `move`；右爪另有 `rotate` `rotate_by` `rotate_revolutions` `home_rotation`。
```bash
ros2 service call /dual_gripper_driver/left/execute arm_msgs/srv/GripperExecute \
"{command: {command: 'open', wait_complete: true, timeout: 10.0}}"
ros2 service call /dual_gripper_driver/right/execute arm_msgs/srv/GripperExecute \
"{command: {command: 'rotate', position: 90.0, wait_complete: true, timeout: 10.0}}"
```
USB 未插齐时节点仍可起来（`allow_offline:=true`），对应侧 `status.error_msg` 会写连接失败。
用户需在 `dialout`：`sudo usermod -aG dialout $USER`。
docs/HANDEYE.md
+44
View File
@@ -0,0 +1,44 @@
# 手眼标定（眼在手上）
每台机械臂一份结果，**禁止混用**：
| 臂 | 结果 | 示教关节 | 工作目录 |
|----|------|----------|----------|
| 89 左 | `src/arm_application/config/handeye_result_89.yaml` | `handeye_poses_89.yaml` | `/tmp/handeye_calib_89` |
| 91 右 | `src/arm_application/config/handeye_result_91.yaml` | `handeye_poses_91.yaml` | `/tmp/handeye_calib_91` |
`T_4x4` = **相机 → 法兰**。跟踪和静态 TF 都读当前 `--arm` 对应的文件。
棋盘尺寸只改 `site.yaml` → `handeye.board_*` / `square_size_m`（内角点 = 方格数 − 1）。两臂可共用同一块板。
## 流程
`teach`（RDK 软拖拽）与 `run_real.sh` **互斥**。全程带同一 `--arm`。
```bash
# 1) 停掉 run_real.sh 和视觉伺服
# 2) 示教 8–12 个姿态（棋盘清晰、占画面 30–70%，姿态拉开）
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
# 窗口：d=拖拽 Enter=拍照 q=结束
# 接着采、不清空旧点： teach --append
# 3) 写入该臂的 handeye_poses_XX.yaml
bash ~/ros2/scripts/run_handeye.sh --arm 89 merge
# 4) 再开实机 + 相机，棋盘不要动
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate
```
91 把 `--arm 89` 改成 `--arm 91`。91 的结果文件初始是 `uncalibrated: true`，标定成功后才会写出 `T_4x4`。
查看路径与板参数：`bash ~/ros2/scripts/run_handeye.sh --arm 89 show`
## 结果怎么用
`visual_servo.launch.py` 读当前臂 `T_4x4`，发布 `{ee_frame}` → `{camera_frame}` 的 static TF。
`servo_node` 用同一份 T 做 `hover` / `lookat`。换结果后**重启** `run_visual_servo_rt.sh --arm …`。
装配 HMI（`ros_flexiv`）选左/右臂后，读同一份 `handeye_result_89.yaml` / `_91.yaml`。
docs/HOLE_DETECT.md
+164
View File
@@ -0,0 +1,164 @@
# 入孔检测
只做视觉定位（像素孔心），**不包含**装配插入运动 / HMI。跟踪仍用 `run_visual_servo_rt.sh`。
日常改 **`config/site.yaml` → `hole_detect:`**（Hough、居中门控、间距都会覆盖包内 yaml）。
`src/arm_application/config/hole_detect.yaml` 只是缺省值。
源码：
| 文件 | 作用 |
|------|------|
| `src/arm_application/arm_application/hole_detector.py` | 统一入口：`yolo` / `hough` / `two_stage` |
| `src/arm_application/arm_application/hole_detect_hough.py` | YOLO 工件框 + HoughCircles 左右孔 |
| `src/arm_application/arm_application/hole_detect_node.py` | 订当前臂 D405 彩图 |
| `src/arm_application/config/hole_detect.yaml` | `two_stage` 参数 |
| `src/arm_application/config/hole_insert.yaml` | `yolo`/`hough` 参数 |
| `models/hole_insert/` | 孔模型权重（二进制不进 git） |
## 跑
先开该臂相机，再开检测：
```bash
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
```
91 把 `--arm` 换成 `91`（话题 `/d405_right/...`）。
`site.yaml`：
```yaml
hole_detect:
backend: "two_stage" # two_stage | yolo | hough
weights: "/home/USER/ros2/models/hole_insert/best.pt"
workpiece_weights: "" # 空 = paths.yolo_weights
conf: 0.25
hough_param2: 26
hole_distance_min: 150
```
- `two_stage`：用工件 YOLO 找 bbox，再在 ROI 里 Hough 找左右螺纹孔（原 `camera_test/hole_detect.py`）。
- `yolo`：模型框直接当孔（原 `hole_insert` HoleDetector）。
- `hough`：整幅图圆检测，不用权重。
权重放到 `~/ros2/models/hole_insert/best.pt`。`two_stage` 的工件模型默认就是跟踪用的 `paths.yolo_weights`。
## ROS
| 话题 | 类型 |
|------|------|
| `/hole_detect/result` | `arm_msgs/msg/HoleDetections` |
| `/hole_detect/holes` | `std_msgs/Float32MultiArray` `[ok, n, u,v,r,conf, …]` |
| `/hole_detect/overlay` | `sensor_msgs/Image` 画框画孔 |
改 `hole_detect.yaml` 的 Hough / 居中门控后重启 `run_hole_detect.sh`。`two_stage` 会做时序稳帧（`stabilize_*`）。
## 三阶段串联（推荐日常入口）
一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
```bash
# 终端 1 —— 控制栈（一直开着）
bash ~/ros2/scripts/run_real.sh --arm 89
# 终端 2 —— 相机预览（一直开着，看跟踪画面）
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# 终端 3 —— 串联（搜到孔后轻压保持，不拧）
bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔 + 搜孔轻压
bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 换 k2
bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
# 若这一次就要拧入（一般留给后续阶段）：
bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
```
`run_pipeline.sh` 是 `run_all.sh` 的别名。
**跑之前确认**：夹爪夹着通止规；工件在相机大概能看见的位置（一阶段会自己找）。
左指触觉如果掉线会自动降级成单指。
盯这几行就知道跑到哪了：
1. `[pipeline] === stage1_track ===` → 等跟踪锁定
2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
`/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
`robots[89].servo.hole_hover_z_m`（拍照悬停高）。
分阶段单独跑仍用 `run_visual_servo_rt.sh`、`run_stage2_locate.sh`、
`run_stage3_search.sh`，下面两节。
## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑）
先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 →
深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动夹爪/通止规轴线到目标孔正上方 →
复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。
```bash
# 前置：T1 run_real.sh --arm 89；T3 run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程（自动拉起触觉服务，仅左臂）
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只垂直+悬停锁定，用来调高度
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2 # 对准 k2（默认 k1）
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高，不动 yaml
```
悬停锁高接口：`site.yaml → robots[89].servo.hole_hover_z_m`（91 在同名段）。
工件权重：`site.yaml → hole_detect.workpiece_weights`（默认 `models/hole_insert/detect_best_1.onnx`）。
其余二阶段参数：`site.yaml → hole_detect.stage2:`（对准容差、迭代次数、垂直判据等）。
| 话题 | 类型 |
|------|------|
| `/stage2/state` | `std_msgs/String` 阶段状态机 |
| `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
| `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |
## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）
前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
1. **verify** — 垂直度核验；Orisys 双指触觉话题判活（connected=true 才算数），
并自动测 ~1.2s **静止基线 μ0/σ0**（日志打印「触觉基线/派生阈值」两行）
2. **approach** — 1.5mm 小步下降，`fnormal > μ0 + max(4σ0, touch_delta)` 判定触面，记录 `z_contact`
3. **seat** — Z 力闭环建立轻触（fn ≈ 触面阈值 + `press_delta`）
4. **spiral** — 阿基米德螺旋搜孔：XY 沿螺旋走（螺距 `spiral_pitch_mm`，保证轨迹重叠
覆盖），Z 由力闭环维持轻触；`fnormal` 相对接触均值跌落 `drop_ratio` 持续
`drop_frames` 帧 → **尖端入孔**。找不到则每次扩大 `spiral_expand_step_mm` 再搜
5. **center** — 入孔点十字微扫（±`center_scan_mm`），剪切力最小点 = 孔心
6. **insert** — 力限下压 `insert_depth_mm`（超限回退微抖重试 `insert_retry` 次）；
`screw_pitch_mm ≠ 0` 时按螺距同步绕世界 Z 旋入（**螺纹通止规**：>0 右旋 / <0 左旋；
光面规保持 0）。`insert_depth_mm: 0` = 只搜孔定心不插入
7. **hold** — 锁定关节，发布结果
```bash
# 前置：T1 run_real.sh --arm 89；二阶段已完成（臂保持不动）
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # M6 螺纹规旋入
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准一次
```
**力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且
夹持基线会漂移，节点每次启动自动测基线后派生全部阈值。要调灵敏度就改
`site.yaml → hole_detect.stage3:` 的 `touch_delta / press_delta / safe_press_delta /
shear_delta`（日志里能看到实际派生值）。
安全（任一触发立即抬 5mm 中止）：Orisys `fnormal > μ0 + safe_press_delta`；腕部 F/T
`|F| > ft_safe_n`（订 `/<sn>/flexiv_robot_states`，订不到只警告）；触觉断流 >
`tactile_timeout_s`。切向力超 `μ0shear + shear_delta` 自动抬 0.3mm 减拖。
| 话题 | 类型 |
|------|------|
| `/stage3/state` | `std_msgs/String` 阶段状态机 |
| `/stage3/result` | `std_msgs/String` JSON：孔心基座坐标、触面 Z、插入深度、峰值力 |
结果同时落盘 `/tmp/stage3_result.json`。退出码：0 成功；3 未触面；4 搜孔失败；
5 插入受阻；6 安全中止；7 触觉断流。
docs/INTERFACES.md
+149
View File
@@ -0,0 +1,149 @@
# 对外接口
三层：操作员脚本、ROS 话题/服务、可 import 的 Python。业务代码不要直接调 `rizon_arm_controller`。
---
## 1. 操作员脚本（推荐）
所有脚本在 `scripts/`。先 `source scripts/env.sh`。
| 脚本 | 输入 | 输出 / 行为 |
|------|------|-------------|
| `env.sh` | — | conda + Humble + `install/setup.bash` + `SITE_*` |
| `build_all.sh` | 源码 | colcon 编 `arm_*` + `flexiv_*` |
| `run_real.sh [--sn SN]` | `site.yaml` robot | 实机 stack，`backend:=real` |
| `run_camera_d405.sh [--preview]` | `camera.*` | 发布图像；可选 Qt 四宫格 |
| `run_visual_servo_rt.sh [--control]` | `paths` `yolo` `servo` | perception + servo + 手眼 TF |
| `go_home.sh` | `go_home.*` | 杀伺服后 `/set_named_pose` |
| `run_handeye.sh teach\|merge\|calibrate\|show` | `handeye.*` | 示教 / 写入 poses / 解 T |
| `run_gripper.sh --arm 89\|91 --cli …` | `robots[].gripper` + `gripper.*` | PGEA / RGI Modbus |
| `run_dual_gripper.sh` | 两侧 `robots[].gripper` | 双爪 ROS 节点 |
| `run_hole_detect.sh --arm …` | `hole_detect.*` `camera` | 入孔像素 |
| `run_tactile.sh [--cli]` | `tactile.*` | Orisys 左右指 |
| `export_yolo_trt.sh [onnx] [imgsz]` | 权重 | 同名 `.engine` |
| `show_site_config.sh` | `site.yaml` | 打印解析结果 |
| `rdk_switch_tool.sh --list` | SN | Studio 工具列表 |
| `run_sim.sh` | — | mock stack |
| `convert_traj.sh FILE.traj` | Flexiv 轨迹 | `*_trajectory.yaml` |
环境变量契约由 `scripts/lib/export_site_env.py` 定义：`flatten()`、`perception_overrides()`、`servo_overrides()`、`gripper_overrides()`。
---
## 2. ROS 接口（稳定）
### 机械臂（`run_real.sh`）
| 名称 | 类型 | 方向 | 说明 |
|------|------|------|------|
| `/set_named_pose` | `arm_msgs/srv/SetNamedPose` | 应用 → 控制 | `home` `ready` `inspect` `pos_left` |
| `/get_arm_state` | `arm_msgs/srv/GetArmState` | 查询 | 关节 + EE 位姿 |
| `/arm_status` | `arm_msgs/msg/ArmStatus` | 状态 | 模式 / 是否在动 |
| `/follow_joint_trajectory` | `arm_msgs/action/FollowJointTrajectory` | 应用 → 适配器 | 应用只走这个 |
| `/rizon_arm_controller/joint_trajectory` | `trajectory_msgs/JointTrajectory` | 伺服 → 驱动 | **仅 servo_node 写** |
| `/joint_states` | `sensor_msgs/JointState` | 驱动 → 全局 | |
| `/tf` `/tf_static` | TF | | 基座 / 连杆 / 法兰 / 手眼 |
命名位姿表：`src/arm_control/config/named_poses.yaml`。
```bash
ros2 service call /set_named_pose arm_msgs/srv/SetNamedPose \
"{pose_name: 'home', velocity_scale: 0.25, acceleration_scale: 0.25}"
```
### 相机（`realsense_color_pub`）
| 话题 | 类型 | 说明 |
|------|------|------|
| `/d405/color/image_raw` | `sensor_msgs/Image` bgr8 | 彩图 |
| `/d405/color/camera_info` | `sensor_msgs/CameraInfo` | 内参 |
| `/d405/aligned_depth_to_color/image_raw` | `sensor_msgs/Image` 16UC1 | 对齐深度，毫米 |
`d405` 来自 `camera.name`。QoS：SensorData（BEST_EFFORT）。
### 感知 / 伺服
| 名称 | 类型 | 说明 |
|------|------|------|
| `/visual_servo/filtered_point` | `geometry_msgs/PointStamped` | 相机系工件点，perception → servo |
| `/workpiece/detection` | `std_msgs/Float32MultiArray` | `[x1,y1,x2,y2,conf,cls,cx,cy,z]`，空=清绿框 |
| `/cmd_vel` | `geometry_msgs/Twist` | 伺服调试用，真机动臂走轨迹 |
`PointStamped.header.frame_id` = `d405_color_optical_frame`。
### 夹爪（IPC Modbus，`arm_msgs`）
PGEA 单爪节点（`--arm 89` 无 `--cli`）：
| 服务 | 类型 |
|------|------|
| `/gripper/open` | `std_srvs/srv/Trigger` |
| `/gripper/close` | `std_srvs/srv/Trigger` |
双爪 / 右爪 RGI：
| 名称 | 类型 |
|------|------|
| `/dual_gripper_driver/{left,right}/command` | `arm_msgs/msg/GripperCommand` |
| `/dual_gripper_driver/{left,right}/status` | `arm_msgs/msg/GripperStatus` |
| `/dual_gripper_driver/{left,right}/execute` | `arm_msgs/srv/GripperExecute` |
开合力/速在 `site.yaml` → `gripper` 与 `robots[].gripper`。
### 入孔检测（`hole_detect`）
| 话题 | 类型 |
|------|------|
| `/hole_detect/result` | `arm_msgs/msg/HoleDetections` |
| `/hole_detect/holes` | `std_msgs/Float32MultiArray` |
| `/hole_detect/overlay` | `sensor_msgs/Image` |
### 视触觉（`tactile` 节点）
| 话题 | 类型 |
|------|------|
| `/tactile/left/status` | `arm_msgs/msg/TactileStatus` |
| `/tactile/right/status` | `arm_msgs/msg/TactileStatus` |
---
## 3. `site.yaml` 契约
字段说明见 [SITE_CONFIG.md](SITE_CONFIG.md)。Launch 覆盖键：
- 感知：`perception_overrides()` → `weights` `conf` `imgsz` `backend` …
- 伺服：`servo_overrides()` → `ee_z_m` `xy_track` `v_max_xy` `kp_xy` …
改 yaml **不会**热加载权重 / 分辨率 / 锁高（`overlay_hold_sec` 相机会跟）。重启对应脚本。
---
## 4. Python（二次开发）
```python
from arm_application.yolo_detector import YoloDetector, resolve_weights, pick_best
from arm_application.dh_pgea import DhPgeaGripper
from arm_application.dual_gripper import DualArmGripperManager, load_from_site
from arm_application.hole_detector import HoleDetector
from arm_application.orisys_tactile import make_tactile, load_orisys_from_site
from arm_application.geom_ik import load_handeye_T, geometric_jacobian, dls_twist
from arm_application.handeye_lib import calibrate_hand_eye
from arm_application.plane_feature import geometric_center_on_plane
```
模块职责写在 `src/arm_application/arm_application/__init__.py`。
不要从业务节点里 `import flexivrdk`（ABI 在 `rdk_py310`）。夹爪走 Modbus；示教拖拽走 `scripts/rdk_drag_bridge.py` 子进程。
---
## 5. 坐标系
| 帧 | 谁发布 | 含义 |
|----|--------|------|
| `{SN}_base_link` | robot_state_publisher | 基座 |
| `{SN}_flange` | robot_state_publisher | 法兰 |
| `d405_color_optical_frame` | 手眼 static TF | 相机光心，Z 朝外 |
`hover`：基座 XY 上法兰对齐工件。`ee_z_m`：法兰在基座 Z。
docs/QUICKSTART.md
+67
View File
@@ -0,0 +1,67 @@
# 快速上手
更完整的目录说明：[WORKSPACE.md](WORKSPACE.md)。
## 依赖
- Ubuntu 22.04 + ROS 2 Humble
- Conda 环境 `ros_flexiv`（本仓库脚本默认这个名字）
- Flexiv RDK Python：`/home/cc/rdk_py310/bin/python`（flexivrdk 1.9.x）
- 工控机与机器人在同一网段（例：`192.168.2.10` ↔ `192.168.2.101`）
- Intel RealSense D405，`pyrealsense2`
- YOLO：`ultralytics`；TensorRT：`tensorrt-cu12`（不要装 meta 包 `tensorrt`）
- 夹爪：`pyserial`，USB–RS485 → `/dev/ttyUSB0`（PGEA）`/dev/ttyUSB1`（RGI）
每次新开终端：
```bash
conda activate ros_flexiv
source ~/ros2/scripts/env.sh
```
## 配置
```bash
cp ~/ros2/config/site.example.yaml ~/ros2/config/site.yaml
```
必填：`robot.sn`、`robot.robots[].ip`、`camera.serial`（多相机时）、`paths.yolo_weights`。
查看解析结果：
```bash
bash ~/ros2/scripts/show_site_config.sh
```
## 编译
```bash
cd ~/ros2
./scripts/build_all.sh
source scripts/env.sh
```
只改 Python 应用时，develop 安装会直接读 `src/`。改了 `setup.py` 的 `console_scripts` 后需要再编一次 `arm_application`。
## 第一次实机
1. 示教器：AUTO + Remote Ethernet，清故障。关 Studio。
2. `bash ~/ros2/scripts/test_rdk_python.sh`（可选）
3. T1 `run_real.sh`，T2 `run_camera_d405.sh --preview`
4. 手眼：见 [HANDEYE.md](HANDEYE.md)
5. T3 `run_visual_servo_rt.sh` 先观察，再 `--control`
回零：
```bash
bash ~/ros2/scripts/go_home.sh
```
会先杀掉 perception/servo，避免轨迹抢臂。
## Mock（无机器人）
```bash
bash ~/ros2/scripts/run_sim.sh
ros2 run arm_application demo_named_poses
```
docs/SITE_CONFIG.md
+74
View File
@@ -0,0 +1,74 @@
# site.yaml
路径：`~/ros2/config/site.yaml`（从 `config/site.example.yaml` 复制，**不进 git**）。
目录总表：[WORKSPACE.md](WORKSPACE.md)。查看解析：`bash ~/ros2/scripts/show_site_config.sh`
改完后重启对应进程才会进节点（夹爪 CLI 每次重读）。
## 三台相机
| 相机 | 序列号 | 位置 | 脚本 |
|------|--------|------|------|
| 左腕 D405 | `352122274408` | 89 平行夹爪 | `run_camera_d405.sh --arm 89` |
| 右腕 D405 | `260522274442` | 91 旋转夹爪（新，易过曝） | `run_camera_d405.sh --arm 91` |
| 中间 D435 | `262522073161` | 双臂之间手外 | `run_camera_d435.sh --preview` |
列出：`bash ~/ros2/scripts/list_realsense.sh`
右腕过曝：`robots[]` 里 `ae_limit_us: 1500` 且关投射器。仍亮则把该臂 `auto_exposure: false` 并设 `exposure_us: 800`。
## 双臂（89 / 91）
89 和 91 **各有一份** 手眼、腕部相机序列号、夹爪串口、named poses。
`robot.robots[]` 里写该臂覆盖项；`run_*.sh --arm 89|91` 会合并进去。
| 臂 | SN | IP | 手眼 | 夹爪 |
|----|----|----|------|------|
| 左 89 | Rizon4-063989 | 192.168.2.101 | `handeye_result_89.yaml` | PGEA `/dev/ttyUSB0` |
| 右 91 | Rizon4-063991 | 192.168.2.100 | `handeye_result_91.yaml` | RGI `/dev/ttyUSB1` |
```bash
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
bash ~/ros2/scripts/run_handeye.sh --arm 91 teach # 91 必须单独标定
```
每个终端都要带同一 `--arm`，或改 `robot.sn`。不要把 89 的 `T_4x4` 拷到 91。
棋盘尺寸仍是全局 `handeye.board_*`（两臂共用同一块板）。
## 谁读哪些段
| 段 | 读者 |
|----|------|
| `robot` `network` | `run_real.sh`、RDK 工具切换 |
| `camera` | `run_camera_d405.sh`；`overlay_hold_sec` 运行中会跟文件 |
| `paths` `yolo` `servo` | `visual_servo.launch.py` |
| `handeye` `paths.handeye_*` | `run_handeye.sh` |
| `gripper` | `run_gripper.sh` / `dh_pgea` / `dual_gripper` |
| `hole_detect` | `run_hole_detect.sh`（算法参数也在这一段） |
| `tactile` | `run_tactile.sh` |
| `go_home` | `go_home.sh` |
## 常改项
| 键 | 含义 |
|----|------|
| `robot.sn` / `--arm` | 当前臂。可被 `run_real.sh --arm 91` 覆盖 |
| `robot.robots[].camera.serial` | 该臂腕部 D405。91 必须单独填写 |
| `robot.robots[].paths.handeye_file` | 该臂手眼结果，禁止混用 |
| `paths.yolo_weights` | `.onnx` 或 `.pt`；`backend: tensorrt` 时必须有同名 `.engine` |
| `yolo.imgsz` | 必须等于模型输入边长 |
| `yolo.backend` | `tensorrt` / `onnx` / `auto` |
| `camera.serial` `width` `height` `fps` | D405 流。改分辨率要重启相机 |
| `servo.ee_z_m` | 法兰锁高 [m]；**写在 `robots[].servo`，89/91 分开** |
| `servo.xy_track` | `hover`（法兰/爪心正对工件）或 `lookat` |
| `camera.name` | `d405_left` / `d405_right`，话题与预览按臂分开 |
| `servo.v_max_xy` `kp_xy` `a_max` | 跟踪快慢 |
| `gripper.force_pct` `open_position` `close_position` | 左臂 PGEA 力与开度 |
| `robots[].gripper.port` `model` | 该臂串口与型号（91 为 RGI） |
| `hole_detect.backend` `hough_*` `conf` | 入孔检测 |
| `tactile.left_video` `right_video` `config` | Orisys 左右指设备与 dd03 |
路径写成**绝对路径**。换机器改 `USER` 和 SN，不要改 Python。
docs/TACTILE.md
+40
View File
@@ -0,0 +1,40 @@
# Orisys 视触觉
夹爪左右指上的视触觉相机（**不是**腕部 D405）。SDK 用厂家 wheel 安装
（`pip install --no-deps ~/sensor_ws/0.4.2/orisys-0.4.2-cp311-*.whl` 到 ros_flexiv 环境），
本仓库只放适配器、现场参数和 `dd03.json`。
源码：
| 文件 | 作用 |
|------|------|
| `arm_application/orisys_tactile.py` | SDK 适配 + `load_orisys_from_site` + `tactile_worker`（多进程工作器） |
| `arm_application/tactile_interface.py` | 可替换接口 / Mock |
| `arm_application/tactile_node.py` | 发布 `/tactile/left\|right/status` |
| `config/orisys/dd03.json` | 传感器几何/跟踪（厂家配置，可进 git） |
**架构：每侧手指一个独立进程**（厂家示例 `two_sensor_pipeline.py` 的推荐做法，
每进程独立 CUDA 上下文）。实测单进程双开两路会互相拖垮（30→0.8fps），
双进程各 30fps。`tactile` 节点内部用 `multiprocessing(spawn)` 起两个 worker，
主进程只做队列→话题转发。worker 进程 comm 名 `orisys_tac_w`，runner 脚本会
连带清理（**孤儿 worker 会占死 /dev/video\***，表现为下次启动
"Failed to open video source"，手动清：`pkill orisys_tac_w`）。
现场要点（都在 **`config/site.yaml` → `tactile:`**）：
- `left_video/right_video` 用 **`/dev/v4l/by-path/...` 稳定路径**——Orisys 是
"USB Camera"（不是 RealSense！），video 编号随插拔漂移，by-path 绑定物理 USB 口
- `cuda: false` + `infer_backend: cpu`——双路并行抢 GPU 上下文会互相拖慢，
CPU 解算仅 ~9ms/帧足够（单路想快可开 cuda）
- 读数是**形变任意单位**（非牛顿）且夹持基线会漂移；三阶段节点启动时自动测
静止基线，所有力阈值都是相对增量（见 `docs/HOLE_DETECT.md` 三阶段段落）
```bash
bash ~/ros2/scripts/run_tactile.sh --cli # 打印力 若干秒
bash ~/ros2/scripts/run_tactile.sh # ROS 节点
```
无硬件时：`tactile.backend: mock`。
搜孔插入运动（机械臂螺旋 + 力闭环）已由三阶段实现：`stage3_search_node.py`，
见 `docs/HOLE_DETECT.md` 三阶段段落，跑 `bash ~/ros2/scripts/run_stage3_search.sh --arm 89`。
docs/TRACKING.md
+45
View File
@@ -0,0 +1,45 @@
# 视觉伺服
## 启动
```text
T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 或 --arm 91
T2 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
T3 bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 # 只看框
bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control # 动臂
```
三个终端必须带**同一个** `--arm`。89 相机话题是 `/d405_left/...`，91 是 `/d405_right/...`，预览顶栏会写「左臂89 / 右臂91」。不要混用。
`--control` 从当前法兰位姿开节点，不再先抬 `safe_z_m`、也不回 `home`。
`hover` + `ee_vertical_down`：法兰（夹爪中心）XY 压在工件上，法兰轴线朝下。腕部相机有倾角，绿框**不必**对准画面十字。锁高在 `robots[].servo.ee_z_m`，89/91 分开。
## 数据流
```
D405 彩图+深度
→ perception_node（绿框中心 × 立体平面 Z；红点应在绿框里）
→ /visual_servo/filtered_point (相机系)
→ servo_node 100 Hz
hover: 法兰 XY → 工件 XY（基座）
锁高: 法兰 Z → ee_z_m
姿态: 法兰 +Z 朝世界 −Z
→ /rizon_arm_controller/joint_trajectory
```
绿框：相机回调立刻把最新帧丢给 GPU；画出的框会按像素速度补上推理延迟，所以应贴在**当前**工件上，而不是 YOLO 看到的旧位置。丢检时绿框只滑行几帧（不驱动 3D）；预览 `overlay_hold_sec` 必须短，否则旧框会糊在新画面上。丢检空窗内伺服用**基座系 KF 预测**继续跟（眼在手上安全，不随臂动漂移），超过 `stale_hold_sec` 才钉住。
## 调参（`site.yaml` → `servo` / `yolo`）
| 目标 | 键 |
|------|-----|
| 绿框跟手、少丢 | 略降 `conf`/`min_conf`；加大 `jump_px`、`miss_clear_frames`；重启 T3 |
| 绿框稳、少跳 | 略降 `box_ema_alpha`；重启 T3 |
| 臂跟手 | 加大 `v_max_xy` `kp_xy` |
| 臂丝滑 | 加大 `a_max` 会更快起步；减小 `a_max` / `vel_lpf_alpha` 更顺 |
| 轨迹预测 | `predict_enable` `predict_sec`；乱飞则减小 `predict_sec` 或关预测 |
| 画面中心对准 | `xy_track: lookat` |
| 换模型 | `paths.yolo_weights` + `yolo.imgsz` + 导出 `.engine` |
改完重启 T3。
docs/TRAJECTORIES.md
+14
View File
@@ -0,0 +1,14 @@
# 关节轨迹回放
把 Flexiv Elements `.traj` 转成 YAML，经 `/follow_joint_trajectory` 播放（不要直写控制器）。
```bash
./scripts/convert_traj.sh src/arm_application/trajectories/xunhuan.traj
# 生成 src/arm_application/config/xunhuan_trajectory.yaml
# 先 run_real.sh
ros2 run arm_application run_joint_trajectory --ros-args \
--params-file src/arm_application/config/xunhuan_trajectory.yaml
```
YAML 用扁平数组：`waypoint_times`、`waypoint_positions`（每点 7 关节）。
docs/WORKSPACE.md
+189
View File
@@ -0,0 +1,189 @@
# 工作空间地图
本仓库 **`~/ros2`** 是完整的 Flexiv Rizon **眼在手上视觉伺服** 工作空间：实机驱动、腕部 D405、手眼标定、YOLO 跟踪、法兰悬停、DH 夹爪、入孔检测、Orisys 视触觉读数。
**不依赖** `camera_test` / `ros_flexiv` HMI。装配插入运动不在这里。
路径必须是 ASCII（推荐 `~/ros2`）。不要在中文目录里 `colcon build` 消息包。
日常只改 **`config/site.yaml`**（从 `config/site.example.yaml` 复制，**不进 git**）。
日常只跑 **`scripts/*.sh`**。包内 yaml 是默认值，被 site 覆盖。
---
## 顶层
| 路径 | 进 git？ | 做什么 |
|------|----------|--------|
| `README.md` | 是 | 克隆后怎么跑 |
| `docs/` | 是 | 说明文档（本文件是目录总表） |
| `config/site.example.yaml` | 是 | 现场配置模板 |
| `config/site.yaml` | **否** | 本机 SN / IP / 相机 / 锁高 |
| `config/fastdds_visual_servo.xml` | 是 | 可选 FastDDS；默认不用 |
| `scripts/` | 是 | 唯一操作入口 |
| `src/` | 是 | ROS 2 包源码 |
| `models/yolo/` | 目录是；`.pt/.onnx/.engine` **否** | 工件 YOLO 权重，自己放 |
| `models/hole_insert/` | 目录是；权重二进制 **否** | 入孔 YOLO（`backend=yolo` 时） |
| `build/` `install/` `log/` | **否** | `colcon` 产物，本地编译生成 |
| `thirdparty/` `vendor/` | **否** | 可选离线 RDK / Isaac 克隆 |
| `.gitignore` | 是 | 上面「否」的规则 |
---
## `config/` — 现场配置
| 文件 | 作用 |
|------|------|
| `site.example.yaml` | 克隆后 `cp` 成 `site.yaml`，填你的 SN、IP、相机序列号、权重路径 |
| `site.yaml` | 本机唯一入口。`robots[]` 里 89/91 各写相机、手眼、锁高、夹爪 |
| `fastdds_visual_servo.xml` | 仅当 DDS 丢图时再把路径填进 `site.yaml` → `paths.fastdds_profile` |
`site.yaml` 如何进节点：`scripts/lib/export_site_env.py` 按 `--arm` 合并 `robots[]`，导出 `SITE_*` 和环境，launch 再覆盖 `perception.yaml` / `servo.yaml`。
---
## `scripts/` — 操作入口
先：`conda activate ros_flexiv && source ~/ros2/scripts/env.sh`
### 每天用
| 脚本 | 作用 |
|------|------|
| `run_real.sh --arm 89\|91` | Flexiv RDK + ros2_control。示教器 AUTO + Remote Ethernet，先关 Studio |
| `run_camera_d405.sh --arm … --preview` | 该臂腕部 D405。89→`/d405_left/`，91→`/d405_right/` |
| `run_camera_d435.sh --preview` | 顶视 D435（手外，可选） |
| `list_realsense.sh` | 列出已接 RealSense 序列号 |
| `run_visual_servo_rt.sh --arm … [--control]` | YOLO + 伺服。无 `--control` 只看框；有则动臂。从当前姿态开始，不回 home |
| `go_home.sh --arm …` | 回 named pose `home`（会先停伺服） |
| `show_site_config.sh` | 打印当前臂解析出的路径/锁高/相机 |
### 手眼
| 脚本 | 作用 |
|------|------|
| `run_handeye.sh --arm … teach` | RDK 软拖拽记点。必须先停 `run_real.sh`（RDK 独占） |
| `run_handeye.sh --arm … merge` | 把 `/tmp/handeye_calib_XX/taught_poses.txt` 写入该臂 `handeye_poses_XX.yaml` |
| `run_handeye.sh --arm … calibrate` | 走 12 点拍棋盘，解 T，写入 `handeye_result_XX.yaml`。需要 T1 实机 + T2 相机 |
| `run_handeye.sh --arm … show` | 打印该臂板尺寸、poses 路径、点数 |
| `merge_handeye_poses.py` | merge 的实现，一般不直接调 |
### 夹爪 / 网络 / 编译
| 脚本 | 作用 |
|------|------|
| `run_gripper.sh --arm 89\|91 --cli …` | 左 PGEA / 右 RGI。无 `--cli` 起该臂 ROS 节点 |
| `run_dual_gripper.sh` | 双爪同一 ROS 节点（`arm_msgs`） |
| `run_hole_detect.sh --arm … [--preview]` | 入孔检测（只出像素孔心，不插入） |
| `run_tactile.sh [--cli]` | Orisys 左右指视触觉 |
| `env.sh` | 加载 conda + ROS + `install/setup.bash` + `site.yaml` |
| `build_all.sh` | `build.sh`（arm 包）+ `build_real.sh`（Flexiv 硬件包） |
| `build.sh` | colcon：`arm_*` + `flexiv_description` |
| `build_real.sh` | colcon：`flexiv_hardware` 等，需本机已装 RDK（`~/flexiv_install`） |
| `check_robot_network.sh` `fix_robot_lan.sh` | 查/修工控机到机器人网段 |
| `test_rdk_python.sh` | 测 RDK Python 能否连上 |
| `rdk_switch_tool.sh` | 切换 Studio Tools 名（`site.yaml` `robot.tool_name`） |
| `export_yolo_trt.sh` | ONNX → TensorRT `.engine` |
| `run_sim.sh` `run_mock_demo.sh` | 无真机 mock |
| `run_display.sh` | RViz 等显示 |
| `convert_traj.sh` `convert_flexiv_traj.py` | 非夕轨迹 → ROS |
| `fetch_flexiv.sh` `build_flexiv_rdk.sh` `install_rdk_offline.sh` `use_local_flexiv.sh` | 拉/编官方 RDK（新机器） |
| `rdk_drag_bridge.py` `rdk_gripper.py` | 底层 RDK 辅助，由 wrapper 调用 |
### `scripts/lib/`
| 文件 | 作用 |
|------|------|
| `parse_arm_args.sh` | 解析 `--arm 89\|91\|left\|right` |
| `load_site_config.sh` | 调 Python，把 `site.yaml` 变成 bash `SITE_*` |
| `export_site_env.py` | 合并当前臂 overlay；`--print` 给人看 |
---
## `src/` — ROS 2 包
### 本仓库业务（改跟踪/手眼看这里）
| 包 | 作用 |
|----|------|
| `arm_application` | 相机发布、预览、YOLO、伺服、手眼、PGEA/RGI 夹爪、入孔检测、Orisys 触觉 |
| `arm_bringup` | `stack.launch.py`：`backend:=real\|mock`，被 `run_real.sh` 调用 |
| `arm_control` | 命名位姿 `/set_named_pose`、状态、轨迹适配。点位：`config/named_poses_89.yaml` / `_91.yaml` |
| `arm_msgs` | 本栈消息 / 服务 / action |
| `arm_description` | 本栈用的机器人描述（若与 flexiv_description 并存，以 bringup 为准） |
| `arm_moveit_config` | MoveIt 配置（跟踪主路径不用） |
`arm_application` 节点见该包 README。手眼结果：
- `src/arm_application/config/handeye_result_89.yaml` / `_91.yaml` — **相机→法兰** `T_4x4`，标定写入
- `handeye_poses_89.yaml` / `_91.yaml` — 示教关节
- `perception.yaml` `servo.yaml` `handeye_calibrate.yaml` — 默认值，被 site 覆盖
### Flexiv 官方（一般不要改）
| 包 | 作用 |
|----|------|
| `flexiv_hardware` | ros2_control 硬件接口，RDK 发关节 |
| `flexiv_bringup` | 官方 launch |
| `flexiv_msgs` | 官方消息 |
| `flexiv_gripper` | 官方夹爪（本栈 **不用**；腕部爪走 IPC Modbus） |
| `flexiv_description` | URDF / 网格 |
| `flexiv_controllers` `flexiv_moveit_config` `flexiv_test_nodes` | 官方控制器 / MoveIt / 测试 |
| `flexiv_rdk` | RDK 源码树（编译安装到 `~/flexiv_install`） |
| `flexiv_ros2/` | 官方仓库的完整拷贝；`build_real.sh` 选其中的包 |
---
## `models/yolo/` 与 `models/hole_insert/`
工件跟踪权重放 `models/yolo/`，`site.yaml` → `paths.yolo_weights`。
入孔 `backend: yolo` 的孔模型放 `models/hole_insert/`。
二进制不进 git。`backend: tensorrt` 时必须有同名 `.engine`（`export_yolo_trt.sh`）。
---
## `docs/`
| 文件 | 内容 |
|------|------|
| `WORKSPACE.md` | 本页，目录与职责 |
| `QUICKSTART.md` | 环境、编译、第一次实机 |
| `ARCHITECTURE.md` | 进程与数据流 |
| `SITE_CONFIG.md` | `site.yaml` 字段 |
| `HANDEYE.md` | 示教 / merge / 标定 |
| `TRACKING.md` | 视觉伺服、hover、锁高 |
| `GRIPPER.md` | PGEA + RGI |
| `HOLE_DETECT.md` | 入孔检测 |
| `TACTILE.md` | Orisys 视触觉 |
| `INTERFACES.md` | 脚本 / ROS topic / 参数契约 |
| `TRAJECTORIES.md` | 关节轨迹回放 |
---
## 别人克隆后
```bash
cd ~/ros2
cp config/site.example.yaml config/site.yaml # 填硬件
# 权重放到 models/yolo/，改 site.yaml paths.yolo_weights
conda activate ros_flexiv
source scripts/env.sh
./scripts/build_all.sh
source scripts/env.sh
```
示教器 Remote Ethernet，关 Studio。三终端同一 `--arm`：
```bash
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
```
91 把 `89` 换成 `91`。91 必须用自己的手眼文件，不要拷 89 的 `T_4x4`。
## 本仓库不包含
- `camera_test` 训练 GUI / `infer_realtime.py`
- `ros_flexiv` 装配 HMI、孔插入运动
- colcon 的 `build/` `install/` `log/`
- 本机 `site.yaml` 与 YOLO 权重文件
models/hole_insert/README.md
+23
View File
@@ -0,0 +1,23 @@
# 入孔 / 孔定位权重
二进制（`.pt` / `.onnx` / `.engine`）**不进 git**。
`hole_detect.backend: yolo` 时把本目录的检测权重绝对路径写进 `config/site.yaml`：
```yaml
paths:
hole_weights: "/home/USER/ros2/models/hole_insert/best.pt"
hole_detect:
backend: "yolo"
weights: "/home/USER/ros2/models/hole_insert/best.pt"
```
`backend: two_stage` 用的是工件 YOLO（`paths.yolo_weights`），本目录可空。
从旧工程拷贝示例：
```bash
mkdir -p ~/ros2/models/hole_insert
# 若有独立孔模型：
# cp /path/to/best.pt ~/ros2/models/hole_insert/best.pt
```
models/yolo/.gitkeep
View File
models/yolo/README.md
+19
View File
@@ -0,0 +1,19 @@
# YOLO 权重
二进制（`.pt` / `.onnx` / `.engine`）**不进 git**。放到本目录后改 `config/site.yaml`：
```yaml
paths:
yolo_weights: "/home/USER/ros2/models/yolo/YOUR.onnx"
yolo:
backend: "tensorrt"
imgsz: 640 # 必须等于该模型输入
```
TensorRT：
```bash
bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/YOUR.onnx 640
```
会生成同名 `.engine`。换 ONNX 或 imgsz 后重导。然后重启 `run_visual_servo_rt.sh`。
scripts/README.md
+16
View File
@@ -0,0 +1,16 @@
# scripts — 唯一操作入口
实现细节在 [docs/WORKSPACE.md](../docs/WORKSPACE.md) 和 [docs/](../docs/)。
**每天：** `run_real.sh` · `run_camera_d405.sh` · `run_visual_servo_rt.sh` · `go_home.sh`
**手眼：** `run_handeye.sh`（`teach` / `merge` / `calibrate` / `show`）
**夹爪：** `run_gripper.sh --arm 89|91` · `run_dual_gripper.sh`
**入孔：** `run_hole_detect.sh --arm … [--preview]`
**触觉：** `run_tactile.sh [--cli]`
**权重：** `export_yolo_trt.sh`
**配置：** `show_site_config.sh` · `lib/export_site_env.py`
**环境：** `env.sh` · `build_all.sh`
**RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh`
`lib/parse_arm_args.sh` 解析 `--arm 89|91`。
`lib/load_site_config.sh` 把 `config/site.yaml` 变成 `SITE_*`。
scripts/build.sh
Executable
+32
View File
@@ -0,0 +1,32 @@
#!/usr/bin/env bash
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
cd "$ROOT"
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
set +u
conda activate ros_flexiv 2>/dev/null || true
set -u 2>/dev/null || true
fi
if [[ -z "${ROS_DISTRO:-}" ]]; then
if [[ -f /opt/ros/jazzy/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/jazzy/setup.bash
elif [[ -f /opt/ros/humble/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash
elif [[ -f /opt/ros/iron/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/iron/setup.bash
else
echo "未检测到系统 ROS 2；使用 conda ros_flexiv 环境"
fi
fi
PACKAGES=(flexiv_description arm_msgs arm_description arm_control arm_moveit_config arm_bringup arm_application)
colcon build --symlink-install --packages-select "${PACKAGES[@]}" "$@"
echo "构建完成。请执行: source ${ROOT}/install/setup.bash"
scripts/build_all.sh
Executable
+30
View File
@@ -0,0 +1,30 @@
#!/usr/bin/env bash
# Build entire workspace: arm stack + flexiv real drivers
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
cd "$ROOT"
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
set +u
conda activate ros_flexiv 2>/dev/null || true
set -u 2>/dev/null || true
fi
if [[ -z "${ROS_DISTRO:-}" ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
fi
echo "=== [1/2] arm stack ==="
bash "${ROOT}/scripts/build.sh"
echo "=== [2/2] flexiv real stack ==="
bash "${ROOT}/scripts/build_real.sh"
echo ""
echo "Done. Use:"
echo " source ${ROOT}/scripts/env.sh"
echo " bash ${ROOT}/scripts/run_real.sh"
scripts/build_flexiv_rdk.sh
Executable
+34
View File
@@ -0,0 +1,34 @@
#!/usr/bin/env bash
# Build Flexiv RDK v1.x into ~/flexiv_install (required for real robot / flexiv_hardware).
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
RDK="${ROOT}/src/flexiv_rdk"
PREFIX="${FLEXIV_INSTALL_PREFIX:-$HOME/flexiv_install}"
if [[ ! -d "$RDK" ]]; then
echo "Missing $RDK — run scripts/fetch_flexiv.sh first"
exit 1
fi
echo "Building RDK thirdparty deps into $PREFIX ..."
# Prefer system /opt/ros if present; else conda env already has compilers
if [[ -f /opt/ros/humble/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash
fi
cd "$RDK"
if [[ -f thirdparty/build_and_install_dependencies_not_in_ros2.sh ]]; then
bash thirdparty/build_and_install_dependencies_not_in_ros2.sh "$PREFIX"
elif [[ -f thirdparty/build_and_install_dependencies.sh ]]; then
bash thirdparty/build_and_install_dependencies.sh "$PREFIX"
else
echo "No thirdparty build script found; see RDK README"
ls thirdparty || true
fi
rm -rf build && mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX="$PREFIX" -DCMAKE_BUILD_TYPE=Release
cmake --build . --target install -j"$(nproc)"
echo "RDK installed to $PREFIX"
echo "Then: cd $ROOT && colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=$PREFIX"
scripts/build_real.sh
Executable
+36
View File
@@ -0,0 +1,36 @@
#!/usr/bin/env bash
# Build Flexiv hardware packages for real robot (after RDK installed to ~/flexiv_install)
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
cd "$ROOT"
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
set +u
conda activate ros_flexiv 2>/dev/null || true
set -u 2>/dev/null || true
fi
if [[ -z "${ROS_DISTRO:-}" ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
fi
PREFIX="${HOME}/flexiv_install"
CMAKE_PREFIX="${PREFIX}"
if [[ -n "${CONDA_PREFIX:-}" ]]; then
CMAKE_PREFIX="${PREFIX};${CONDA_PREFIX}"
fi
colcon build --symlink-install --packages-select \
flexiv_hardware \
flexiv_msgs \
flexiv_gripper \
flexiv_test_nodes \
flexiv_robot_states_broadcaster \
flexiv_bringup \
--cmake-args "-DCMAKE_PREFIX_PATH=${CMAKE_PREFIX}" "$@"
echo "Real stack build done. source ${ROOT}/scripts/env.sh"
scripts/check_robot_network.sh
Executable
+90
View File
@@ -0,0 +1,90 @@
#!/usr/bin/env bash
# Check PC ↔ Flexiv robot network (addresses from site.yaml)
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
ROBOT_SN="${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}"
PC_IP="${PC_IP:-${SITE_PC_IP:-192.168.100.10}}"
pick_iface() {
if [[ -n "${IFACE:-}" ]]; then
echo "$IFACE"
return
fi
if [[ -n "${SITE_NET_IFACE:-}" && -d "/sys/class/net/${SITE_NET_IFACE}" ]]; then
echo "${SITE_NET_IFACE}"
return
fi
local d
for d in enp4s0 enp5s0 eth0; do
[[ -d /sys/class/net/$d ]] && echo "$d" && return
done
for d in $(ls /sys/class/net); do
[[ "$d" == lo ]] && continue
[[ "$d" == en* || "$d" == eth* ]] || continue
local state
state=$(cat /sys/class/net/"$d"/operstate 2>/dev/null || echo down)
[[ "$state" == up ]] && echo "$d" && return
done
echo enp4s0
}
IFACE="$(pick_iface)"
echo "=== 本机网卡 ==="
ip -4 -br addr
echo
echo "=== 目标机器人（site.yaml） ==="
echo " SN $ROBOT_SN"
echo " 标签 ${SITE_ROBOT_LABEL:-}"
echo " 目录 ${SITE_ROBOT_CATALOG:-}"
echo " IP $ROBOT_IP"
echo " 本机 $PC_IP"
echo " 网卡 $IFACE"
if ping -c 1 -W 1 "$ROBOT_IP" >/dev/null 2>&1; then
echo " ping UP — 链路 OK"
else
echo " ping DOWN — PC 与机器人必须同网段（本机 ${PC_IP} ↔ 机器人 ${ROBOT_IP}）"
echo
echo " 一键修复（交换机 / 有线）："
echo " bash ~/ros2/scripts/fix_robot_lan.sh"
echo " 独立交换机（不要办公网 DHCP）可用："
echo " bash ~/ros2/scripts/fix_robot_lan.sh --only"
fi
echo
echo "=== 常见地址探测 ==="
CANDIDATES=(
"$ROBOT_IP"
192.168.2.100 192.168.2.10 192.168.101.1 192.168.101.10
192.168.100.1 192.168.100.10
192.168.100.1 192.168.100.2 192.168.100.10
192.168.1.1
)
seen=""
for ip in "${CANDIDATES[@]}"; do
[[ " ${seen} " == *" ${ip} "* ]] && continue
seen+=" ${ip}"
if ping -c 1 -W 1 "$ip" >/dev/null 2>&1; then
echo " UP $ip"
else
echo " down $ip"
fi
done
echo
echo "=== 当前邻居 (ARP) ==="
ip neigh | grep -v FAILED || true
echo
echo "说明："
echo "1) 网线接机器人控制柜网口 ↔ 本机 $IFACE。"
echo "2) 示教器开启 RDK / 远程控制后再连。"
echo "3) RDK 连接用 SN（不是 IP）：flexivrdk.Robot('$ROBOT_SN')"
echo "4) 切换机械臂：改 ~/ros2/config/site.yaml 的 robot.sn"
echo "5) 手册: https://flexiv.cn/software/rdk/manual/index.html"
scripts/convert_flexiv_traj.py
Executable
+117
View File
@@ -0,0 +1,117 @@
#!/usr/bin/env python3
"""Convert Flexiv Elements .traj (text) -> arm_application YAML waypoints."""
from __future__ import annotations
import argparse
import math
import re
import sys
from pathlib import Path
from typing import Any
def parse_flexiv_traj(text: str) -> list[dict[str, Any]]:
"""Parse Elements Studio scatter-point .traj file."""
blocks = re.split(r'pattern_params\s*\{', text)[1:]
waypoints: list[dict[str, Any]] = []
for block in blocks:
name_m = re.search(r'pattern_name:\s*"([^"]+)"', block)
vel_m = re.search(r'velocity:\s*"([^"]+)"', block)
joints: dict[str, float] = {}
for j in range(1, 8):
m = re.search(rf'joint{j}:\s*"([^"]+)"', block)
if m:
joints[f'joint{j}'] = math.radians(float(m.group(1)))
if len(joints) != 7:
continue
positions = [joints[f'joint{i}'] for i in range(1, 8)]
waypoints.append({
'name': name_m.group(1) if name_m else f'WPT{len(waypoints)}',
'positions': positions,
'velocity': float(vel_m.group(1)) if vel_m else 0.2,
})
return waypoints
def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None:
"""Cumulative time_from_start from joint-space segment length."""
t = 0.0
prev = None
for wp in waypoints:
pos = wp['positions']
if prev is None:
t += min_seg
else:
delta = max(abs(a - b) for a, b in zip(pos, prev))
# ~4 s per radian max joint move, floored at min_seg
t += max(min_seg, delta * 4.0)
wp['time'] = round(t, 2)
prev = pos
def to_yaml(
waypoints: list[dict[str, Any]],
*,
joint_prefix: str = 'Rizon4-063989_',
velocity_scale: float | None = None,
) -> str:
if velocity_scale is None:
velocity_scale = waypoints[0].get('velocity', 0.2) if waypoints else 0.2
times = [wp['time'] for wp in waypoints]
flat: list[float] = []
for wp in waypoints:
flat.extend(wp['positions'])
pos_line = ', '.join(f'{v:.6f}' for v in flat)
time_line = ', '.join(str(t) for t in times)
names = ', '.join(f' # {wp.get("name", "")}' if i == 0 else '' for i, wp in enumerate(waypoints))
_ = names # names kept in comments below
lines = [
'# Auto-converted from Flexiv Elements .traj (joints: deg -> rad)',
'# ROS params: flat waypoint_positions + waypoint_times (no nested dicts)',
'# Edit velocity_scale / loop_count as needed (no rebuild).',
'run_joint_trajectory:',
' ros__parameters:',
f' joint_prefix: "{joint_prefix}"',
' joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]',
f' velocity_scale: {velocity_scale}',
' loop_count: 1 # 1=once, 0=forever, N=repeat N',
' pause_between_loops: 0.5',
f' waypoint_times: [{time_line}]',
' waypoint_positions: [',
]
for i, wp in enumerate(waypoints):
pos = ', '.join(f'{v:.6f}' for v in wp['positions'])
lines.append(f' {pos}, # {wp.get("name", f"WPT{i}")}')
lines.append(' ]')
return '\n'.join(lines) + '\n'
def main() -> int:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument('input', type=Path, help='Flexiv Elements .traj file')
ap.add_argument('-o', '--output', type=Path, help='Output YAML path')
ap.add_argument('--prefix', default='Rizon4-063989_', help='Joint name prefix (site SN + _)')
ap.add_argument('--min-seg', type=float, default=3.0, help='Min seconds per segment')
args = ap.parse_args()
text = args.input.read_text(encoding='utf-8')
wps = parse_flexiv_traj(text)
if not wps:
print('ERROR: no waypoints parsed', file=sys.stderr)
return 1
assign_times(wps, min_seg=args.min_seg)
yaml_text = to_yaml(wps, joint_prefix=args.prefix)
out = args.output
if out:
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(yaml_text, encoding='utf-8')
print(f'Wrote {len(wps)} waypoints -> {out}')
else:
print(yaml_text)
return 0
if __name__ == '__main__':
raise SystemExit(main())
scripts/convert_traj.sh
Executable
+69
View File
@@ -0,0 +1,69 @@
#!/usr/bin/env bash
# Convert Flexiv Elements .traj → arm_application YAML (standard entry for all trajectories).
# Usage:
# ./scripts/convert_traj.sh ~/文档/my_task.traj
# ./scripts/convert_traj.sh ~/文档/my_task.traj -o /path/to/out.yaml
# ./scripts/convert_traj.sh ~/文档/my_task.traj --min-seg 2.5 --no-copy
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PY="${ROOT}/scripts/convert_flexiv_traj.py"
if [[ $# -lt 1 ]]; then
echo "Usage: $0 INPUT.traj [-o OUT.yaml] [--min-seg SECONDS] [--no-copy]"
echo "Example: $0 ~/文档/my_task.traj"
exit 1
fi
INPUT="$(realpath "$1")"
shift
BASENAME="$(basename "${INPUT}" .traj)"
OUT_DEFAULT="${ROOT}/src/arm_application/config/${BASENAME}_trajectory.yaml"
TRAJ_COPY="${ROOT}/src/arm_application/trajectories/${BASENAME}.traj"
OUT="${OUT_DEFAULT}"
MIN_SEG="3.0"
DO_COPY=1
EXTRA=()
while [[ $# -gt 0 ]]; do
case "$1" in
-o|--output)
OUT="$(realpath -m "$2")"
shift 2
;;
--min-seg)
MIN_SEG="$2"
shift 2
;;
--no-copy)
DO_COPY=0
shift
;;
*)
EXTRA+=("$1")
shift
;;
esac
done
if [[ ! -f "${INPUT}" ]]; then
echo "ERROR: input not found: ${INPUT}" >&2
exit 1
fi
python3 "${PY}" "${INPUT}" -o "${OUT}" --min-seg "${MIN_SEG}" "${EXTRA[@]}"
if [[ "${DO_COPY}" -eq 1 ]]; then
mkdir -p "$(dirname "${TRAJ_COPY}")"
cp -f "${INPUT}" "${TRAJ_COPY}"
echo "Copied source traj → ${TRAJ_COPY}"
fi
echo ""
echo "Next (stack must be running):"
echo " conda activate ros_flexiv"
echo " source ${ROOT}/install/setup.bash"
echo " export LD_LIBRARY_PATH=~/flexiv_install/lib:\$LD_LIBRARY_PATH"
echo " ros2 run arm_application run_joint_trajectory --ros-args \\"
echo " --params-file ${OUT}"
scripts/env.sh
Executable
+36
View File
@@ -0,0 +1,36 @@
#!/usr/bin/env bash
# Source this workspace environment (conda + ROS + flexiv libs).
# Usage: source scripts/env.sh
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
if [[ -f /opt/ros/humble/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash
elif [[ -f /opt/ros/jazzy/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/jazzy/setup.bash
fi
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
conda activate ros_flexiv 2>/dev/null || true
fi
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
# Pip TensorRT (tensorrt-cu12) — needed to load YOLO .engine
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/site-packages"
for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do
if [[ -d "${_d}" ]]; then
export LD_LIBRARY_PATH="${_d}:${LD_LIBRARY_PATH}"
fi
done
unset _d _TRT_SITE
export YOLO_AUTOINSTALL=false
export ROS2_WS="${ROOT}"
# Master site config → SITE_* (YOLO weights, camera, robot SN, …)
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
scripts/export_yolo_trt.sh
Executable
+90
View File
@@ -0,0 +1,90 @@
#!/usr/bin/env bash
# Export YOLO .pt/.onnx → TensorRT .engine (FP16).
#
# bash ~/ros2/scripts/export_yolo_trt.sh
# bash ~/ros2/scripts/export_yolo_trt.sh /home/cc/ros2/models/yolo/detect_best.onnx 640
#
# Needs: tensorrt-cu12 + onnx + onnxslim in ros_flexiv (do NOT pip install
# the meta package `tensorrt`, it pulls CUDA 13 wheels).
# Then set yolo.backend: tensorrt in site.yaml and restart visual servo.
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
PT="${1:-${SITE_YOLO_WEIGHTS:-${ROOT}/models/yolo/detect_best.onnx}}"
IMGSZ="${2:-${SITE_YOLO_IMGSZ:-640}}"
PY="${HOME}/anaconda3/envs/ros_flexiv/bin/python"
if [[ ! -f "${PT}" ]]; then
echo "ERROR: weights not found: ${PT}" >&2
exit 1
fi
# Pip TensorRT shared libs (needed at export + runtime).
TRT_LIBS="$("${PY}" - <<'PY'
import pathlib, tensorrt
p = pathlib.Path(tensorrt.__file__).resolve().parent
cands = [p, p.parent / "tensorrt_libs", p.parent / "nvidia" / "tensorrt" / "lib"]
print(":".join(str(x) for x in cands if x.exists()))
PY
)"
export LD_LIBRARY_PATH="${TRT_LIBS}${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}"
export YOLO_AUTOINSTALL=false
echo "Export TensorRT engine from ${PT} imgsz=${IMGSZ}"
"${PY}" - <<PY
from pathlib import Path
import tensorrt as trt
src = Path("${PT}").expanduser()
print("tensorrt", trt.__version__)
print("source", src)
engine_path = src.with_suffix(".engine")
if src.suffix.lower() == ".onnx":
logger = trt.Logger(trt.Logger.INFO)
flag = 1 << int(trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH)
builder = trt.Builder(logger)
network = builder.create_network(flag)
parser = trt.OnnxParser(network, logger)
raw = src.read_bytes()
if not parser.parse(raw):
for i in range(parser.num_errors):
print("ONNX parse error:", parser.get_error(i))
raise SystemExit("ONNX parse failed")
config = builder.create_builder_config()
config.set_memory_pool_limit(trt.MemoryPoolType.WORKSPACE, 4 << 30)
if builder.platform_has_fast_fp16:
config.set_flag(trt.BuilderFlag.FP16)
print("FP16 enabled")
serialized = builder.build_serialized_network(network, config)
if serialized is None:
raise SystemExit("TensorRT build failed")
engine_path.write_bytes(bytes(serialized))
print("wrote", engine_path)
else:
from ultralytics import YOLO
m = YOLO(str(src))
try:
out = m.export(
format="engine",
imgsz=int("${IMGSZ}"),
device=0,
quantize=16,
simplify=True,
workspace=4,
)
except TypeError:
out = m.export(
format="engine",
imgsz=int("${IMGSZ}"),
device=0,
half=True,
simplify=True,
workspace=4,
)
print("wrote", out)
PY
echo "Done. Restart: bash ~/ros2/scripts/run_visual_servo_rt.sh --control"
scripts/fetch_flexiv.sh
Executable
+116
View File
@@ -0,0 +1,116 @@
#!/usr/bin/env bash
# Clone official Flexiv ROS 2 stack for Rizon (RDK v1.x → humble-v1 branch).
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
SRC="${ROOT}/src"
BRANCH="${FLEXIV_ROS2_BRANCH:-humble-v1}"
mkdir -p "${SRC}"
cd "${SRC}"
# Always ensure official URDF package is present (meshes + xacro)
if [[ ! -d flexiv_description ]]; then
echo "Fetching flexiv_description (${BRANCH})..."
if ! git clone --depth 1 -b "${BRANCH}" https://github.com/flexivrobotics/flexiv_description.git; then
echo "git clone slow/failed — downloading zip..."
python3 - <<PY
import urllib.request, socket, zipfile, io, shutil, os
url = "https://codeload.github.com/flexivrobotics/flexiv_description/zip/refs/heads/${BRANCH}"
old = socket.getaddrinfo
socket.getaddrinfo = lambda h, p, f=0, t=0, pr=0, fl=0: [ai for ai in old(h, p, socket.AF_INET, t, pr, fl)]
data = urllib.request.urlopen(urllib.request.Request(url, headers={"User-Agent": "Mozilla/5.0"}), timeout=300).read()
dest = "${SRC}/flexiv_description"
with zipfile.ZipFile(io.BytesIO(data)) as z:
z.extractall("/tmp")
shutil.rmtree(dest, ignore_errors=True)
shutil.move("/tmp/flexiv_description-${BRANCH}", dest)
print("flexiv_description ready at", dest)
PY
fi
else
echo "flexiv_description already present"
fi
if [[ ! -d flexiv_ros2 ]]; then
echo "Cloning flexiv_ros2 (${BRANCH})..."
git clone https://github.com/flexivrobotics/flexiv_ros2.git -b "${BRANCH}"
else
echo "flexiv_ros2 already present — pull if needed: git -C flexiv_ros2 pull"
fi
REPOS_FILE=""
for candidate in flexiv_ros2/flexiv.humble.repos flexiv_ros2/flexiv.repos; do
if [[ -f "${candidate}" ]]; then
REPOS_FILE="${candidate}"
break
fi
done
if [[ -z "${REPOS_FILE}" ]]; then
echo "ERROR: no .repos file found under flexiv_ros2"
exit 1
fi
echo "Importing dependencies from ${REPOS_FILE}..."
if ! command -v vcs >/dev/null 2>&1; then
echo "Please install: sudo apt install python3-vcstool"
exit 1
fi
# Prefer local Flexiv_Rizon4 RDK (avoids GitHub clone when present)
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"
if [[ -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
echo "Using local RDK: ${LOCAL_FLEXIV}/flexiv_rdk"
bash "${ROOT}/scripts/use_local_flexiv.sh"
elif [[ -d flexiv_rdk ]]; then
touch flexiv_rdk/COLCON_IGNORE
else
echo "Importing flexiv_rdk from ${REPOS_FILE}..."
vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing
if [[ -d flexiv_rdk ]]; then
touch flexiv_rdk/COLCON_IGNORE
fi
fi
# Always import remaining deps from repos (description already handled; skip-existing)
vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing || true
cat <<EOF
Done. Next steps (Ubuntu 22.04 / Humble + Rizon):
Local RDK wired via: scripts/use_local_flexiv.sh
src/flexiv_rdk -> ${LOCAL_FLEXIV}/flexiv_rdk (your copy, often RDK 2.x)
src/flexiv_rdk_v1 -> RDK 1.9.1 for flexiv_ros2 humble-v1 (if present)
1) Install ROS deps:
sudo apt install -y python3-colcon-common-extensions python3-vcstool \\
ros-humble-ros2-control ros-humble-ros2-controllers \\
ros-humble-moveit ros-humble-xacro ros-humble-joint-state-publisher-gui
2) Build & install RDK **v1.9.1** into ~/flexiv_install (for flexiv_hardware):
RDK_SRC=${SRC}/flexiv_rdk_v1
# fallback if only one tree: RDK_SRC=${SRC}/flexiv_rdk
cd \${RDK_SRC}/thirdparty
source /opt/ros/humble/setup.bash # or: conda activate ros_flexiv
bash build_and_install_dependencies_not_in_ros2.sh ~/flexiv_install
cd \${RDK_SRC} && rm -rf build && mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install -DCMAKE_PREFIX_PATH=~/flexiv_install
cmake --build . --target install --config Release
3) Build this workspace:
cd ${ROOT}
source /opt/ros/humble/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/flexiv_install
source install/setup.bash
4) Real robot (site SN, no gripper yet):
ros2 launch arm_bringup real.launch.py
# or explicitly:
ros2 launch arm_bringup real.launch.py robot_sn:=Rizon4-063989 load_gripper:=false
Official fake hardware:
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=Rizon4-063989 use_fake_hardware:=true load_gripper:=false
EOF
scripts/fetch_isaac_flexiv.sh
Executable
+46
View File
@@ -0,0 +1,46 @@
#!/usr/bin/env bash
# Clone Flexiv Isaac Sim integration repos into thirdparty/
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
TP="${ROOT}/thirdparty"
mkdir -p "${TP}"
cd "${TP}"
clone_or_update() {
local url="$1"
local dir="$2"
if [[ -d "${dir}/.git" ]]; then
echo "Updating ${dir}..."
git -C "${dir}" pull --ff-only || true
else
echo "Cloning ${dir}..."
git clone --depth 1 "${url}" "${dir}"
fi
}
clone_or_update https://github.com/flexivrobotics/flexiv_sim_plugin.git flexiv_sim_plugin
clone_or_update https://github.com/flexivrobotics/isaac_sim_ws.git isaac_sim_ws
clone_or_update https://github.com/flexivrobotics/flexiv_description.git flexiv_description
# Optional: RDK sources next to sim (if not already under src/)
if [[ ! -d "${ROOT}/src/flexiv_rdk" ]]; then
clone_or_update https://github.com/flexivrobotics/flexiv_rdk.git flexiv_rdk
fi
cat <<EOF
Isaac / description repos are under: ${TP}
Next (Isaac path):
1) Install NVIDIA Isaac Sim
2) Follow flexiv_sim_plugin README (Elements Studio → External / Isaac)
3) cd ${TP}/isaac_sim_ws && bash install_ws.sh <isaac_sim_root>
4) Configure robot instance for Rizon4-063989 in app_config.yaml
5) Primitive docs: https://primitive.flexiv.cn/primitives/zh-CN/3.10/rizon4/index.html
6) RDK manual (CN): https://flexiv.cn/software/rdk/manual/index.html
ROS 2 path (separate):
${ROOT}/scripts/fetch_flexiv.sh
EOF
scripts/fix_robot_lan.sh
Executable
+146
View File
@@ -0,0 +1,146 @@
#!/usr/bin/env bash
# 工控机 User1（enp4s0）↔ 交换机 ↔ 两台 Flexiv
#
# 本机 192.168.2.10/24（User1）。两台臂：063989=192.168.2.101 063991=192.168.2.100
#
# bash ~/ros2/scripts/fix_robot_lan.sh
# bash ~/ros2/scripts/fix_robot_lan.sh --only # 不要办公网 DHCP（独立交换机）
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
PC_IP_PRIMARY="${SITE_PC_IP:-192.168.2.10}"
PC_ADDRS=("192.168.2.10/24" "192.168.100.10/24")
ROBOT_IPS=("${SITE_ROBOT_IP:-192.168.2.100}")
# Catalog extras (second arm)
if [[ -n "${SITE_ROBOT_CATALOG:-}" ]]; then
:
fi
MODE=secondary
while [[ $# -gt 0 ]]; do
case "$1" in
--only) MODE=only; shift ;;
--secondary) MODE=secondary; shift ;;
-h|--help)
sed -n '2,12p' "$0"
exit 0
;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
pick_iface() {
if [[ -n "${IFACE:-}" ]]; then
echo "$IFACE"
return
fi
if [[ -n "${SITE_NET_IFACE:-}" && -d "/sys/class/net/${SITE_NET_IFACE}" ]]; then
echo "${SITE_NET_IFACE}"
return
fi
for d in enp4s0 enp5s0 eth0; do
[[ -d /sys/class/net/$d ]] || continue
echo "$d"
return
done
return 1
}
IFACE="$(pick_iface)" || {
echo "ERROR: 找不到 User1 有线网卡" >&2
ip -br link
exit 1
}
ADDR_CSV="$(IFS=,; echo "${PC_ADDRS[*]}")"
echo "=== User1 / ${IFACE} 模式=${MODE} ==="
echo "本机副 IP: ${ADDR_CSV}"
echo "当前臂: SN=${SITE_ROBOT_SN:-?} IP=${SITE_ROBOT_IP:-?} catalog=${SITE_ROBOT_CATALOG:-}"
ip -4 -br addr
echo
if ! command -v nmcli >/dev/null; then
echo "ERROR: 需要 nmcli" >&2
exit 1
fi
CON="$(nmcli -t -f NAME,DEVICE connection show --active | awk -F: -v d="$IFACE" '$2==d{print $1; exit}')"
if [[ -z "$CON" ]]; then
CON="$(nmcli -t -f NAME,DEVICE connection show | awk -F: -v d="$IFACE" '$2==d{print $1; exit}')"
fi
if [[ -z "$CON" ]]; then
echo "ERROR: NetworkManager 没有绑定 $IFACE" >&2
nmcli device status
exit 1
fi
echo "NM 连接: $CON"
nmcli_mod() {
if nmcli connection modify "$CON" "$@"; then
return 0
fi
sudo nmcli connection modify "$CON" "$@"
}
nmcli_up() {
if nmcli connection up "$CON"; then
return 0
fi
sudo nmcli connection up "$CON"
}
if [[ "$MODE" == only ]]; then
nmcli_mod \
ipv4.method manual \
ipv4.addresses "${PC_IP_PRIMARY}/24" \
ipv4.gateway "" \
ipv4.dns "" \
ipv4.ignore-auto-dns yes \
ipv4.never-default yes
else
nmcli_mod \
ipv4.method auto \
ipv4.addresses "${ADDR_CSV}" \
ipv4.never-default no
fi
nmcli_up
echo
echo "=== 修复后 ==="
ip -4 -br addr show "$IFACE"
echo
ping_one() {
local ip="$1"
if ping -c 2 -W 1 "$ip" >/dev/null 2>&1; then
echo " UP $ip"
return 0
fi
echo " down $ip"
return 1
}
echo "=== ping 机器人网段 ==="
ok=0
for ip in 192.168.2.100 192.168.101.1 192.168.2.101 "${SITE_ROBOT_IP:-}"; do
[[ -z "$ip" ]] && continue
ping_one "$ip" && ok=1 || true
done
# unique
echo
if ping -c 1 -W 1 "${SITE_ROBOT_IP:-192.168.2.100}" >/dev/null 2>&1; then
echo "OK — 当前臂 ${SITE_ROBOT_SN:-} @ ${SITE_ROBOT_IP} 可达。RDK 用 SN 连接。"
echo " 063989 @ 192.168.2.101；063991 @ 192.168.2.100。"
else
echo "当前臂 ${SITE_ROBOT_IP:-192.168.2.100} 仍不通。检查："
echo " 1) User1 网线 ↔ 交换机 ↔ 控制柜网口（灯亮）"
echo " 2) 示教器 About 里的 IPv4（出厂多为 192.168.2.100）"
echo " 3) Remote / Ethernet RDK 已开"
exit 2
fi
scripts/gen_site_example.py
+139
View File
@@ -0,0 +1,139 @@
#!/usr/bin/env python3
"""从 ``config/site.yaml`` 生成 ``config/site.example.yaml``。
为什么要有这个脚本
------------------
``config/site.yaml`` 是现场唯一权威配置，也是全项目注释最全的文件。
``config/site.example.yaml`` 供新环境部署时复制参考。
过去两者是各写各的，结果是模板长期落后于现场配置：分析时发现模板缺了
114 项（``servo.*``、``hole_detect.stage2.*``、``sim.*`` 等），新人照着模板
配完会发现一堆功能不工作。
本脚本把「模板」变成「site.yaml 的匿名化副本」，于是模板永远不会漂移：
改完 site.yaml 重跑本脚本即可。
做什么
------
读 ``config/site.yaml``，把现场专属的值替换成占位值，其余**逐字节保留**
（包括全部中文注释与排版），然后写入 ``config/site.example.yaml``。
用法::
python3 scripts/gen_site_example.py # 生成 / 更新模板
python3 scripts/gen_site_example.py --check # 只检查是否已同步（CI / 测试用）
``--check`` 在不一致时以退出码 1 结束，方便接进测试。
"""
from __future__ import annotations
import argparse
import re
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
SITE = ROOT / "config" / "site.yaml"
EXAMPLE = ROOT / "config" / "site.example.yaml"
#: 生成文件顶部的说明。生成物是只读产物，手改会在下次生成时丢失。
HEADER = """\
# =============================================================================
# site.example.yaml — 新环境部署模板
#
# ！本文件由 scripts/gen_site_example.py 从 config/site.yaml 自动生成，请勿手改。
# 要改模板内容，请改 config/site.yaml（它是唯一权威）后重跑：
# python3 scripts/gen_site_example.py
# 或在改完 site.yaml 后跑一次以确认模板同步：
# python3 scripts/gen_site_example.py --check
#
# 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
# 现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
#
# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
# 部署时请改成实际用户的 home 目录。
# =============================================================================
"""
#: 逐字替换。长串必须排在前面，否则会被短串规则截断。
LITERAL_REPLACEMENTS: list[tuple[str, str]] = [
# 头部那句"从 example 复制"与本文件的实际流程相反，改掉。
(
"# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制）",
"# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制而来）",
),
# 用户名匿名化。放在其它规则之前，避免路径被二次处理。
("/home/cc", "/home/USER"),
# 注释里裸写的机械臂编号（无引号，正则抓不到）。
(
"# User1=enp4s0 本机 192.168.2.10 ↔ 063989=.101 / 063991=.100",
"# User1=enp4s0 本机 <PC_IP> ↔ <左臂SN>=.101 / <右臂SN>=.100",
),
]
#: 正则替换。按顺序应用。
REGEX_REPLACEMENTS: list[tuple[str, str]] = [
# 机械臂序列号：Rizon4-063989 -> Rizon4-XXXXXX
(r'"(Rizon4)-\d+"', r'"\1-XXXXXX"'),
# IP 地址（仅带引号的配置值；注释里的裸 IP 由上面的字面替换处理）
(r'"192\.168\.\d{1,3}\.\d{1,3}"', '"192.168.2.XXX"'),
# 8 位及以上的纯数字字符串 = 设备序列号（相机 12 位 / 夹爪 8 位）。
# 不会误伤别的值：本文件里没有其它 8 位以上的数字字符串字面量。
(r'"\d{8,}"', '"<序列号>"'),
# USB 视频设备的 by-path 稳定路径：端口号随机器而变。
(r"(usb-0:)\d+\.\d+\.\d+", r"\1X.Y.Z"),
]
def render(site_text: str) -> str:
"""Turn the live config text into the anonymised template text."""
out = site_text
for old, new in LITERAL_REPLACEMENTS:
out = out.replace(old, new)
for pattern, replacement in REGEX_REPLACEMENTS:
out = re.sub(pattern, replacement, out)
return HEADER + out
def read_site() -> str:
if not SITE.is_file():
print(f"错误: 找不到 {SITE}", file=sys.stderr)
raise SystemExit(2)
return SITE.read_text(encoding="utf-8")
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(
description="从 config/site.yaml 生成 config/site.example.yaml",
)
parser.add_argument(
"--check",
action="store_true",
help="只检查模板是否与 site.yaml 同步；不同步则以退出码 1 结束（不写文件）",
)
args = parser.parse_args(argv)
expected = render(read_site())
if args.check:
actual = EXAMPLE.read_text(encoding="utf-8") if EXAMPLE.is_file() else ""
if actual == expected:
print(f"模板已同步: {EXAMPLE}")
return 0
print(
"模板与 config/site.yaml 不同步。\n"
" config/site.yaml 改动后，请重跑: python3 scripts/gen_site_example.py",
file=sys.stderr,
)
return 1
EXAMPLE.parent.mkdir(parents=True, exist_ok=True)
EXAMPLE.write_text(expected, encoding="utf-8")
print(f"已生成 {EXAMPLE}")
return 0
if __name__ == "__main__":
raise SystemExit(main())
scripts/go_home.sh
Executable
+283
View File
@@ -0,0 +1,283 @@
#!/usr/bin/env bash
# Move Rizon4 to a named pose (default: home) and FORCE stop.
#
# Critical: visual_servo.launch / servo_node must be killed first, otherwise
# they keep streaming JointTrajectory and the arm never settles.
#
# Usage:
# bash ~/ros2/scripts/go_home.sh
# bash ~/ros2/scripts/go_home.sh --pose ready --speed 0.3
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
POSE="${POSE:-}"
SPEED="${SPEED:-}"
KEEP_SERVO=0
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
usage() {
cat <<'EOF'
Move arm to a named pose (default: home), then hold with zero velocity.
Default: kill perception/servo/launch first (required to stop shaking).
Options:
--pose NAME home | ready | inspect | pos_left
--speed FLOAT velocity_scale (0,1] default 0.25
--arm 89|91 与 run_real 同一臂
--keep-servo do NOT kill servo (arm may keep shaking)
-h, --help
EOF
}
while [[ $# -gt 0 ]]; do
case "$1" in
--pose) POSE="${2:?}"; shift 2 ;;
--speed) SPEED="${2:?}"; shift 2 ;;
--keep-servo) KEEP_SERVO=1; shift ;;
--stop-track) shift ;; # legacy
-h|--help) usage; exit 0 ;;
*) echo "Unknown: $1" >&2; usage >&2; exit 1 ;;
esac
done
if ! command -v ros2 >/dev/null 2>&1 || [[ -z "${ROS_DISTRO:-}" ]]; then
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
fi
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
POSE="${POSE:-${SITE_GO_HOME_POSE:-home}}"
SPEED="${SPEED:-${SITE_GO_HOME_SPEED:-0.25}}"
kill_streamers() {
echo "=== killing visual-servo streamers ==="
# Prefer disable control before kill (if node still up)
ros2 param set /servo_node enable_control false >/dev/null 2>&1 || true
# Kill launch parent first, then executables (SIGTERM then SIGKILL)
local patterns=(
'visual_servo.launch.py'
'/lib/arm_application/servo_node'
'/lib/arm_application/perception_node'
)
local p
for p in "${patterns[@]}"; do
if pgrep -f "$p" >/dev/null 2>&1; then
echo " TERM $p"
pkill -TERM -f "$p" 2>/dev/null || true
fi
done
sleep 1.0
for p in "${patterns[@]}"; do
if pgrep -f "$p" >/dev/null 2>&1; then
echo " KILL $p"
pkill -KILL -f "$p" 2>/dev/null || true
fi
done
sleep 0.5
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
echo "ERROR: servo_node still alive — refuse to move (would keep shaking)" >&2
pgrep -af '/lib/arm_application/servo_node' >&2 || true
exit 2
fi
echo " servo_node gone OK"
}
print_joints() {
python3 <<'PY'
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
rclpy.init()
n = Node("go_home_joints")
box = [None]
n.create_subscription(JointState, "joint_states", lambda m: box.__setitem__(0, m), 10)
for _ in range(40):
rclpy.spin_once(n, timeout_sec=0.05)
if box[0] is not None:
break
if box[0] is None:
print(" (no joint_states)")
else:
m = dict(zip(box[0].name, box[0].position))
v = dict(zip(box[0].name, box[0].velocity)) if box[0].velocity else {}
for i in range(1, 8):
k = f"{__import__('os').environ.get('SITE_JOINT_PREFIX','Rizon4-063989_')}joint{i}"
if k in m:
print(f" joint{i}: {m[k]:+.4f} qd={v.get(k, 0.0):+.4f}")
n.destroy_node(); rclpy.shutdown()
PY
}
# Strong settle via controller ACTION (not topic — matches JTC reliably)
settle_hold_action() {
local pose="$1"
python3 - "$pose" <<'PY'
import sys, time
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
POSES = {
"home": [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
"ready": [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
"inspect": [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
"pos_left": [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
}
name = sys.argv[1]
target = POSES.get(name)
if target is None:
raise SystemExit(f"unknown pose {name}")
import os
PREFIX = os.environ.get("SITE_JOINT_PREFIX", "Rizon4-063989_")
JOINTS = [f"{PREFIX}joint{i}" for i in range(1, 8)]
rclpy.init()
n = Node("go_home_hold_action")
js_box = [None]
n.create_subscription(JointState, "joint_states", lambda m: js_box.__setitem__(0, m), 10)
cli = ActionClient(n, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory")
if not cli.wait_for_server(timeout_sec=5.0):
print(" WARN: JTC action not available — skip action hold")
n.destroy_node(); rclpy.shutdown(); raise SystemExit(0)
def spin_until(pred, timeout=3.0):
t0 = time.monotonic()
while time.monotonic() - t0 < timeout:
rclpy.spin_once(n, timeout_sec=0.05)
if pred():
return True
return False
spin_until(lambda: js_box[0] is not None, 3.0)
# Two-point: current -> target (or target hold), BOTH with zero velocity
q_now = list(target)
if js_box[0] is not None:
m = dict(zip(js_box[0].name, js_box[0].position))
try:
q_now = [float(m[j]) for j in JOINTS]
except KeyError:
pass
def send_hold(q, T=0.40):
traj = JointTrajectory()
traj.joint_names = list(JOINTS)
# start
p0 = JointTrajectoryPoint()
p0.positions = [float(v) for v in q]
p0.velocities = [0.0] * 7
p0.accelerations = [0.0] * 7
p0.time_from_start.sec = 0
p0.time_from_start.nanosec = 50_000_000
# end same pose, still zero vel (pin)
p1 = JointTrajectoryPoint()
p1.positions = [float(v) for v in q]
p1.velocities = [0.0] * 7
p1.accelerations = [0.0] * 7
ns = int(round(T * 1e9))
p1.time_from_start.sec = ns // 1_000_000_000
p1.time_from_start.nanosec = ns % 1_000_000_000
traj.points = [p0, p1]
goal = FollowJointTrajectory.Goal()
goal.trajectory = traj
fut = cli.send_goal_async(goal)
spin_until(lambda: fut.done(), 3.0)
if not fut.done():
print(" hold goal send timeout")
return False
gh = fut.result()
if gh is None or not gh.accepted:
print(" hold goal rejected")
return False
rfut = gh.get_result_async()
spin_until(lambda: rfut.done(), T + 3.0)
return True
# 1) snap toward named pose with zero end vel
print(f" action hold → {name} (zero vel)")
send_hold(target, T=0.60)
time.sleep(0.4)
# 2–4) re-pin measured pose several times (align JTC open-loop cmd to encoder)
for i in range(3):
spin_until(lambda: js_box[0] is not None, 1.0)
if js_box[0] is None:
break
m = dict(zip(js_box[0].name, js_box[0].position))
q_pin = [float(m[j]) for j in JOINTS]
print(f" action re-pin #{i+1} @ measured joints (zero vel)")
send_hold(q_pin, T=0.45)
time.sleep(0.25)
# Wait until |qd| small
print(" waiting for joints to stop ...")
t0 = time.monotonic()
while time.monotonic() - t0 < 5.0:
rclpy.spin_once(n, timeout_sec=0.05)
if js_box[0] is None or not js_box[0].velocity:
continue
vmap = dict(zip(js_box[0].name, js_box[0].velocity))
vmax = max(abs(float(vmap.get(j, 0.0))) for j in JOINTS)
if vmax < 0.015:
print(f" settled |qd|_max={vmax:.4f} rad/s")
break
else:
print(" WARN: still some residual velocity")
n.destroy_node()
rclpy.shutdown()
PY
}
if [[ "${KEEP_SERVO}" -eq 0 ]]; then
kill_streamers
else
echo "WARNING: --keep-servo — shaking likely"
fi
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
echo "ERROR: /set_named_pose missing — start run_real.sh" >&2
exit 1
fi
echo "=== joints before ==="
print_joints
echo "=== move: ${POSE} speed=${SPEED} ==="
RESP="$(
ros2 service call /set_named_pose arm_msgs/srv/SetNamedPose \
"{pose_name: '${POSE}', velocity_scale: ${SPEED}, acceleration_scale: 0.0}" 2>&1
)"
echo "${RESP}"
if ! echo "${RESP}" | grep -q 'success=True'; then
echo "ERROR: move failed" >&2
exit 1
fi
WAIT_SEC="$(python3 -c "print(max(2.5, 1.0 / max(float('${SPEED}'), 0.05) + 2.0))")"
echo "Waiting ${WAIT_SEC}s ..."
sleep "${WAIT_SEC}"
echo "=== settle (action, vel=0) ==="
settle_hold_action "${POSE}"
# Final check: servo must still be dead
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
echo "WARNING: servo_node came back — killing again"
kill_streamers
settle_hold_action "${POSE}"
fi
echo "=== joints after ==="
print_joints
echo "Done. Arm should be still."
echo "Tracking later: bash ~/ros2/scripts/run_visual_servo_rt.sh --control"
scripts/install_rdk_offline.sh
Executable
+104
View File
@@ -0,0 +1,104 @@
#!/usr/bin/env bash
# Offline-friendly Flexiv RDK v1.9.1 install for ROS (humble-v1 / flexiv_hardware).
# CMakeLists already supports FLEXIV_RDK_OFFLINE_LIB / vendor/rdk_libs/.
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
RDK_SRC="${RDK_SRC:-$ROOT/src/flexiv_rdk_v1}"
PREFIX="${PREFIX:-$HOME/flexiv_install}"
LIB_NAME="libflexiv_rdk.x86_64-linux-gnu.a"
EXPECTED_HASH="$(tr -d '[:space:]' < "${RDK_SRC}/lib/${LIB_NAME}.sha256")"
VENDOR_LIB="${ROOT}/vendor/rdk_libs/${LIB_NAME}"
JOBS="${JOBS:-$(nproc)}"
# Prefer conda ros_flexiv toolchain (cmake/g++ often only there)
if ! command -v cmake >/dev/null 2>&1; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh" 2>/dev/null \
|| source "${HOME}/miniconda3/etc/profile.d/conda.sh" 2>/dev/null \
|| true
conda activate "${CONDA_ENV:-ros_flexiv}" 2>/dev/null || true
fi
if ! command -v cmake >/dev/null 2>&1; then
echo "ERROR: cmake not found. Activate ros_flexiv (or install cmake) first."
exit 1
fi
echo "Using cmake: $(command -v cmake) ($(cmake --version | head -1))"
mkdir -p "${ROOT}/vendor/rdk_libs" "${PREFIX}"
if [[ -f "${VENDOR_LIB}" ]] && [[ "$(sha256sum "${VENDOR_LIB}" | awk '{print $1}')" == "${EXPECTED_HASH}" ]]; then
echo "OK: ${VENDOR_LIB}"
else
echo "Need ${LIB_NAME} (SHA256 ${EXPECTED_HASH})"
base="https://github.com/flexivrobotics/flexiv_rdk/releases/download/v1.9.1/${LIB_NAME}"
ok=0
for url in \
"${base}" \
"https://ghproxy.net/${base}" \
"https://gh.ddlc.top/${base}" \
"https://hub.gitmirror.com/${base}" \
"https://cf.ghproxy.cc/${base}"
do
echo "TRY ${url}"
if curl -fL --connect-timeout 15 --max-time 120 -o "${VENDOR_LIB}.part" "${url}" \
&& [[ "$(sha256sum "${VENDOR_LIB}.part" | awk '{print $1}')" == "${EXPECTED_HASH}" ]]; then
mv "${VENDOR_LIB}.part" "${VENDOR_LIB}"
ok=1
break
fi
rm -f "${VENDOR_LIB}.part"
done
if [[ "${ok}" -ne 1 ]]; then
cat <<EOF
FAILED to download ${LIB_NAME}.
Manual (any PC with GitHub access):
wget -O ${LIB_NAME} '${base}'
sha256sum ${LIB_NAME} # must be ${EXPECTED_HASH}
scp ${LIB_NAME} $(hostname):${VENDOR_LIB}
Then re-run: $0
EOF
exit 1
fi
fi
export FLEXIV_RDK_OFFLINE_LIB="${VENDOR_LIB}"
echo "Install deps into ${PREFIX} (may still need git mirrors)..."
DEP_SCRIPT=""
for s in \
"${RDK_SRC}/thirdparty/build_and_install_dependencies_not_in_ros2.sh" \
"${RDK_SRC}/thirdparty/build_and_install_dependencies.sh"
do
[[ -f "$s" ]] && DEP_SCRIPT="$s" && break
done
if [[ -n "${DEP_SCRIPT}" ]]; then
bash "${DEP_SCRIPT}" "${PREFIX}" "${JOBS}" || {
echo "WARN: deps script failed. If Eigen/spdlog/Fast-DDS/RBDyn already in ${PREFIX}, continue."
}
else
echo "WARN: no dependency script found"
fi
rm -rf "${RDK_SRC}/build"
mkdir -p "${RDK_SRC}/build" && cd "${RDK_SRC}/build"
cmake .. -DCMAKE_BUILD_TYPE=Release \
-DCMAKE_INSTALL_PREFIX="${PREFIX}" \
-DCMAKE_PREFIX_PATH="${PREFIX}"
cmake --build . --target install -j "${JOBS}"
cat <<EOF
RDK installed to ${PREFIX}
Next:
conda activate ros_flexiv
cd ${ROOT}
colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=${PREFIX} \\
--packages-up-to flexiv_bringup flexiv_hardware arm_bringup
Python (already ready, no GitHub):
/home/cc/rdk_py310/bin/python
bash ${ROOT}/scripts/test_rdk_python.sh
EOF
scripts/lib/export_site_env.py
+563
View File
@@ -0,0 +1,563 @@
#!/usr/bin/env python3
"""Load ~/ros2/config/site.yaml and print bash export lines or JSON.
Usage:
eval "$(python3 ~/ros2/scripts/lib/export_site_env.py)"
python3 ~/ros2/scripts/lib/export_site_env.py --json
python3 ~/ros2/scripts/lib/export_site_env.py --print
"""
from __future__ import annotations
import argparse
import json
import os
import sys
from pathlib import Path
try:
import yaml
except ImportError:
yaml = None # type: ignore
def _root() -> Path:
return Path(__file__).resolve().parents[2]
def site_path() -> Path:
env = os.environ.get("SITE_CONFIG", "").strip()
if env:
return Path(env).expanduser()
return _root() / "config" / "site.yaml"
def load_site(path: Path | None = None) -> dict:
path = path or site_path()
if not path.is_file():
raise FileNotFoundError(f"site config not found: {path}")
text = path.read_text(encoding="utf-8")
if yaml is None:
raise RuntimeError("PyYAML required: pip/conda install pyyaml")
data = yaml.safe_load(text) or {}
if not isinstance(data, dict):
raise ValueError(f"invalid site.yaml: {path}")
return data
def list_robots(data: dict) -> list[dict]:
"""Catalog entries from robot.robots (list or SN-keyed map)."""
raw = (data.get("robot") or {}).get("robots") or []
out: list[dict] = []
if isinstance(raw, dict):
for sn, item in raw.items():
row = dict(item or {}) if isinstance(item, dict) else {}
row.setdefault("sn", str(sn))
out.append(row)
return out
if isinstance(raw, list):
for item in raw:
if isinstance(item, dict) and str(item.get("sn") or "").strip():
out.append(dict(item))
return out
# --arm 89|91|left|right 与 SN 别名。禁止把 89 的手眼/相机套到 91。
_ARM_ALIASES = {
"89": "Rizon4-063989",
"left": "Rizon4-063989",
"左": "Rizon4-063989",
"左臂": "Rizon4-063989",
"左臂89": "Rizon4-063989",
"91": "Rizon4-063991",
"right": "Rizon4-063991",
"右": "Rizon4-063991",
"右臂": "Rizon4-063991",
"右臂91": "Rizon4-063991",
}
_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")
def _deep_merge(base: dict, overlay: dict) -> dict:
out = dict(base)
for k, v in overlay.items():
if isinstance(out.get(k), dict) and isinstance(v, dict):
out[k] = _deep_merge(out[k], v)
else:
out[k] = v
return out
def normalize_robot_sn(data: dict, want: str) -> str:
"""Map 89/91/left/right/label → full Flexiv SN."""
w = (want or "").strip()
if not w:
return w
mapped = _ARM_ALIASES.get(w) or _ARM_ALIASES.get(w.lower())
if mapped:
return mapped
for item in list_robots(data):
sn = str(item.get("sn") or "").strip()
label = str(item.get("label") or "").strip()
arm = str(item.get("arm") or "").strip().lower()
if w == sn or w == label or w.lower() == arm:
return sn
if sn.endswith(w) and w.isdigit() and len(w) >= 2:
return sn
return w
def resolve_robot(data: dict, sn: str | None = None) -> dict:
"""Active arm: ROBOT_SN env overrides robot.sn; catalog fills ip/iface/frames."""
robot = dict(data.get("robot") or {})
catalog = list_robots(data)
want = normalize_robot_sn(
data,
sn or os.environ.get("ROBOT_SN") or os.environ.get("ROBOT_ARM") or robot.get("sn") or "",
)
match: dict = {}
for item in catalog:
if str(item.get("sn") or "").strip() == want:
match = dict(item)
break
out = {k: v for k, v in robot.items() if k != "robots"}
out.update({k: v for k, v in match.items() if k != "robots"})
out["sn"] = want
if not str(out.get("arm") or "").strip():
if "063989" in want:
out["arm"] = "left"
elif "063991" in want:
out["arm"] = "right"
if not str(out.get("joint_prefix") or "").strip() and want:
out["joint_prefix"] = f"{want}_"
if not str(out.get("base_frame") or "").strip() and want:
out["base_frame"] = f"{want}_base_link"
if not str(out.get("ee_frame") or "").strip() and want:
out["ee_frame"] = f"{want}_flange"
net = data.get("network") or {}
if not str(out.get("iface") or "").strip():
out["iface"] = str(net.get("prefer_iface") or "")
if not str(out.get("ip") or "").strip():
out["ip"] = "192.168.100.1"
if not str(out.get("pc_ip") or "").strip():
out["pc_ip"] = "192.168.100.10"
return out
def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]:
"""Merge robots[] camera/gripper/paths/servo onto site defaults for the active SN."""
robot = resolve_robot(data, sn)
merged = dict(data)
for key in _ARM_SECTION_KEYS:
base = merged.get(key) if isinstance(merged.get(key), dict) else {}
ov = robot.get(key) if isinstance(robot.get(key), dict) else {}
merged[key] = _deep_merge(dict(base or {}), dict(ov or {}))
return merged, robot
def camera_ns(data: dict) -> str:
"""Topic namespace for the active wrist camera (d405_left / d405_right)."""
cam = data.get("camera") or {}
name = str(cam.get("name") or "d405").strip().strip("/")
return name or "d405"
def camera_frames(data: dict) -> tuple[str, str, str]:
"""(namespace, optical_frame, depth_topic) for the active arm."""
ns = camera_ns(data)
yolo = data.get("yolo") or {}
frame = str(yolo.get("camera_frame") or "").strip()
depth = str(yolo.get("depth_topic") or "").strip()
if frame in ("", "d405_color_optical_frame"):
frame = f"{ns}_color_optical_frame"
if depth in ("", "/d405/aligned_depth_to_color/image_raw"):
depth = f"/{ns}/aligned_depth_to_color/image_raw"
return ns, frame, depth
def flatten(data: dict) -> dict:
"""Flat SITE_* env map for bash scripts."""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
cam = data.get("camera") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
home = data.get("go_home") or {}
net = data.get("network") or {}
he = data.get("handeye") or {}
grip = data.get("gripper") or {}
named_poses = str(
robot.get("named_poses_file")
or paths.get("named_poses_file")
or str(_root() / "src" / "arm_control" / "config" / "named_poses.yaml")
)
weights = str(paths.get("yolo_weights") or "")
# Resolve active.pt symlink to real path for display
try:
weights_resolved = str(Path(weights).expanduser().resolve())
except Exception:
weights_resolved = weights
out = {
"SITE_CONFIG": str(site_path()),
"SITE_ROS2_ROOT": str(_root()),
"SITE_ROBOT_SN": str(robot.get("sn") or "Rizon4-063989"),
"SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
"SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
"SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
"SITE_BASE_FRAME": str(robot.get("base_frame") or ""),
"SITE_EE_FRAME": str(robot.get("ee_frame") or ""),
"SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
"SITE_ROBOT_LABEL": str(robot.get("label") or ""),
"SITE_ROBOT_ARM": str(robot.get("arm") or ""),
"SITE_NAMED_POSES_FILE": named_poses,
"SITE_ROBOT_CATALOG": ",".join(
str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
),
"SITE_YOLO_WEIGHTS": weights,
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
"SITE_HANDEYE_WAYPOINTS": str(
paths.get("handeye_waypoints")
or str(_root() / "src" / "arm_application" / "config" / "handeye_poses.yaml")
),
"SITE_HANDEYE_CALIBRATE": str(
paths.get("handeye_calibrate")
or str(_root() / "src" / "arm_application" / "config" / "handeye_calibrate.yaml")
),
"SITE_HANDEYE_TOUR": str(
paths.get("handeye_tour")
or str(_root() / "src" / "arm_application" / "config" / "handeye_waypoints.yaml")
),
"SITE_HANDEYE_WORKDIR": str(
paths.get("handeye_workdir") or "/tmp/handeye_calib"
),
"SITE_HANDEYE_FILE": str(
paths.get("handeye_file")
or str(_root() / "src" / "arm_application" / "config" / "handeye_result.yaml")
),
"SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"),
"SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 11)),
"SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 16)),
"SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.011)),
"SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)),
"SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)),
"SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)),
"SITE_HANDEYE_SETTLE_QD": str(float(he.get("settle_qd_max") or 0.015)),
"SITE_HANDEYE_SETTLE_STABLE": str(float(he.get("settle_stable_sec") or 0.6)),
"SITE_HANDEYE_SETTLE_TO": str(float(he.get("settle_timeout_sec") or 8.0)),
"SITE_HANDEYE_PIN_HOLDS": str(int(he.get("pin_holds") if he.get("pin_holds") is not None else 2)),
"SITE_HANDEYE_PIN_SEC": str(float(he.get("pin_hold_sec") or 0.45)),
"SITE_HANDEYE_SKIP_UNSTABLE": (
"true" if he.get("skip_unstable_poses", True) else "false"
),
"SITE_FASTDDS_PROFILE": str(paths.get("fastdds_profile") or ""),
"SITE_CAM_NAME": camera_ns(data),
"SITE_CAM_SERIAL": str(cam.get("serial") or ""),
"SITE_CAM_WIDTH": str(int(cam.get("width") or 848)),
"SITE_CAM_HEIGHT": str(int(cam.get("height") or 480)),
"SITE_CAM_FPS": str(int(cam.get("fps") or 30)),
"SITE_CAM_ENABLE_DEPTH": "1" if cam.get("enable_depth", True) else "0",
"SITE_CAM_ENABLE_INFRA": "1" if cam.get("enable_infra", True) else "0",
"SITE_CAM_PREVIEW": "1" if cam.get("preview", True) else "0",
"SITE_CAM_PREVIEW_STEREO": "1" if cam.get("preview_stereo", True) else "0",
"SITE_CAM_OVERLAY_HOLD": str(float(cam.get("overlay_hold_sec") or 1.5)),
"SITE_CAM_USE_SITE_RES": "1" if cam.get("use_site_resolution", True) else "0",
"SITE_CAM_AUTO_EXPOSURE": "1" if cam.get("auto_exposure", True) else "0",
"SITE_CAM_AE_LIMIT_US": str(float(cam.get("ae_limit_us") if cam.get("ae_limit_us") is not None else 4000)),
"SITE_CAM_EMITTER": "1" if cam.get("emitter_enabled", False) else "0",
"SITE_CAM_LASER_POWER": str(float(cam.get("laser_power") if cam.get("laser_power") is not None else 0)),
"SITE_CAM_EXPOSURE_US": str(float(cam.get("exposure_us") or 0)),
"SITE_CAM_GAIN": str(float(cam.get("gain") or 0)),
"SITE_D435_SERIAL": str(((data.get("cameras") or {}).get("d435") or {}).get("serial") or ""),
"SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
"SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
"SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
"SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or "352122274408"),
"SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or "260522274442"),
"SITE_YOLO_CONF": str(yolo.get("conf") or 0.35),
"SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 416)),
"SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
"SITE_YOLO_MIN_CONF": str(yolo.get("min_conf") or 0.35),
"SITE_GO_HOME_POSE": str(home.get("pose") or "home"),
"SITE_GO_HOME_SPEED": str(home.get("speed") or 0.25),
"SITE_NET_IFACE": str(robot.get("iface") or net.get("prefer_iface") or ""),
"SITE_SERVO_Z_REF": str(servo.get("z_ref_m") or 0.40),
"SITE_EE_X_M": str(float(servo.get("ee_x_m") or 0.25)),
"SITE_EE_Y_M": str(float(servo.get("ee_y_m") or 0.0)),
"SITE_EE_Z_M": str(float(servo.get("ee_z_m") or 0.24)),
"SITE_SERVO_WATCHDOG": str(servo.get("watchdog_timeout_sec") or 0.3),
"SITE_CAMERA_FRAME": camera_frames(data)[1],
"SITE_GRIP_VENDOR": str(grip.get("vendor") or "dh"),
"SITE_GRIP_MODEL": str(grip.get("model") or "PGEA-100-20-W"),
"SITE_GRIP_SN": str(grip.get("sn") or ""),
"SITE_GRIP_BACKEND": str(grip.get("backend") or "usb"),
"SITE_GRIP_DEVICE": str(grip.get("device_name") or "GripperDahuanModbus"),
"SITE_GRIP_FORCE_N": str(float(grip.get("force_n") or 50.0)),
"SITE_GRIP_VEL_MPS": str(float(grip.get("vel_mps") or 0.05)),
"SITE_GRIP_PORT": str(grip.get("port") or "/dev/ttyUSB0"),
"SITE_GRIP_BAUD": str(int(grip.get("baud") or 115200)),
"SITE_GRIP_SLAVE": str(int(grip.get("slave_id") or 1)),
"SITE_GRIP_STROKE_MM": str(float(grip.get("stroke_mm") or 20.0)),
"SITE_GRIP_FORCE": str(int(grip.get("force_pct") or 50)),
"SITE_GRIP_SPEED": str(int(grip.get("speed_pct") or 50)),
"SITE_GRIP_AUTO_INIT": "true" if grip.get("auto_init", True) else "false",
"SITE_GRIP_RS485_RTS": str(grip.get("rs485_rts") or ""),
"SITE_HOLE_BACKEND": str((data.get("hole_detect") or {}).get("backend") or "two_stage"),
"SITE_HOLE_WEIGHTS": str(
(data.get("hole_detect") or {}).get("weights")
or paths.get("hole_weights")
or str(_root() / "models" / "hole_insert" / "best.pt")
),
"SITE_HOLE_WORKPIECE_WEIGHTS": str(
(data.get("hole_detect") or {}).get("workpiece_weights")
or paths.get("hole_workpiece_weights")
or weights
),
"SITE_HOLE_CONFIG": str(
(data.get("hole_detect") or {}).get("config")
or (
str(_root() / "src" / "arm_application" / "config" / "hole_detect.yaml")
if str((data.get("hole_detect") or {}).get("backend") or "two_stage")
in ("two_stage", "hough_roi")
else str(_root() / "src" / "arm_application" / "config" / "hole_insert.yaml")
)
),
"SITE_TACTILE_BACKEND": str((data.get("tactile") or {}).get("backend") or "orisys"),
"SITE_TACTILE_CONFIG": str((data.get("tactile") or {}).get("config") or "dd03"),
"SITE_TACTILE_CONFIG_FILE": str(
(data.get("tactile") or {}).get("config_file")
or str(_root() / "src" / "arm_application" / "config" / "orisys" / "dd03.json")
),
"SITE_TACTILE_LEFT": str((data.get("tactile") or {}).get("left_video", 0)),
"SITE_TACTILE_RIGHT": str((data.get("tactile") or {}).get("right_video", 2)),
}
return out
def gripper_overrides(data: dict) -> dict:
"""ROS parameter dict for dh_pgea_node (DH Modbus, not Flexiv gripper)."""
data, _robot = apply_active_arm(data)
g = data.get("gripper") or {}
return {
"port": str(g.get("port") or "/dev/ttyUSB0"),
"baud": int(g.get("baud") or 115200),
"slave_id": int(g.get("slave_id") or 1),
"stroke_mm": float(g.get("stroke_mm") or 20.0),
"force_pct": int(g.get("force_pct") or 50),
"speed_pct": int(g.get("speed_pct") or 50),
"auto_init": bool(g.get("auto_init", True)),
"state_hz": float(g.get("state_hz") or 20.0),
"model": str(g.get("model") or "PGEA-100-20-W"),
"serial_number": str(g.get("sn") or ""),
"open_position": int(g.get("open_position") if g.get("open_position") is not None else 1000),
"close_position": int(g.get("close_position") if g.get("close_position") is not None else 0),
"rs485_rts": str(g.get("rs485_rts") or ""),
}
def perception_overrides(data: dict) -> dict:
"""ROS parameter dict for perception_node."""
data, _robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
cam_ns, cam_frame, depth_topic = camera_frames(data)
obj = str(yolo.get("object_frame") or "").strip()
if not obj or obj == "tracked_object":
arm = str((_robot or {}).get("arm") or "")
sn = str((_robot or {}).get("sn") or "")
if "063991" in sn or arm == "right":
obj = "tracked_object_91"
elif "063989" in sn or arm == "left":
obj = "tracked_object_89"
else:
obj = "tracked_object"
return {
"weights": str(paths.get("yolo_weights") or ""),
"conf": float(yolo.get("conf") or 0.10),
"iou": float(yolo.get("iou") or 0.45),
"imgsz": int(yolo.get("imgsz") or 416),
"device": str(yolo.get("device") or "0"),
"half_precision": bool(yolo.get("half_precision", True)),
"backend": str(yolo.get("backend") or "auto"),
"mp_infer": bool(yolo.get("mp_infer", True)),
"min_conf": float(yolo.get("min_conf") or 0.12),
"vision_hz": float(yolo.get("vision_hz") or 30.0),
"infer_max_width": int(yolo.get("infer_max_width") if yolo.get("infer_max_width") is not None else 0),
"box_ema_alpha": float(yolo.get("box_ema_alpha") or 0.85),
"miss_clear_frames": int(yolo.get("miss_clear_frames") or 12),
"overlay_hold_on_miss": bool(yolo.get("overlay_hold_on_miss", True)),
"jump_px": float(yolo.get("jump_px") or 280.0),
"jump_z_m": float(yolo.get("jump_z_m") or 0.20),
"z_ref_m": float(yolo.get("z_ref_m") or 0.22),
"min_depth_m": float(yolo.get("min_depth_m") or 0.04),
"max_depth_m": float(yolo.get("max_depth_m") or 0.55),
"work_z_max_m": float(yolo.get("work_z_max_m") or 0.50),
"hold_point_on_miss": bool(yolo.get("hold_point_on_miss", True)),
"miss_hold_point_frames": int(yolo.get("miss_hold_point_frames") or 2),
"meas_blend": float(yolo.get("meas_blend") or 0.65),
"z_alpha_min": float(yolo.get("z_alpha_min") or 0.10),
"z_alpha_max": float(yolo.get("z_alpha_max") or 0.60),
"z_alpha_gain": float(yolo.get("z_alpha_gain") or 2.0),
"camera_frame": cam_frame,
"object_frame": obj,
"depth_topic": depth_topic,
"camera_source": cam_ns,
"feature_mode": str(yolo.get("feature_mode") or "plane"),
"plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
"plane_band_m": float(yolo.get("plane_band_m") or 0.06),
}
def servo_overrides(data: dict) -> dict:
"""ROS parameter dict for servo_node."""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
_ns, cam_frame, _depth = camera_frames(data)
out = {
"joint_prefix": str(robot.get("joint_prefix") or ""),
"base_frame": str(robot.get("base_frame") or ""),
"ee_frame": str(robot.get("ee_frame") or ""),
"handeye_file": str(paths.get("handeye_file") or ""),
"camera_frame": cam_frame,
"handeye_child_frame": cam_frame,
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
"z_ref_m": float(servo.get("z_ref_m") or yolo.get("z_ref_m") or 0.40),
}
# Pass through remaining servo keys that match node params
keys = [
"control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec",
"max_extrapolate_sec", "predict_enable", "predict_sec",
"predict_v_max", "predict_jump_m",
"kp_xy", "ki_xy", "kd_xy", "kp_z", "ki_z", "kd_z", "i_sep_pos_m",
"deadzone_m", "deadzone_z_m", "v_max_xy", "v_max_z", "v_approach_z",
"z_min_m", "approach_xy_gate_m", "min_ee_z_m", "e_xy_clip_m",
"ee_x_m", "ee_y_m", "ee_z_m", "ee_hold_xy", "ee_vertical_down",
"xy_track", "hover_offset_ee_xy_m", "arm_label",
"ee_hold_kp", "ee_w_max",
"lock_hold_sec",
"a_max", "max_qdot", "damping",
"vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
"min_cmd_delta_rad", "point_topic", "traj_topic",
"use_static_handeye_tf",
"workspace_xyz_min", "workspace_xyz_max",
]
for k in keys:
if k in servo:
out[k] = servo[k]
return out
def main() -> int:
ap = argparse.ArgumentParser()
ap.add_argument("--json", action="store_true")
ap.add_argument("--print", action="store_true", dest="do_print")
ap.add_argument("--config", default="")
ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|full SN")
args = ap.parse_args()
if args.sn:
os.environ["ROBOT_SN"] = str(args.sn)
path = Path(args.config).expanduser() if args.config else site_path()
data = load_site(path)
flat = flatten(data)
if args.json:
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
return 0
if args.do_print:
print(f"site config: {path}")
print(
f" robot SN : {flat['SITE_ROBOT_SN']}"
f"{(' (' + flat['SITE_ROBOT_LABEL'] + ')') if flat.get('SITE_ROBOT_LABEL') else ''}"
f" arm={flat.get('SITE_ROBOT_ARM') or '?'}"
)
print(f" catalog : {flat.get('SITE_ROBOT_CATALOG') or '(none)'}")
print(f" robot IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}")
print(f" frames : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}")
print(f" tool name : {flat.get('SITE_ROBOT_TOOL_NAME') or '(keep current)'}")
catalog_sns = [str(r.get("sn") or "") for r in list_robots(data)]
if catalog_sns and flat["SITE_ROBOT_SN"] not in catalog_sns:
print(
f" WARNING: robot.sn {flat['SITE_ROBOT_SN']!r} is not in robots[]",
file=sys.stderr,
)
print(f" yolo weights : {flat['SITE_YOLO_WEIGHTS']}")
print(f" resolved : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}")
print(f" handeye : {flat['SITE_HANDEYE_FILE']}")
print(f" handeye wp : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" handeye dir : {flat['SITE_HANDEYE_WORKDIR']}")
print(f" named poses : {flat.get('SITE_NAMED_POSES_FILE')}")
print(
" board : "
f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
f"inner, square={flat['SITE_HANDEYE_SQUARE_M']} m"
)
print(
f" camera : serial={flat['SITE_CAM_SERIAL'] or '(empty)'} "
f"{flat['SITE_CAM_WIDTH']}x{flat['SITE_CAM_HEIGHT']}@{flat['SITE_CAM_FPS']} "
f"ae={flat.get('SITE_CAM_AUTO_EXPOSURE')} "
f"exp={flat.get('SITE_CAM_EXPOSURE_US')}us "
f"gain={flat.get('SITE_CAM_GAIN')} "
f"ae_limit={flat.get('SITE_CAM_AE_LIMIT_US')}us emitter={flat.get('SITE_CAM_EMITTER')}"
)
print(
f" catalog cam : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} "
f"rightD405={flat.get('SITE_D405_RIGHT_SERIAL')} "
f"D435={flat.get('SITE_D435_SERIAL')}"
)
print(
f" gripper : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}"
)
print(
f" hole_detect : backend={flat.get('SITE_HOLE_BACKEND')} "
f"weights={flat.get('SITE_HOLE_WEIGHTS')}"
)
print(
f" tactile : backend={flat.get('SITE_TACTILE_BACKEND')} "
f"L={flat.get('SITE_TACTILE_LEFT')} R={flat.get('SITE_TACTILE_RIGHT')} "
f"cfg={flat.get('SITE_TACTILE_CONFIG')}"
)
print(f" go_home : pose={flat['SITE_GO_HOME_POSE']} speed={flat['SITE_GO_HOME_SPEED']}")
he_path = Path(flat["SITE_HANDEYE_FILE"]).expanduser()
if not he_path.is_file():
print(f" WARNING: hand-eye file missing: {he_path}", file=sys.stderr)
else:
try:
he_data = yaml.safe_load(he_path.read_text(encoding="utf-8")) or {}
except Exception:
he_data = {}
if isinstance(he_data, dict) and (
he_data.get("uncalibrated") or "T_4x4" not in he_data
):
print(
f" WARNING: {he_path.name} is not calibrated for this arm "
f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})",
file=sys.stderr,
)
if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"):
print(
" WARNING: 91 is using the LEFT wrist D405 serial — check robots[].camera.serial",
file=sys.stderr,
)
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
print(
" NOTE: 本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
file=sys.stderr,
)
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
if not w.is_file():
print(f" WARNING: weights file missing: {w}", file=sys.stderr)
return 2
return 0
# bash exports
for k, v in flat.items():
# escape for eval
vv = str(v).replace("'", "'\"'\"'")
print(f"export {k}='{vv}'")
return 0
if __name__ == "__main__":
raise SystemExit(main())
scripts/lib/load_site_config.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/usr/bin/env bash
# Load ~/ros2/config/site.yaml into SITE_* environment variables.
# Usage (from other scripts):
# source "${ROOT}/scripts/lib/load_site_config.sh"
_SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)"
_SITE_EXPORTER="${_SITE_ROOT}/scripts/lib/export_site_env.py"
_SITE_FILE="${SITE_CONFIG:-${_SITE_ROOT}/config/site.yaml}"
if [[ ! -f "${_SITE_FILE}" ]]; then
echo "[site] WARN: missing ${_SITE_FILE}" >&2
else
# shellcheck disable=SC1090
eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"
export SITE_CONFIG="${_SITE_FILE}"
# Resolve 89/91/left/right aliases to the full Flexiv SN for RDK.
if [[ -n "${SITE_ROBOT_SN:-}" ]]; then
export ROBOT_SN="${SITE_ROBOT_SN}"
fi
fi
scripts/lib/parse_arm_args.sh
+26
View File
@@ -0,0 +1,26 @@
#!/usr/bin/env bash
# Parse --sn / --arm from argv. Must be sourced BEFORE load_site_config.sh
# so ROBOT_SN is visible to export_site_env.py.
#
# source "${ROOT}/scripts/lib/parse_arm_args.sh"
# parse_arm_args "$@"
# set -- "${PARSE_ARM_REST[@]}"
#
# Aliases (89|91|left|right) are resolved in export_site_env.py.
parse_arm_args() {
PARSE_ARM_REST=()
while [[ $# -gt 0 ]]; do
case "$1" in
--sn|--arm)
export ROBOT_SN="${2:?}"
export ROBOT_ARM="${2}"
shift 2
;;
*)
PARSE_ARM_REST+=("$1")
shift
;;
esac
done
}
scripts/list_realsense.sh
Executable
+21
View File
@@ -0,0 +1,21 @@
#!/usr/bin/env bash
# List plugged RealSense cameras (serials used in site.yaml).
set -eo pipefail
python3 - <<'PY'
import pyrealsense2 as rs
known = {
"352122274408": "left wrist D405 (arm 89, parallel gripper)",
"260522274442": "right wrist D405 (arm 91, rotary gripper)",
"262522073161": "overhead D435 (between arms, eye-to-hand)",
}
ds = list(rs.context().devices)
print(f"n={len(ds)}")
for d in ds:
sn = d.get_info(rs.camera_info.serial_number)
name = d.get_info(rs.camera_info.name)
usb = d.get_info(rs.camera_info.usb_type_descriptor)
fw = d.get_info(rs.camera_info.firmware_version)
role = known.get(sn, "(unassigned — add to site.yaml cameras:)")
print(f" {name:22} serial={sn} USB={usb} fw={fw}")
print(f" -> {role}")
PY
scripts/merge_handeye_poses.py
Executable
+143
View File
@@ -0,0 +1,143 @@
#!/usr/bin/env python3
"""Merge taught joint lines into handeye waypoints YAML.
Input (from handeye_teach / handeye_waypoints append file):
# pose 0
0.123456, -0.700000, ...
Target can be either:
- config/handeye/waypoints.yaml (/**: ros__parameters)
- handeye_calibrate.yaml (handeye_calibrate: ros__parameters)
Usage:
python3 scripts/merge_handeye_poses.py \\
~/ros2/config/handeye/taught_poses.txt \\
~/ros2/config/handeye/waypoints.yaml
# or via wrapper:
bash ~/ros2/scripts/run_handeye.sh merge
"""
from __future__ import annotations
import re
import sys
from pathlib import Path
def parse_poses(path: Path) -> list[list[float]]:
lines = path.read_text(encoding="utf-8").splitlines()
poses: list[list[float]] = []
buf: list[float] = []
num_re = re.compile(r"[-+]?(?:\d+\.\d*|\.\d+|\d+)(?:[eE][-+]?\d+)?")
for raw in lines:
line = raw.split("#", 1)[0].strip()
if not line:
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints, got {len(buf)}: {buf}")
poses.append(buf)
buf = []
continue
vals = [float(x) for x in num_re.findall(line)]
if not vals:
continue
if len(vals) == 7:
poses.append(vals)
elif len(vals) < 7:
buf.extend(vals)
if len(buf) > 7:
raise ValueError(f"Too many joint values in line: {line!r}")
else:
poses.append(vals[:7])
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints at EOF, got {len(buf)}")
poses.append(buf)
return poses
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
rows = [f"{indent}waypoint_positions: ["]
for i, q in enumerate(poses):
rows.append(f"{indent} # pose {i}")
s = ", ".join(f"{v:.6f}" for v in q) + ","
rows.append(f"{indent} {s}")
rows.append(f"{indent}]")
return "\n".join(rows)
def merge(yaml_path: Path, poses: list[list[float]]) -> None:
text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n")
# Detect indent of waypoint_positions (2 or 4 spaces typical)
m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE)
indent = m_ind.group(1) if m_ind else " "
block = format_positions_block(poses, indent=indent)
pattern = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]",
re.MULTILINE,
)
# Also match a one-line empty list: waypoint_positions: []
one_line = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]",
re.MULTILINE,
)
if pattern.search(text):
new_text = pattern.sub(block, text, count=1)
elif one_line.search(text):
new_text = one_line.sub(block, text, count=1)
else:
raise RuntimeError("waypoint_positions block not found in YAML")
# Keep num_poses in sync when present
n = len(poses)
if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE):
new_text = re.sub(
r"^(\s*num_poses:\s*)\d+",
rf"\g<1>{n}",
new_text,
count=1,
flags=re.MULTILINE,
)
else:
# Insert num_poses just before waypoint_positions
new_text = new_text.replace(
block,
f"{indent}num_poses: {n}\n{block}",
1,
)
yaml_path.write_text(new_text, encoding="utf-8")
def main(argv: list[str]) -> int:
if len(argv) != 3:
print(
"Usage: merge_handeye_poses.py <taught_poses.txt> <waypoints.yaml>",
file=sys.stderr,
)
return 1
src = Path(argv[1]).expanduser()
dst = Path(argv[2]).expanduser()
if not src.is_file():
print(f"Not found: {src}", file=sys.stderr)
return 2
if not dst.is_file():
print(f"Not found: {dst}", file=sys.stderr)
return 3
poses = parse_poses(src)
if len(poses) < 5:
print(f"Need >=5 poses, got {len(poses)}", file=sys.stderr)
return 4
merge(dst, poses)
print(f"Merged {len(poses)} poses -> {dst}")
return 0
if __name__ == "__main__":
raise SystemExit(main(sys.argv))
scripts/rdk_drag_bridge.py
Executable
+329
View File
@@ -0,0 +1,329 @@
#!/usr/bin/env python3
"""RDK Free-Drive bridge (must run under rdk_py310 / Python 3.10).
Why soft-impedance drag
-----------------------
Under AUTO (Remote) the Studio Free Drive button is unavailable (remote owns
the robot). RDK FloatingJoint/FloatingCartesian often need an extra license.
So we default to soft NRT_JOINT_IMPEDANCE: low stiffness + target tracks
measured joints → arm can be pushed by hand while Remote stays on.
IMPORTANT: flexivrdk.Robot is NOT thread-safe. All robot API calls (including
the drag loop) share one lock. Joint reads during drag use a cache so Enter
does not race Stop()/SendJointPosition.
Line protocol (JSON on stdout only; RDK logs → stderr):
{"cmd":"ping"}
{"cmd":"connect","sn":"Rizon4-063989"}
{"cmd":"drag_on","mode":"soft"} # soft | floating
{"cmd":"drag_off"}
{"cmd":"joints"}
{"cmd":"quit"}
"""
from __future__ import annotations
import json
import sys
import threading
import time
from typing import List, Optional
_JSON_OUT = sys.stdout
sys.stdout = sys.stderr
def reply(obj: dict) -> None:
_JSON_OUT.write(json.dumps(obj, ensure_ascii=False) + '\n')
_JSON_OUT.flush()
def log(msg: str) -> None:
print(f'[rdk_bridge] {msg}', file=sys.stderr, flush=True)
class SoftDrag:
def __init__(self, robot, flexivrdk, lock: threading.Lock) -> None:
self.robot = robot
self.flexivrdk = flexivrdk
self._lock = lock
self._stop = threading.Event()
self._thread: Optional[threading.Thread] = None
self.latest_q: List[float] = []
self._groups = {}
def start(self) -> None:
mode = self.flexivrdk.Mode
robot = self.robot
with self._lock:
robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
st = robot.states()
self.latest_q = [float(v) for v in list(st.q)[:7]]
try:
info = robot.info()
self._groups = dict(getattr(info, 'single_arm_groups', None) or {})
except Exception as exc:
log(f'info.single_arm_groups: {exc}')
self._groups = {}
if self._groups:
for g in self._groups:
try:
nom = list(info.K_q_nom[g])
soft = [max(8.0, 0.08 * float(v)) for v in nom]
robot.SetJointImpedance(g, soft)
log(f'SetJointImpedance {g} K={soft}')
except Exception as exc:
log(f'SetJointImpedance group {g}: {exc}')
else:
n = max(7, len(self.latest_q))
try:
robot.SetJointImpedance([35.0] * n, [0.7] * n)
except Exception as exc:
log(f'SetJointImpedance flat: {exc}')
self._stop.clear()
self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True)
self._thread.start()
def _loop(self) -> None:
robot = self.robot
flexivrdk = self.flexivrdk
while not self._stop.is_set():
try:
with self._lock:
if self._stop.is_set():
break
st = robot.states()
q = [float(v) for v in list(st.q)[:7]]
self.latest_q = q
if self._groups:
cmds = {}
for g in self._groups:
try:
gq = list(st[g].q)
except Exception:
gq = q
n = len(gq)
cmds[g] = flexivrdk.NrtJointPositionCmd(
gq, [0.0] * n, [1.5] * n, [2.0] * n
)
robot.SendJointPosition(cmds)
else:
n = len(q)
robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n)
except Exception as exc:
log(f'soft loop: {exc}')
time.sleep(0.15)
# ~20 Hz — keep lock released most of the time
for _ in range(10):
if self._stop.is_set():
return
time.sleep(0.005)
def stop(self) -> None:
self._stop.set()
th = self._thread
if th is not None:
th.join(timeout=4.0)
if th.is_alive():
log('soft loop still alive after join (will idle anyway)')
self._thread = None
def _ensure_operational(robot) -> Optional[str]:
try:
if robot.fault():
robot.ClearFault()
time.sleep(0.5)
if not robot.operational():
for fn in ('ServoOn', 'Enable'):
if hasattr(robot, fn):
try:
getattr(robot, fn)()
break
except Exception:
pass
t0 = time.monotonic()
while not robot.operational() and time.monotonic() - t0 < 10:
time.sleep(0.2)
if not robot.operational():
return (
'Robot not operational (fault/E-Stop?). '
'Clear fault on pendant, release E-Stop, AUTO Remote.'
)
except Exception as exc:
return str(exc)
return None
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
"""Switch to IDLE without blocking the stdin loop forever."""
done = threading.Event()
def run() -> None:
try:
acquired = lock.acquire(timeout=2.0)
try:
if acquired:
try:
robot.Stop()
except Exception as exc:
log(f'Stop: {exc}')
try:
robot.SwitchMode(flexivrdk.Mode.IDLE)
except Exception as exc:
log(f'SwitchMode IDLE: {exc}')
finally:
if acquired:
lock.release()
finally:
done.set()
threading.Thread(target=run, name='idle', daemon=True).start()
ok = done.wait(timeout)
if not ok:
log('IDLE/Stop timed out — stdin loop continues')
return ok
def main() -> int:
import flexivrdk
robot = None
drag_on = False
soft: Optional[SoftDrag] = None
rlock = threading.Lock()
cached_q: List[float] = []
for line in sys.stdin:
line = line.strip()
if not line:
continue
try:
msg = json.loads(line)
except Exception as exc: # noqa: BLE001
reply({'ok': False, 'error': f'bad json: {exc}'})
continue
cmd = str(msg.get('cmd', '')).strip()
try:
if cmd == 'ping':
reply({'ok': True, 'py': sys.version.split()[0]})
elif cmd == 'connect':
sn = str(msg.get('sn', 'Rizon4-063989'))
iface = str(msg.get('iface') or '')
robot = None
last_err = None
attempts = []
if iface:
attempts.extend([(sn, [iface]), (sn, [iface], True)])
attempts.append((sn,))
for args in attempts:
try:
robot = flexivrdk.Robot(*args)
log(f'connected {sn} args={args!r}')
last_err = None
break
except TypeError:
continue
except Exception as exc:
last_err = exc
log(f'connect {args!r}: {exc}')
robot = None
if robot is None:
reply({'ok': False, 'error': str(last_err or 'connect failed')})
continue
err = _ensure_operational(robot)
if err:
reply({'ok': False, 'error': err})
continue
with rlock:
q = [float(v) for v in list(robot.states().q)[:7]]
cached_q = q
reply({'ok': True, 'sn': sn, 'q': q})
elif cmd == 'drag_on':
if robot is None:
reply({'ok': False, 'error': 'not connected'})
continue
err = _ensure_operational(robot)
if err:
reply({'ok': False, 'error': err})
continue
prefer = str(msg.get('mode', 'soft')).strip().lower()
if prefer in ('floating', 'primitive', 'floatingjoint'):
name = str(msg.get('primitive', 'FloatingJoint')).replace('()', '')
try:
with rlock:
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
robot.ExecutePrimitive(name, {})
drag_on = True
reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name})
continue
except Exception as exc:
log(f'Floating failed, fallback soft: {exc}')
try:
if soft is not None:
soft.stop()
soft = SoftDrag(robot, flexivrdk, rlock)
soft.start()
if soft.latest_q:
cached_q = list(soft.latest_q)
drag_on = True
log('soft drag ON')
reply({
'ok': True,
'drag': True,
'mode': 'soft',
'hint': 'Push the arm gently; low joint impedance. Enter=photo (s=hold).',
})
except Exception as exc:
reply({'ok': False, 'error': f'soft drag failed: {exc}'})
elif cmd == 'drag_off':
log('drag_off requested')
if soft is not None:
if soft.latest_q:
cached_q = list(soft.latest_q)
soft.stop()
soft = None
if robot is not None:
_idle_async(robot, rlock, flexivrdk, timeout=4.0)
drag_on = False
log('drag_off done')
reply({'ok': True, 'drag': False, 'q': cached_q})
elif cmd == 'joints':
q: Optional[List[float]] = None
if soft is not None and len(soft.latest_q) >= 7:
q = list(soft.latest_q)
elif len(cached_q) >= 7:
q = list(cached_q)
if q is None or len(q) < 7:
reply({'ok': False, 'error': 'no joint cache'})
continue
cached_q = q
reply({'ok': True, 'q': q, 'drag': drag_on})
elif cmd == 'quit':
if soft is not None:
soft.stop()
soft = None
if robot is not None and drag_on:
_idle_async(robot, rlock, flexivrdk, timeout=3.0)
reply({'ok': True})
return 0
else:
reply({'ok': False, 'error': f'unknown cmd {cmd}'})
except Exception as exc: # noqa: BLE001
log(f'cmd={cmd} error: {exc}')
reply({'ok': False, 'error': str(exc)})
return 0
if __name__ == '__main__':
raise SystemExit(main())
scripts/rdk_gripper.py
Executable
+207
View File
@@ -0,0 +1,207 @@
#!/usr/bin/env python3
"""Control Studio device GripperDahuan / GripperDahuanModbus via Flexiv RDK.
Must run under RDK Python (rdk_py310). Uses a *lite* RDK instance so it can
share the robot with run_real.sh (ros2_control holds the normal RDK).
The controller talks RS485 itself (its own /dev/ttyUSB0), not the IPC serial.
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py --list
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py status
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py init
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py open
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py close
"""
from __future__ import annotations
import argparse
import sys
import time
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts"))
from rdk_switch_tool import _iface_ipv4s # noqa: E402
def connect_lite(sn: str, iface: str = ""):
import flexivrdk
ips = _iface_ipv4s(iface)
if ips:
try:
return flexivrdk.Robot(sn, ips, True, True)
except TypeError:
pass
except RuntimeError as exc:
msg = str(exc).lower()
if "whitelist" not in msg and "filtered out" not in msg:
raise
return flexivrdk.Robot(sn, [], True, True)
def _fmt_params(p) -> str:
return (
f"width=[{p.min_width:.4f},{p.max_width:.4f}] m "
f"vel=[{p.min_vel:.4f},{p.max_vel:.4f}] m/s "
f"force=[{p.min_force:.1f},{p.max_force:.1f}] N"
)
def _fmt_states(s) -> str:
return f"width={s.width:.4f} m force={s.force:.2f} N moving={s.is_moving}"
def _clamp(val: float, lo: float, hi: float) -> float:
if lo > hi:
lo, hi = hi, lo
return max(lo, min(hi, val))
def _move(grip, params, width: float, vel: float, force: float) -> None:
vmin = float(params.min_vel)
vmax = float(params.max_vel)
if vmax <= 0:
vmax = 0.05
# Move velocity cannot be 0 (RDK).
vel = _clamp(vel, max(vmin, 1e-4), vmax)
force = _clamp(force, float(params.min_force), float(params.max_force))
width = _clamp(width, float(params.min_width), float(params.max_width))
print(f"Move width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N")
grip.Move(width, vel, force)
def main() -> int:
ap = argparse.ArgumentParser(description="Flexiv RDK gripper (GripperDahuanModbus)")
ap.add_argument("--sn", default="")
ap.add_argument("--iface", default="")
ap.add_argument("--device", default="", help="Studio Device name")
ap.add_argument("--force-n", type=float, default=0.0, help="force limit [N]; 0=mid")
ap.add_argument("--vel", type=float, default=0.0, help="speed [m/s]; 0=half max")
ap.add_argument(
"cmd",
nargs="?",
default="status",
choices=["list", "status", "init", "open", "close", "pos", "enable"],
)
ap.add_argument("value", nargs="?", default="", help="pos: meters or 0–1000")
args = ap.parse_args()
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
from export_site_env import load_site, resolve_robot # type: ignore
data = load_site()
robot_cfg = resolve_robot(data)
g = data.get("gripper") or {}
sn = (args.sn or robot_cfg.get("sn") or "").strip()
iface = (args.iface or robot_cfg.get("iface") or "").strip()
device = (args.device or g.get("device_name") or "GripperDahuanModbus").strip()
if not sn:
print("ERROR: robot SN missing", file=sys.stderr)
return 2
import flexivrdk
print(f"=== RDK lite gripper SN={sn} device={device} iface={iface or 'auto'} ===")
robot = connect_lite(sn, iface)
dev = flexivrdk.Device(robot)
names = list(dev.list() or [])
if args.cmd == "list":
print("Studio devices:")
for n in names:
try:
en = dev.enabled(n)
cn = dev.connected(n)
except Exception:
en, cn = "?", "?"
mark = " <--" if n == device else ""
print(f" {n}: enabled={en} connected={cn}{mark}")
return 0
if device not in names:
print(
f"ERROR: device {device!r} not in Studio pool.\n available: {', '.join(names)}",
file=sys.stderr,
)
return 1
print(
f" Studio: enabled={dev.enabled(device)} connected={dev.connected(device)}"
)
if not dev.connected(device):
print(
"WARN: connected=False — 控制器自己的 /dev/ttyUSB0 没接到爪。\n"
" USB–RS485 要插 Flexiv 机箱 USB（不是工控机），A/B + 24V 到 PGEA。\n"
" 或用法兰工具口 485（腕部白绿+/绿-）。"
)
grip = flexivrdk.Gripper(robot)
if args.cmd in ("enable", "status", "init", "open", "close", "pos"):
try:
grip.Enable(device)
print(f"Enable [{device}] OK")
except Exception as exc:
print(f"ERROR: Enable failed: {exc}", file=sys.stderr)
return 1
params = grip.params()
print(" params:", _fmt_params(params))
print(" states:", _fmt_states(grip.states()))
force = float(args.force_n or g.get("force_n") or 0.0)
if force <= 0:
force = 0.5 * (float(params.min_force) + float(params.max_force))
vel = float(args.vel or g.get("vel_mps") or 0.0)
if vel <= 0:
vel = 0.5 * float(params.max_vel)
if args.cmd in ("enable", "status"):
if not dev.connected(device):
return 2
return 0
if args.cmd == "init":
print("Init (fingers home, ~10 s) …")
grip.Init()
time.sleep(2.0)
print(" states:", _fmt_states(grip.states()))
return 0
if args.cmd == "open":
_move(grip, params, float(params.max_width), vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
return 0
if args.cmd == "close":
_move(grip, params, float(params.min_width), vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
return 0
if args.cmd == "pos":
if args.value == "":
print("usage: pos <meters|0-1000>", file=sys.stderr)
return 2
raw = float(args.value)
if raw > 1.5:
span = float(params.max_width) - float(params.min_width)
width = float(params.min_width) + (raw / 1000.0) * span
else:
width = raw
_move(grip, params, width, vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
return 0
return 0
if __name__ == "__main__":
try:
raise SystemExit(main())
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
raise SystemExit(1)
scripts/rdk_switch_tool.py
Executable
+292
View File
@@ -0,0 +1,292 @@
#!/usr/bin/env python3
"""Switch the Flexiv active tool (TCP + payload) to a Studio-configured name.
Must run under RDK Python (rdk_py310). Robot must be IDLE; stop run_real.sh first
if ros2_control already owns the RDK connection.
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --list
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --name Flange
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py # site.yaml robot.tool_name
"""
from __future__ import annotations
import argparse
import sys
import time
def _single_arm_groups(robot):
"""RDK 1.9 Tool API is single-arm (no JointGroup). Older builds use groups."""
info = robot.info()
groups = getattr(info, "single_arm_groups", None) or {}
if isinstance(groups, dict):
keys = list(groups.keys())
else:
keys = list(groups)
if keys:
return keys
try:
import flexivrdk
if hasattr(flexivrdk, "JointGroup"):
return [flexivrdk.JointGroup.ARM_1]
except Exception:
pass
return [None] # sentinel: call Tool.name()/Switch() without a group
def _tool_name(tool, group):
if group is None:
return tool.name()
return tool.name(group)
def _tool_params(tool, group):
if group is None:
return tool.params()
return tool.params(group)
def _tool_switch(tool, group, target: str) -> None:
if group is None:
tool.Switch(target)
return
tool.Switch(group, target)
def _group_name(group) -> str:
if group is None:
return "arm"
try:
import flexivrdk
return str(flexivrdk.kJointGroupNames[group])
except Exception:
return str(group)
def _fmt_params(params) -> str:
mass = getattr(params, "mass", None)
com = list(getattr(params, "CoM", []) or [])
tcp = list(getattr(params, "tcp_location", []) or [])
com_s = ",".join(f"{float(v):.4f}" for v in com[:3]) if com else "?"
tcp_s = ",".join(f"{float(v):.4f}" for v in tcp[:3]) if tcp else "?"
return f"mass={mass} kg CoM=[{com_s}] m TCP_xyz=[{tcp_s}] m"
def _ensure_idle(robot, mode) -> None:
try:
robot.SwitchMode(mode.IDLE)
return
except Exception:
pass
if hasattr(robot, "Enable"):
robot.Enable()
else:
robot.ServoOn()
for _ in range(40):
if robot.operational():
break
time.sleep(0.25)
if not robot.operational():
raise RuntimeError(
"robot not operational — AUTO Remote + Enable, release E-stop"
)
robot.SwitchMode(mode.IDLE)
def _resolve_name(tool, want: str) -> str:
want = (want or "").strip()
if not want:
return ""
names = list(tool.list() or [])
if want in names:
return want
lower = {n.lower(): n for n in names}
hit = lower.get(want.lower())
if hit:
return hit
raise SystemExit(
"tool %r not found in Studio pool.\n available: %s"
% (want, ", ".join(names) or "(empty)")
)
def _iface_ipv4s(name: str) -> list:
"""FastDDS whitelist wants IPv4 addresses, not NIC names like enp4s0."""
import socket
import fcntl
import struct
name = (name or "").strip()
if not name:
return []
if name.count(".") == 3:
return [name]
addrs = []
try:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
s.close()
except OSError:
pass
try:
import subprocess
out = subprocess.check_output(
["ip", "-4", "-o", "addr", "show", "dev", name],
text=True,
stderr=subprocess.DEVNULL,
)
for line in out.splitlines():
parts = line.split()
if "inet" in parts:
cidr = parts[parts.index("inet") + 1]
addrs.append(cidr.split("/")[0])
except Exception:
pass
# unique, keep order
seen = set()
uniq = []
for a in addrs:
if a and a not in seen:
seen.add(a)
uniq.append(a)
return uniq
def _rdk_whitelist(iface: str) -> list[list[str]]:
"""RDK 手册要的是「连机器人的那张网卡」。enp4s0 上还有办公网 IP，不能整张卡的地址都塞进白名单。"""
import os
lists: list[list[str]] = []
pc = (os.environ.get("SITE_PC_IP") or "").strip()
if pc:
lists.append([pc])
if iface:
lists.append([iface])
robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")]
if robot_lan:
lists.append(robot_lan)
# unique
seen: set[tuple[str, ...]] = set()
out: list[list[str]] = []
for item in lists:
key = tuple(item)
if key not in seen:
seen.add(key)
out.append(item)
return out
def connect_rdk(sn: str, iface: str = ""):
import flexivrdk
for listen in _rdk_whitelist(iface):
print("RDK whitelist:", ", ".join(listen))
for args in ((sn, listen), (sn, listen, True), (sn, listen, True, False)):
try:
return flexivrdk.Robot(*args)
except TypeError:
continue
except RuntimeError as exc:
msg = str(exc)
if "whitelist" in msg.lower() or "filtered out" in msg.lower():
print(" whitelist rejected, try next")
break
if "Failed to connect" in msg or "failed to connect" in msg.lower():
print(" search timeout with this whitelist")
break
raise
print("retry without iface filter")
return flexivrdk.Robot(sn)
def main() -> int:
ap = argparse.ArgumentParser(description="Switch Flexiv Studio tool / TCP")
ap.add_argument("--sn", default="", help="robot serial (default: site.yaml)")
ap.add_argument("--iface", default="", help="NIC whitelist for RDK (e.g. enp4s0)")
ap.add_argument(
"--name",
default="",
help="Studio tool name (Flange = no tool). Empty = site.yaml robot.tool_name",
)
ap.add_argument("--list", action="store_true", help="list tools and exit")
args = ap.parse_args()
sn = (args.sn or "").strip()
want = (args.name or "").strip()
if not sn:
print("ERROR: --sn is required (run via run_real.sh / rdk_switch_tool.sh)", file=sys.stderr)
return 2
import flexivrdk
mode = flexivrdk.Mode
iface = (args.iface or "").strip()
print(f"=== RDK tool SN={sn} iface={iface or 'auto'} ===")
robot = None
try:
robot = connect_rdk(sn, iface)
if robot.fault():
print("fault present, ClearFault ...")
if not robot.ClearFault():
print("ERROR: fault cannot be cleared", file=sys.stderr)
return 1
tool = flexivrdk.Tool(robot)
names = list(tool.list() or [])
groups = _single_arm_groups(robot)
print("Studio tools:", ", ".join(names) or "(none)")
for g in groups:
cur = _tool_name(tool, g)
extra = ""
try:
extra = " " + _fmt_params(_tool_params(tool, g))
except Exception:
pass
print(f" active [{_group_name(g)}]: {cur}{extra}")
if args.list:
return 0
if not want:
print("tool_name empty — keep current (set robot.tool_name in site.yaml to switch)")
print("RDK OK")
return 0
target = _resolve_name(tool, want)
already = all(_tool_name(tool, g) == target for g in groups)
if already:
print(f"already on [{target}]")
print("RDK OK")
return 0
print(f"switching → [{target}] (requires IDLE)")
_ensure_idle(robot, mode)
for g in groups:
_tool_switch(tool, g, target)
for g in groups:
cur = _tool_name(tool, g)
extra = ""
try:
extra = " " + _fmt_params(_tool_params(tool, g))
except Exception:
pass
print(f" now [{_group_name(g)}]: {cur}{extra}")
if cur != target:
print(f"ERROR: switch failed, still {cur!r}", file=sys.stderr)
return 1
print("RDK OK")
return 0
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(" Stop run_real.sh first (RDK is exclusive).", file=sys.stderr)
print(" Pendant: AUTO Remote + Enable Ethernet/RDK.", file=sys.stderr)
return 1
finally:
robot = None
if __name__ == "__main__":
raise SystemExit(main())
scripts/rdk_switch_tool.sh
Executable
+42
View File
@@ -0,0 +1,42 @@
#!/usr/bin/env bash
# Switch / list Flexiv Studio tools via RDK (reads name from site.yaml).
#
# bash ~/ros2/scripts/rdk_switch_tool.sh # switch to robot.tool_name
# bash ~/ros2/scripts/rdk_switch_tool.sh --list # print Studio tool pool
# bash ~/ros2/scripts/rdk_switch_tool.sh --name Flange
#
# Stop run_real.sh first — RDK allows only one client.
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
SN="${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}"
NAME="${SITE_ROBOT_TOOL_NAME:-}"
IFACE="${SITE_NET_IFACE:-}"
LIST=false
while [[ $# -gt 0 ]]; do
case "$1" in
--list|-l) LIST=true; shift ;;
--name) NAME="$2"; shift 2 ;;
--sn) SN="$2"; shift 2 ;;
--iface) IFACE="$2"; shift 2 ;;
-h|--help) sed -n '2,12p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
ARGS=(--sn "${SN}")
if [[ -n "${IFACE}" ]]; then
ARGS+=(--iface "${IFACE}")
fi
if [[ "${LIST}" == true ]]; then
ARGS+=(--list)
elif [[ -n "${NAME}" ]]; then
ARGS+=(--name "${NAME}")
fi
exec "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${ARGS[@]}"
scripts/run_camera_d405.sh
Executable
+158
View File
@@ -0,0 +1,158 @@
#!/usr/bin/env bash
# Terminal: D405 via official pyrealsense2 SDK — color + depth + stereo IR topics.
# With --preview the Qt window runs in a SEPARATE viewer process (realsense_view)
# so painting never slows the camera stream. Closing the window ≠ stop camera.
# Do NOT start view_realsense_depth.sh at the same time (USB is exclusive).
#
# Usage:
# bash ~/ros2/scripts/run_camera_d405.sh
# bash ~/ros2/scripts/run_camera_d405.sh --preview
# bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=-1 # -1 = use site; 0/1 = force
HIGH_FPS=0
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
while [[ $# -gt 0 ]]; do
case "$1" in
--preview) PREVIEW=1; shift ;;
--no-preview) PREVIEW=0; shift ;;
--30fps) HIGH_FPS=1; shift ;;
-h|--help) sed -n '2,14p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
pkill -f 'lib/arm_application/realsense_color_pub' 2>/dev/null || true
sleep 2
USB_INFO="$(python3 - <<PY 2>/dev/null || true
import pyrealsense2 as rs
want_sn = "${SITE_CAM_SERIAL:-}"
ds = list(rs.context().devices)
if not ds:
print("")
raise SystemExit
pick = None
for d in ds:
sn = d.get_info(rs.camera_info.serial_number)
if want_sn and sn == want_sn:
pick = d
break
if pick is None:
for d in ds:
name = (d.get_info(rs.camera_info.name) or "").lower()
if "405" in name:
pick = d
break
pick = pick or ds[0]
usb = pick.get_info(rs.camera_info.usb_type_descriptor)
sn = pick.get_info(rs.camera_info.serial_number)
name = pick.get_info(rs.camera_info.name)
print(f"{usb}|{sn}|{name}")
PY
)"
USB_TYPE="${USB_INFO%%|*}"
echo "=== RealSense USB=${USB_TYPE:-unknown} ${USB_INFO} ==="
echo "=== site: ${SITE_CONFIG:-?} weights=${SITE_YOLO_WEIGHTS:-?} ==="
if [[ "${USB_TYPE}" == 2* ]]; then
CAM_W=640; CAM_H=480; CAM_FPS=15
echo "=== camera 640x480@15 (USB2: depth+IR still ON) ==="
echo "=== WARN: D405 is on USB2. Plug it into a USB3 (blue) port for 848x480@30 ==="
elif [[ "${SITE_CAM_USE_SITE_RES:-1}" == "1" ]]; then
# site.yaml camera.width/height/fps 优先；--30fps 只抬帧率，不再写死 848x480
CAM_W="${SITE_CAM_WIDTH:-848}"
CAM_H="${SITE_CAM_HEIGHT:-480}"
CAM_FPS="${SITE_CAM_FPS:-30}"
if [[ "${HIGH_FPS}" -eq 1 && "${CAM_FPS}" -lt 30 ]]; then
CAM_FPS=30
fi
echo "=== camera ${CAM_W}x${CAM_H}@${CAM_FPS} serial=${SITE_CAM_SERIAL:-} arm=${SITE_ROBOT_SN:-} ==="
elif [[ "${HIGH_FPS}" -eq 1 ]]; then
CAM_W=848; CAM_H=480; CAM_FPS=30
echo "=== camera 848x480@30 (--30fps, site use_site_resolution=false) ==="
else
CAM_W=1280; CAM_H=720; CAM_FPS=15
echo "=== camera 1280x720@15 ==="
fi
CAM_NAME="${SITE_CAM_NAME:-d405}"
CAM_ARGS=(
--ros-args
-p camera_name:="${CAM_NAME}"
-p width:="${CAM_W}"
-p height:="${CAM_H}"
-p fps:="${CAM_FPS}"
)
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
CAM_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
fi
CAM_ARGS+=(
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-4000}"
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
-p laser_power:="${SITE_CAM_LASER_POWER:-0}"
)
if [[ -n "${SITE_CAM_EXPOSURE_US:-}" && "${SITE_CAM_EXPOSURE_US}" != "0" && "${SITE_CAM_EXPOSURE_US}" != "0.0" ]]; then
CAM_ARGS+=(-p exposure_us:="${SITE_CAM_EXPOSURE_US}")
fi
if [[ -n "${SITE_CAM_GAIN:-}" && "${SITE_CAM_GAIN}" != "0" && "${SITE_CAM_GAIN}" != "0.0" ]]; then
CAM_ARGS+=(-p gain:="${SITE_CAM_GAIN}")
fi
if [[ "${SITE_CAM_ENABLE_DEPTH:-1}" == "1" ]]; then
CAM_ARGS+=(-p enable_depth:=true)
echo "=== depth ON ==="
else
CAM_ARGS+=(-p enable_depth:=false)
echo "=== depth OFF (site.yaml) ==="
fi
DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
if [[ "${PREVIEW}" -ge 0 ]]; then
DO_PREVIEW="${PREVIEW}"
fi
if [[ "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
CAM_ARGS+=(-p enable_infra:=true)
else
CAM_ARGS+=(-p enable_infra:=false)
fi
if [[ "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
CAM_ARGS+=(-p preview_stereo:=true)
else
CAM_ARGS+=(-p preview_stereo:=false)
fi
if [[ "${DO_PREVIEW}" == "1" ]]; then
echo "=== SDK preview ON (separate viewer process, hold ${SITE_CAM_OVERLAY_HOLD:-0.1}s) ==="
echo "=== close window = hide preview only; camera keeps publishing ==="
fi
echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ==="
if [[ "${DO_PREVIEW}" == "1" ]]; then
# Camera publishes only; the Qt viewer runs in its own process so painting
# can never starve the capture loop (shared GIL cost ~70 ms/frame before).
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
CAM_PID=$!
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
ros2 run arm_application realsense_view --ros-args \
-p camera_name:="${CAM_NAME}" \
-p overlay_hold_sec:="${SITE_CAM_OVERLAY_HOLD:-0.1}" \
-p hud_label:="${SITE_ROBOT_LABEL:-} ${SITE_ROBOT_SN:-} ${SITE_CAM_SERIAL:-}"
else
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi
scripts/run_camera_d435.sh
Executable
+50
View File
@@ -0,0 +1,50 @@
#!/usr/bin/env bash
# Overhead D435 between the two arms (eye-to-hand). Does not open either D405.
#
# bash ~/ros2/scripts/run_camera_d435.sh --preview
#
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=1
while [[ $# -gt 0 ]]; do
case "$1" in
--preview) PREVIEW=1; shift ;;
--no-preview) PREVIEW=0; shift ;;
-h|--help) sed -n '2,8p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
SN="${SITE_D435_SERIAL:-}"
if [[ -z "${SN}" ]]; then
echo "ERROR: cameras.d435.serial empty in site.yaml" >&2
exit 2
fi
echo "=== D435 overhead serial=${SN} ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
pkill -f 'camera_name:=d435' 2>/dev/null || true
CAM_ARGS=(
--ros-args
-p camera_name:=d435
-p camera_serial:="${SN}"
-p width:="${SITE_D435_WIDTH:-1280}"
-p height:="${SITE_D435_HEIGHT:-720}"
-p fps:="${SITE_D435_FPS:-30}"
-p enable_depth:=true
-p enable_infra:=false
-p preview_stereo:=false
-p auto_exposure:=true
-p ae_limit_us:=8000
-p emitter_enabled:=true
)
if [[ "${PREVIEW}" == "1" ]]; then
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
CAM_PID=$!
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
ros2 run arm_application realsense_view --ros-args -p camera_name:=d435
else
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi
scripts/run_display.sh
Executable
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env bash
# View URDF only (joint_state_publisher_gui + RViz)
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
ros2 launch arm_description display.launch.py
scripts/run_dual_gripper.sh
Executable
+25
View File
@@ -0,0 +1,25 @@
#!/usr/bin/env bash
# Both DH grippers (left PGEA + right RGI) as one ROS 2 node.
# bash ~/ros2/scripts/run_dual_gripper.sh
# Topics: /dual_gripper_driver/left|right/{command,status}
# Services: /dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute)
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
sleep 0.3
echo "=== dual gripper PGEA ${SITE_GRIP_PORT:-/dev/ttyUSB0} + RGI (site.yaml robots[]) ==="
exec ros2 launch arm_application dual_gripper.launch.py sides:=left,right allow_offline:=true
scripts/run_gripper.sh
Executable
+131
View File
@@ -0,0 +1,131 @@
#!/usr/bin/env bash
# DH grippers on the IPC (Modbus RTU). Not Flexiv Studio / RDK.
#
# Left 89 PGEA parallel /dev/ttyUSB0
# Right 91 RGI rotary /dev/ttyUSB1
#
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli close
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli pos 500
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS node PGEA
# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS node RGI
# bash ~/ros2/scripts/run_dual_gripper.sh # both ROS nodes
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CLI=()
DUAL=0
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
while [[ $# -gt 0 ]]; do
case "$1" in
--cli) shift; CLI=("$@"); break ;;
--dual) DUAL=1; shift ;;
-h|--help) sed -n '2,24p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
BACKEND="${SITE_GRIP_BACKEND:-usb}"
MODEL="${SITE_GRIP_MODEL:-PGEA-100-20-W}"
ARM="${SITE_ROBOT_ARM:-}"
IS_ROTARY=0
if [[ "${MODEL}" == *RGI* || "${ARM}" == "right" ]]; then
IS_ROTARY=1
fi
if [[ "${BACKEND}" == "rdk" || "${BACKEND}" == "flexiv" ]]; then
echo "ERROR: gripper.backend=${BACKEND} is Flexiv Studio. Use usb (IPC Modbus)." >&2
exit 1
fi
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
if [[ ${#CLI[@]} -gt 0 ]]; then
if [[ "${IS_ROTARY}" -eq 1 ]]; then
CMD="${CLI[0]}"
REST=("${CLI[@]:1}")
ARGS=(--arm right --test "${CMD}")
case "${CMD}" in
probe|status) ARGS=(--arm right --test probe) ;;
pos|move)
ARGS=(--arm right --test pos)
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--pos "${REST[0]}"); fi
;;
rotate|rotate_to)
ARGS=(--arm right --test rotate)
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
;;
revolutions)
ARGS=(--arm right --test rotate)
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
;;
esac
echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=91 ==="
exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
fi
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
BAUD="${SITE_GRIP_BAUD:-115200}"
SLAVE="${SITE_GRIP_SLAVE:-1}"
echo "=== DH ${MODEL} SN=${SITE_GRIP_SN:-?} ${PORT} @${BAUD} slave=${SLAVE} ==="
exec python3 "${ROOT}/src/arm_application/arm_application/dh_pgea.py" \
--port "${PORT}" --baud "${BAUD}" --slave "${SLAVE}" \
--force "${SITE_GRIP_FORCE:-50}" --speed "${SITE_GRIP_SPEED:-50}" \
--rts "${SITE_GRIP_RS485_RTS:-}" \
"${CLI[@]}"
fi
if [[ "${DUAL}" -eq 1 ]]; then
exec bash "${ROOT}/scripts/run_dual_gripper.sh"
fi
if [[ "${IS_ROTARY}" -eq 1 ]]; then
pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
sleep 0.3
echo "=== DH RGI ROS node ${SITE_GRIP_PORT:-/dev/ttyUSB1} (arm_msgs dual_gripper right) ==="
exec ros2 launch arm_application dual_gripper.launch.py sides:=right allow_offline:=true
fi
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
BAUD="${SITE_GRIP_BAUD:-115200}"
SLAVE="${SITE_GRIP_SLAVE:-1}"
GSN="${SITE_GRIP_SN:-}"
PY="${ROOT}/src/arm_application/arm_application/dh_pgea.py"
NODE_PY="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"
if [[ ! -e "${PORT}" ]]; then
echo "ERROR: ${PORT} missing. Plug USB–RS485 into the IPC." >&2
exit 1
fi
pkill -f 'arm_application/dh_pgea_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dh_pgea' 2>/dev/null || true
sleep 0.4
echo "=== DH ${MODEL} SN=${GSN:-?} ${PORT} @${BAUD} ROS node ==="
exec python3 "${NODE_PY}" --ros-args \
-p port:="${PORT}" \
-p baud:="${BAUD}" \
-p slave_id:="${SLAVE}" \
-p stroke_mm:="${SITE_GRIP_STROKE_MM:-20.0}" \
-p force_pct:="${SITE_GRIP_FORCE:-50}" \
-p speed_pct:="${SITE_GRIP_SPEED:-50}" \
-p auto_init:="${SITE_GRIP_AUTO_INIT:-true}" \
-p model:="${MODEL}" \
-p "serial_number:='${GSN}'" \
-p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'"
scripts/run_handeye.sh
Executable
+254
View File
@@ -0,0 +1,254 @@
#!/usr/bin/env bash
# Hand-eye calibration helpers.
# Scripts: ~/ros2/scripts/run_handeye.sh
# Package YAML: ~/ros2/src/arm_application/config/handeye_*.yaml
# Site overrides (board / motion): ~/ros2/config/site.yaml → handeye:
# Runtime output: /tmp/handeye_calib/
#
# Usage:
# bash ~/ros2/scripts/run_handeye.sh calibrate
# bash ~/ros2/scripts/run_handeye.sh waypoints
# bash ~/ros2/scripts/run_handeye.sh teach # RDK 软拖拽（推荐）
# bash ~/ros2/scripts/run_handeye.sh teach --append # 接着上次点位继续采
# bash ~/ros2/scripts/run_handeye.sh teach --ros # 示教器 Free Drive
# bash ~/ros2/scripts/run_handeye.sh merge [taught.txt]
# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
# bash ~/ros2/scripts/run_handeye.sh --arm 91 calibrate
#
# calibrate / waypoints 需要:
# T1: bash ~/ros2/scripts/run_real.sh --arm 89|91
# T2: bash ~/ros2/scripts/run_camera_d405.sh --arm 89|91 --preview
# teach（拖拽）需要: 先停掉 run_real.sh 和相机（RDK + D405 都独占）
# 89 与 91 的结果文件、示教点、workdir 完全分开，禁止混用。
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
CMD="${1:-}"
shift || true
usage() {
sed -n '2,18p' "$0"
}
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# Prefer package source tree (editable); fall back to install share
APP_CFG="${ROOT}/src/arm_application/config"
if [[ ! -d "${APP_CFG}" ]]; then
APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true
fi
WP="${SITE_HANDEYE_WAYPOINTS:-${APP_CFG}/handeye_poses.yaml}"
CAL="${SITE_HANDEYE_CALIBRATE:-${APP_CFG}/handeye_calibrate.yaml}"
TOUR="${SITE_HANDEYE_TOUR:-${APP_CFG}/handeye_waypoints.yaml}"
RES="${SITE_HANDEYE_FILE:-${APP_CFG}/handeye_result.yaml}"
WORKDIR="${SITE_HANDEYE_WORKDIR:-/tmp/handeye_calib}"
TAUGHT="${WORKDIR}/taught_poses.txt"
PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}"
CAM="${SITE_CAM_NAME:-d405}"
_kill_servo() {
pkill -f 'visual_servo.launch' 2>/dev/null || true
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
sleep 1
}
_need_files() {
local f
for f in "$@"; do
if [[ ! -f "$f" ]]; then
echo "ERROR: missing $f" >&2
exit 2
fi
done
}
_need_robot() {
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
echo "ERROR: start run_real.sh first" >&2
exit 1
fi
}
_need_camera() {
if ! ros2 topic list 2>/dev/null | grep -q "/${CAM}/color/image_raw"; then
echo "ERROR: no /${CAM}/color/image_raw — start run_camera_d405.sh" >&2
exit 1
fi
}
cmd_show() {
echo "=== hand-eye (package + site) ==="
echo " arm : ${SITE_ROBOT_SN:-} ${SITE_ROBOT_LABEL:-}"
echo " package cfg : ${APP_CFG}"
echo " calibrate : ${CAL}"
echo " poses : ${WP}"
echo " tour : ${TOUR}"
echo " result : ${RES}"
echo " workdir : ${WORKDIR}"
echo " board : ${SITE_HANDEYE_BOARD_COLS:-?}x${SITE_HANDEYE_BOARD_ROWS:-?} inner, square=${SITE_HANDEYE_SQUARE_M:-?} m"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
if [[ -f "${WP}" ]]; then
python3 - <<PY
import re
from pathlib import Path
t = Path("${WP}").read_text(encoding="utf-8")
m = re.search(r"num_poses:\s*(\d+)", t)
n = int(m.group(1)) if m else "?"
print(f" num_poses in poses yaml: {n}")
PY
fi
}
cmd_merge() {
local src="${1:-${TAUGHT}}"
_need_files "${src}" "${WP}"
python3 "${ROOT}/scripts/merge_handeye_poses.py" "${src}" "${WP}"
echo "=== next: bash ~/ros2/scripts/run_handeye.sh calibrate ==="
}
cmd_calibrate() {
_need_files "${CAL}" "${WP}"
_need_robot
_need_camera
echo "=== kill visual_servo (avoid traj fight) ==="
_kill_servo
mkdir -p "${WORKDIR}"
local cols="${SITE_HANDEYE_BOARD_COLS:-11}"
local rows="${SITE_HANDEYE_BOARD_ROWS:-16}"
local sq="${SITE_HANDEYE_SQUARE_M:-0.011}"
local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"
local qd="${SITE_HANDEYE_SETTLE_QD:-0.015}"
local ss="${SITE_HANDEYE_SETTLE_STABLE:-0.6}"
local sto="${SITE_HANDEYE_SETTLE_TO:-8.0}"
local ph="${SITE_HANDEYE_PIN_HOLDS:-2}"
local ps="${SITE_HANDEYE_PIN_SEC:-0.45}"
local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}"
echo "=== calibrate mode=${SITE_HANDEYE_MODE:-eye_in_hand} ==="
echo " board ${cols}x${rows} inner, square=${sq} m (from site.yaml)"
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
echo " poses=${WP}"
echo " result → ${RES}"
exec ros2 run arm_application handeye_calibrate --ros-args \
--params-file "${CAL}" \
--params-file "${WP}" \
-p joint_prefix:="${PREFIX}" \
-p camera_source:="${CAM}" \
-p output_dir:="${WORKDIR}" \
-p result_file:="${RES}" \
-p mode:="${SITE_HANDEYE_MODE:-eye_in_hand}" \
-p board_cols:="${cols}" \
-p board_rows:="${rows}" \
-p square_size_m:="${sq}" \
-p velocity_scale:="${vel}" \
-p move_time_sec:="${mt}" \
-p settle_sec:="${st}" \
-p settle_qd_max:="${qd}" \
-p settle_stable_sec:="${ss}" \
-p settle_timeout_sec:="${sto}" \
-p pin_holds:="${ph}" \
-p pin_hold_sec:="${ps}" \
-p skip_unstable_poses:="${skip}"
}
cmd_waypoints() {
_need_files "${TOUR}" "${WP}"
_need_robot
_need_camera
echo "=== kill visual_servo ==="
_kill_servo
mkdir -p "${WORKDIR}"
echo "=== waypoint tour (Enter=confirm, q=quit) ==="
echo " saves confirmed joints → ${TAUGHT}"
echo " then: bash ~/ros2/scripts/run_handeye.sh merge"
exec ros2 run arm_application handeye_waypoints --ros-args \
--params-file "${TOUR}" \
--params-file "${WP}" \
-p joint_prefix:="${PREFIX}" \
-p camera_source:="${CAM}" \
-p append_poses_file:="${TAUGHT}"
}
cmd_teach() {
local backend="rdk"
local append=false
while [[ $# -gt 0 ]]; do
case "$1" in
--rdk) backend="rdk"; shift ;;
--ros|--pendant) backend="ros"; shift ;;
--append|--keep) append=true; shift ;;
*) echo "Unknown teach arg: $1" >&2; exit 1 ;;
esac
done
echo "=== kill visual_servo + camera (D405 exclusive) ==="
_kill_servo
pkill -f 'run_camera_d405' 2>/dev/null || true
pkill -f 'realsense_color_pub' 2>/dev/null || true
pkill -f 'realsense2_camera_node' 2>/dev/null || true
sleep 1
if [[ "${backend}" == "rdk" ]]; then
if ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
echo "ERROR: RDK 拖拽与 run_real.sh 互斥。请先在那个终端 Ctrl-C 停掉 run_real.sh" >&2
exit 1
fi
else
_need_robot
fi
mkdir -p "${WORKDIR}/images"
if [[ "${append}" == true ]]; then
echo "=== append: keep existing ${TAUGHT} ==="
else
# 重新设点：丢掉上次 taught，避免 merge 混进旧位姿
: > "${TAUGHT}"
fi
echo "=== interactive teach backend=${backend} → ${WORKDIR} ==="
echo " 窗口按键: d=拖拽开 s=拖拽关 Enter=拍照记点 q=结束"
echo " 建议 8–12 个姿态，棋盘占画面约 30–70%"
echo " 结束后: bash ~/ros2/scripts/run_handeye.sh merge"
echo " bash ~/ros2/scripts/run_handeye.sh calibrate"
TEACH_ARGS=(
-p backend:="${backend}"
-p robot_sn:="${SITE_ROBOT_SN:-Rizon4-063989}"
-p joint_prefix:="${PREFIX}"
-p camera_source:="${CAM}"
-p output_dir:="${WORKDIR}"
)
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
fi
if [[ -n "${SITE_CAM_WIDTH:-}" ]]; then
TEACH_ARGS+=(-p width:="${SITE_CAM_WIDTH}" -p height:="${SITE_CAM_HEIGHT:-480}" -p fps:="${SITE_CAM_FPS:-30}")
fi
TEACH_ARGS+=(
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-1800}"
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
-p exposure_us:="${SITE_CAM_EXPOSURE_US:-0}"
-p gain:="${SITE_CAM_GAIN:-0}"
)
exec ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}"
}
case "${CMD}" in
calibrate|calib) cmd_calibrate ;;
waypoints|tour) cmd_waypoints "$@" ;;
teach) cmd_teach "$@" ;;
merge) cmd_merge "$@" ;;
show) cmd_show ;;
-h|--help|help|"") usage; [[ -n "${CMD}" ]] || exit 0; exit 0 ;;
*) echo "Unknown: ${CMD}" >&2; usage >&2; exit 1 ;;
esac
scripts/run_hole_detect.sh
Executable
+42
View File
@@ -0,0 +1,42 @@
#!/usr/bin/env bash
# Hole detection on the current-arm wrist D405. Detect only — no insert motion.
#
# bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
#
# backend from site.yaml hole_detect.backend:
# two_stage YOLO workpiece bbox + HoughCircles (camera_test hole_detect.py)
# yolo YOLO boxes treated as holes
# hough full-image HoughCircles
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=false
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
while [[ $# -gt 0 ]]; do
case "$1" in
--preview) PREVIEW=true; shift ;;
-h|--help) sed -n '2,16p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
pkill -f 'arm_application/hole_detect_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/hole_detect' 2>/dev/null || true
sleep 0.2
BACKEND="${SITE_HOLE_BACKEND:-two_stage}"
echo "=== hole_detect arm=${SITE_ROBOT_ARM:-?} cam=${SITE_CAM_NAME} backend=${BACKEND} preview=${PREVIEW} ==="
exec ros2 launch arm_application hole_detect.launch.py \
backend:="${BACKEND}" \
preview:="${PREVIEW}"
scripts/run_mock_demo.sh
Executable
+11
View File
@@ -0,0 +1,11 @@
#!/usr/bin/env bash
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
source /home/cc/anaconda3/etc/profile.d/conda.sh
conda activate ros_flexiv
source "${ROOT}/install/setup.bash"
ros2 run arm_application demo_named_poses --ros-args \
-p "poses:=['home','ready','inspect','pos_left','home']" \
-p velocity_scale:=2.0 \
-p dwell_sec:=0.2 \
-p loops:=1
scripts/run_pipeline.sh
Executable
+3
View File
@@ -0,0 +1,3 @@
#!/usr/bin/env bash
# 三阶段串联的别名，等价于 run_all.sh。
exec "$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/run_all.sh" "$@"
scripts/run_real.sh
Executable
+96
View File
@@ -0,0 +1,96 @@
#!/usr/bin/env bash
# Real Rizon4 bringup — sets RDK runtime libs + launches stack backend:=real
# Note: no `set -u` — conda/RoboStack hooks reference unset vars (CONDA_BUILD, COLCON_TRACE)
#
# bash ~/ros2/scripts/run_real.sh
# bash ~/ros2/scripts/run_real.sh --arm 91
# bash ~/ros2/scripts/run_real.sh --sn Rizon4-063991 # 本次启动换臂（不改 yaml）
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
FLEXIV_PREFIX="${HOME}/flexiv_install"
LIST_ONLY=false
while [[ $# -gt 0 ]]; do
case "$1" in
--sn|--arm) export ROBOT_SN="${2:?}"; export ROBOT_ARM="${2}"; shift 2 ;;
--list|-l) LIST_ONLY=true; shift ;;
-h|--help)
sed -n '2,8p' "$0"
echo " Extra args are passed to ros2 launch (e.g. use_rviz:=false)."
exit 0
;;
*) break ;;
esac
done
source /home/cc/anaconda3/etc/profile.d/conda.sh
conda activate ros_flexiv
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# flexiv_hardware must load spdlog/RBDyn from flexiv_install (not conda fmt v11)
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
# SITE_ROBOT_SN is the resolved Flexiv serial (89/91 aliases already mapped).
ROBOT_SN="${SITE_ROBOT_SN:-${ROBOT_SN:-Rizon4-063989}}"
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
NET_IFACE="${SITE_NET_IFACE:-}"
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
if [[ "${LIST_ONLY}" == true ]]; then
python3 "${ROOT}/scripts/lib/export_site_env.py" --print
exit 0
fi
echo "=== site: ${SITE_CONFIG:-n/a} SN=${ROBOT_SN} ${SITE_ROBOT_LABEL:-} arm=${SITE_ROBOT_ARM:-} ==="
echo "=== catalog: ${SITE_ROBOT_CATALOG:-} ==="
echo "=== handeye: ${SITE_HANDEYE_FILE:-} ==="
echo "=== camera serial: ${SITE_CAM_SERIAL:-} gripper: ${SITE_GRIP_MODEL:-} ${SITE_GRIP_PORT:-} ==="
echo "=== network iface=${NET_IFACE:-auto} robot=${ROBOT_IP} pc=${SITE_PC_IP:-} ==="
echo "=== stop leftover ros2_control (RDK exclusive) ==="
pkill -f '/controller_manager/ros2_control_node' 2>/dev/null || true
pkill -f 'arm_bringup.*stack.launch' 2>/dev/null || true
sleep 1
# 清掉把 91 的 .100 误绑到 89 MAC 上的陈旧 ARP
if [[ -n "${NET_IFACE}" ]]; then
ip neigh del "${ROBOT_IP}" dev "${NET_IFACE}" 2>/dev/null || true
fi
if ping -c 2 -W 1 "${ROBOT_IP}" >/dev/null 2>&1; then
echo "robot ${ROBOT_IP} ping OK"
else
echo "WARN: cannot ping ${ROBOT_IP} — 这一台不在网里，RDK 大概率会失败。"
echo " 本机: $(ip -4 -o addr show dev "${NET_IFACE:-enp4s0}" 2>/dev/null | awk '{print $4}' | tr '\n' ' ')"
echo " ARP ${ROBOT_IP}: $(ip neigh show "${ROBOT_IP}" 2>/dev/null | head -1 || echo none)"
MAC91="$(ip neigh show "${ROBOT_IP}" 2>/dev/null | awk '{print $5}' | head -1)"
MAC89="$(ip neigh show 192.168.2.101 2>/dev/null | awk '{print $5}' | head -1)"
if [[ -n "${MAC91}" && -n "${MAC89}" && "${MAC91}" == "${MAC89}" ]]; then
echo " ARP MAC 与 89(192.168.2.101) 相同 → .100 是误绑，91 并没有回包。"
fi
echo " 91 请接控制柜 User Port 1（出厂静态 192.168.2.100）到同一交换机，示教器 AUTO+Remote Ethernet。"
echo " 89=192.168.2.101 91=192.168.2.100"
fi
echo "=== RDK preflight + tool (${ROBOT_SN}) ==="
TOOL_NAME="${SITE_ROBOT_TOOL_NAME:-}"
RDK_TOOL_ARGS=(--sn "${ROBOT_SN}")
if [[ -n "${NET_IFACE}" ]]; then
RDK_TOOL_ARGS+=(--iface "${NET_IFACE}")
fi
if [[ -n "${TOOL_NAME}" ]]; then
RDK_TOOL_ARGS+=(--name "${TOOL_NAME}")
echo "=== Studio tool → ${TOOL_NAME} ==="
fi
if ! "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${RDK_TOOL_ARGS[@]}"; then
echo ""
echo "ERROR: RDK cannot connect / switch tool on ${ROBOT_SN}."
echo " 先确认 ping ${ROBOT_IP} 通（91=控制柜 User Port 1 → 192.168.2.100）。"
echo " Stop any other RDK client (this run_real.sh is exclusive)."
echo " On teach pendant: Settings → Remote → enable Ethernet/RDK remote mode."
echo " Test: bash ${ROOT}/scripts/test_rdk_python.sh ${ROBOT_SN}"
exit 1
fi
exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" "$@"
scripts/run_sim.sh
Executable
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env bash
# Quick-start: mock arm + RViz
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
ros2 launch arm_bringup sim.launch.py
scripts/run_stage2_locate.sh
Executable
+136
View File
@@ -0,0 +1,136 @@
#!/usr/bin/env bash
# 二阶段孔定位（一次性拍照，不做实时推理）：垂直朝下 → 悬停锁高 → 拍照 →
# 解 k1/k2 孔基座坐标 → 夹爪/通止规轴线对准目标孔 → 锁定保持。
#
# 前置（各占一个终端）：
# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 控制器（二阶段先只做左臂）
# T3 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了传感器）。
#
# 用法：
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整定位+对准 k1
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2 # 对准 k2
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只垂直+悬停锁定（调高度）
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高（不改 yaml）
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --no-tactile # 不起触觉服务
#
# 悬停锁高固定接口：config/site.yaml → robots[89].servo.hole_hover_z_m
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
HOVER_ONLY=false
HOLE=""
HOVER_Z=""
WITH_TACTILE=true
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
while [[ $# -gt 0 ]]; do
case "$1" in
--hover-only) HOVER_ONLY=true; shift ;;
--hole) HOLE="${2:?}"; shift 2 ;;
--hover-z) HOVER_Z="${2:?}"; shift 2 ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,22p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
fi
echo "=== stage2 locate arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
# ---- 1) 机械臂控制器必须在（和 run_visual_servo_rt.sh 同一套判活） ----
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
ready=false
for _i in $(seq 1 30); do
if timeout 5 ros2 service call /controller_manager/list_controllers \
controller_manager_msgs/srv/ListControllers 2>/dev/null \
| grep -q "name='rizon_arm_controller', state='active'"; then
ready=true
break
fi
sleep 0.5
done
if [[ "${ready}" != true ]]; then
echo "ERROR: 机械臂控制器未激活。先跑: bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
exit 1
fi
# ---- 2) 相机话题必须在发（二阶段拍照用；--hover-only 纯调高度，不需要相机） ----
CAM_NS="${SITE_CAM_NAME:-d405_left}"
IMG_TOPIC="/${CAM_NS}/color/image_raw"
if [[ "${HOVER_ONLY}" == true ]]; then
echo "=== hover-only：跳过相机/触觉检查，直接垂直+悬停 ==="
else
echo "=== check camera topic ${IMG_TOPIC} ==="
cam_ok=false
for _i in $(seq 1 24); do # 最多等 ~12s，相机可能正在起
# 以真实消息为准（topic info 的 Publisher count 依赖 DDS 发现，进程频繁重启后会假阴性）
if timeout 3 ros2 topic echo --once "${IMG_TOPIC}" 2>/dev/null | grep -q "^header:"; then
cam_ok=true
break
fi
sleep 0.5
done
if [[ "${cam_ok}" != true ]]; then
echo "ERROR: ${IMG_TOPIC} 没有发布者。先开相机：" >&2
echo " bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview" >&2
exit 1
fi
echo "camera ${CAM_NS} publishing OK"
fi
# ---- 3) 触觉传感器服务（目前只有左臂 89 装了传感器；hover-only 不需要） ----
if [[ "${WITH_TACTILE}" == true && "${HOVER_ONLY}" != true ]]; then
if [[ "${SITE_ROBOT_ARM}" == "left" || "${SITE_ROBOT_SN}" == "Rizon4-063989" ]]; then
echo "=== start tactile sensor service (left arm) ==="
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker（占死 /dev/video*）
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
sleep 1.0
nohup ros2 launch arm_application tactile.launch.py > /tmp/stage2_tactile.log 2>&1 &
echo " tactile pid=$! log=/tmp/stage2_tactile.log"
t0=$SECONDS
while (( SECONDS - t0 < 30 )); do
if timeout 4 ros2 topic echo /tactile/left/status --once 2>/dev/null | grep -q "connected: true"; then
echo " /tactile/left/status publishing OK"
break
fi
sleep 0.5
done
if ! timeout 4 ros2 topic echo /tactile/left/status --once 2>/dev/null | grep -q "connected: true"; then
echo "WARN: 触觉话题还没起来（传感器没接？），定位流程继续。日志: /tmp/stage2_tactile.log" >&2
fi
else
echo "=== 触觉传感器只有左臂有；当前 ${SITE_ROBOT_ARM}，跳过（--no-tactile 可关此提示） ==="
fi
fi
# ---- 4) 清掉可能打架的旧进程（一阶段伺服/旧二阶段） ----
echo "=== stop old perception/servo/stage2 ==="
pkill -f 'lib/arm_application/stage2_locate' 2>/dev/null || true
pkill -f 'visual_servo.launch' 2>/dev/null || true
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
sleep 1
echo "=== 确认：左臂夹爪已夹持通止规，通止规轴线与法兰轴线一致 ==="
# ---- 5) 起二阶段定位 ----
ARGS=(hover_only:="${HOVER_ONLY}")
if [[ -n "${HOLE}" ]]; then ARGS+=(target_hole:="${HOLE}"); fi
if [[ -n "${HOVER_Z}" ]]; then ARGS+=(hover_z_m:="${HOVER_Z}"); fi
echo "=== launch stage2_locate hover_only=${HOVER_ONLY} hole=${HOLE:-site} hover_z=${HOVER_Z:-site} ==="
exec ros2 launch arm_application stage2_locate.launch.py "${ARGS[@]}"
scripts/run_stage3_search.sh
Executable
+152
View File
@@ -0,0 +1,152 @@
#!/usr/bin/env bash
# 三阶段柔顺搜孔：触面 → 力闭环螺旋搜孔 → 十字定心 → 力限插入/旋入 → 锁定。
# 前置（各占一个终端）：
# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 控制器
# —— 二阶段已完成（run_stage2_locate.sh --arm 89），臂保持悬停不动
# 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了 Orisys 传感器）。
#
# 用法：
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座（site 默认深度）
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 # 对 k2 孔
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入(M6=1.0)
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile # 不起触觉服务（调试用）
#
# 参数固定接口：config/site.yaml → hole_detect.stage3（力阈值/螺旋/插入全部可调）
# 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看 Orisys 读数量级
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
HOLE=""
DEPTH=""
PITCH=""
RMAX=""
RESNAP=false
WITH_TACTILE=true
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
while [[ $# -gt 0 ]]; do
case "$1" in
--hole) HOLE="${2:?}"; shift 2 ;;
--depth) DEPTH="${2:?}"; shift 2 ;;
--pitch) PITCH="${2:?}"; shift 2 ;;
--r-max) RMAX="${2:?}"; shift 2 ;;
--resnap) RESNAP=true; shift ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,24p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
fi
echo "=== stage3 search arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
# ---- 1) 机械臂控制器必须在 ----
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
ready=false
for _i in $(seq 1 30); do
if timeout 5 ros2 service call /controller_manager/list_controllers \
controller_manager_msgs/srv/ListControllers 2>/dev/null \
| grep -q "name='rizon_arm_controller', state='active'"; then
ready=true
break
fi
sleep 0.5
done
if [[ "${ready}" != true ]]; then
echo "ERROR: 机械臂控制器未激活。先跑: bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
exit 1
fi
# ---- 2) 触觉传感器服务（三阶段的核心传感器，必须在线） ----
if [[ "${WITH_TACTILE}" == true ]]; then
if [[ "${SITE_ROBOT_ARM}" == "left" || "${SITE_ROBOT_SN}" == "Rizon4-063989" ]]; then
echo "=== start tactile sensor service (left arm) ==="
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker（占死 /dev/video*）
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
sleep 1.0
nohup ros2 launch arm_application tactile.launch.py > /tmp/stage3_tactile.log 2>&1 &
echo " tactile pid=$! log=/tmp/stage3_tactile.log"
tac_ok=false
for _i in $(seq 1 15); do
# 必须看到真实消息（Publisher count 只反映 DDS 端点存在，不代表数据在流）
# 单侧可用即可（另一指掉线时降级为单指模式，stage3 节点内会告警）
l_live=false; r_live=false
timeout 4 ros2 topic echo /tactile/left/status --once 2>/dev/null | grep -q "connected: true" && l_live=true
timeout 4 ros2 topic echo /tactile/right/status --once 2>/dev/null | grep -q "connected: true" && r_live=true
if [[ "${l_live}" == true || "${r_live}" == true ]]; then
tac_ok=true
[[ "${l_live}" == true && "${r_live}" == true ]] || \
echo " WARN: 单指触觉（左=${l_live} 右=${r_live}）——另一指掉线，降级运行"
break
fi
sleep 0.5
done
if [[ "${tac_ok}" != true ]]; then
echo "ERROR: /tactile/left|right/status 双侧都没起来——三阶段必须有触觉。日志: /tmp/stage3_tactile.log" >&2
echo " 排查: bash ~/ros2/scripts/run_tactile.sh --cli；若报 Failed to open video source: pkill orisys_tac_w 后重试" >&2
exit 1
fi
echo " /tactile status publishing OK"
else
echo "ERROR: 触觉传感器只有左臂 89 有；当前 ${SITE_ROBOT_ARM} 不能跑三阶段" >&2
exit 1
fi
fi
# ---- 3) 二阶段结果参考（不强制，只提示） ----
if [[ -f /tmp/stage2_result.json ]]; then
age=$(( $(date +%s) - $(stat -c %Y /tmp/stage2_result.json) ))
echo "=== stage2 result: $(date -d @$(stat -c %Y /tmp/stage2_result.json) '+%H:%M:%S') (${age}s 前) ==="
if (( age > 900 )); then
echo "WARN: 二阶段结果已超过 15 分钟——工件若被动过请重跑二阶段" >&2
fi
else
echo "WARN: 没有 /tmp/stage2_result.json——确认二阶段已跑过且臂仍悬停在孔上方" >&2
fi
# ---- 4) 复拍模式需要相机 ----
if [[ "${RESNAP}" == true ]]; then
CAM_NS="${SITE_CAM_NAME:-d405_left}"
echo "=== resnap：check camera topic /${CAM_NS}/color/image_raw ==="
if ! timeout 5 ros2 topic info "/${CAM_NS}/color/image_raw" 2>/dev/null | grep -q "Publisher count: [1-9]"; then
echo "ERROR: 相机没开。先: bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview" >&2
exit 1
fi
fi
# ---- 5) 清掉可能打架的旧进程 ----
echo "=== stop old perception/servo/stage2/stage3 ==="
pkill -f 'lib/arm_application/stage3_search' 2>/dev/null || true
pkill -f 'lib/arm_application/stage2_locate' 2>/dev/null || true
pkill -f 'visual_servo.launch' 2>/dev/null || true
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
sleep 1
echo "=== 确认：臂仍悬停在目标孔上方（二阶段完成后没动过），通止规夹持牢固 ==="
# ---- 6) 起三阶段搜孔 ----
ARGS=(resnap:="${RESNAP}")
if [[ -n "${HOLE}" ]]; then ARGS+=(target_hole:="${HOLE}"); fi
if [[ -n "${DEPTH}" ]]; then ARGS+=(insert_depth_mm:="${DEPTH}"); fi
if [[ -n "${PITCH}" ]]; then ARGS+=(screw_pitch_mm:="${PITCH}"); fi
if [[ -n "${RMAX}" ]]; then ARGS+=(spiral_r_max_mm:="${RMAX}"); fi
echo "=== launch stage3_search hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} r_max=${RMAX:-site} resnap=${RESNAP} ==="
exec ros2 launch arm_application stage3_search.launch.py "${ARGS[@]}"
scripts/run_tactile.sh
Executable
+34
View File
@@ -0,0 +1,34 @@
#!/usr/bin/env bash
# Orisys visuo-tactile (finger cameras, not D405). Config: site.yaml tactile:
#
# bash ~/ros2/scripts/run_tactile.sh --cli
# bash ~/ros2/scripts/run_tactile.sh # ROS node /tactile/left|right/status
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CLI=0
while [[ $# -gt 0 ]]; do
case "$1" in
--cli) CLI=1; shift ;;
-h|--help) sed -n '2,10p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
echo "=== tactile backend=${SITE_TACTILE_BACKEND:-orisys} L=${SITE_TACTILE_LEFT:-0} R=${SITE_TACTILE_RIGHT:-2} cfg=${SITE_TACTILE_CONFIG:-dd03} ==="
if [[ "${CLI}" -eq 1 ]]; then
exec python3 -m arm_application.tactile_cli
fi
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker（占死 /dev/video*）
sleep 0.2
exec ros2 launch arm_application tactile.launch.py
scripts/run_visual_servo_rt.sh
Executable
+83
View File
@@ -0,0 +1,83 @@
#!/usr/bin/env bash
# Real-time visual servo (reads ALL tunables from ~/ros2/config/site.yaml).
#
# bash ~/ros2/scripts/run_visual_servo_rt.sh
# bash ~/ros2/scripts/run_visual_servo_rt.sh --control
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
#
# --control starts tracking from the current pose (no lift, no return-to-home).
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
#
# Switch YOLO weights: edit paths.yolo_weights in site.yaml
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CONTROL=false
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"
while [[ $# -gt 0 ]]; do
case "$1" in
--control) CONTROL=true; shift ;;
-h|--help) sed -n '2,12p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
echo "=== FastDDS: ${FASTRTPS_DEFAULT_PROFILES_FILE} ==="
fi
if [[ "${CONTROL}" == true ]]; then
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
# NOTE: action-list can show zombie entries from a crashed ros2_control,
# and `ros2 control` CLI is broken in this env (xmlrpc Fault). A raw
# service call only answers when the controller manager is actually alive.
ready=false
for _i in $(seq 1 40); do
if timeout 5 ros2 service call /controller_manager/list_controllers \
controller_manager_msgs/srv/ListControllers 2>/dev/null \
| grep -q "name='rizon_arm_controller', state='active'"; then
ready=true
break
fi
sleep 0.5
done
if [[ "${ready}" != true ]]; then
echo "ERROR: 机械臂控制器未激活（可能 T1 已崩溃或机器人不在网）。" >&2
echo " 先确认 ping 通机器人，再重启 T1 并等 operational：" >&2
echo " bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-91}" >&2
exit 1
fi
echo "=== stop old perception/servo ==="
pkill -f 'visual_servo.launch' 2>/dev/null || true
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
sleep 1
echo "=== start perception + servo CONTROL ON (lock_z from robots[].servo.ee_z_m) ==="
else
echo "=== launch perception + servo (observe only, no arm motion) ==="
fi
EE="${SITE_EE_FRAME:-Rizon4-063989_flange}"
HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}"
echo " weights=${SITE_YOLO_WEIGHTS} camera=${SITE_CAM_NAME:-} control=${CONTROL}"
LAUNCH_ARGS=(
enable_control:="${CONTROL}"
handeye_file:="${HANDEYE}"
ee_frame:="${EE}"
camera_frame:="${SITE_CAMERA_FRAME:-d405_color_optical_frame}"
)
exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"
scripts/show_site_config.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/usr/bin/env bash
# Show / validate the master site config.
# bash ~/ros2/scripts/show_site_config.sh
# SITE_CONFIG=/path/to/other.yaml bash ~/ros2/scripts/show_site_config.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print ${ROBOT_SN:+--sn "$ROBOT_SN"}
echo
echo "Edit: ${SITE_CONFIG:-$ROOT/config/site.yaml}"
echo "Switch weights example:"
echo " paths.yolo_weights: \"/home/cc/ros2/models/yolo/detect_hc90.pt\""
echo "Or symlink:"
echo " ln -sfn detect_hc90.pt ~/ros2/models/yolo/active.pt"
echo " # then set paths.yolo_weights to .../active.pt"
scripts/test_rdk_python.sh
Executable
+43
View File
@@ -0,0 +1,43 @@
#!/usr/bin/env bash
# Smoke-test Flexiv RDK Python against site robot (no ROS / no GitHub).
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
SN="${1:-${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}}"
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
IFACE="${SITE_NET_IFACE:-}"
PY="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
echo "Using: ${PY}"
echo "Robot SN: ${SN} IP: ${ROBOT_IP} iface: ${IFACE:-auto}"
echo "Ping ${ROBOT_IP} ..."
ping -c 1 -W 1 "${ROBOT_IP}" >/dev/null && echo " network OK" || echo " network FAIL"
"${PY}" - <<PY
import flexivrdk
sn = "${SN}"
iface = "${IFACE}"
print("Connecting...", sn, "iface", iface or "auto")
robot = None
if iface:
for args in ((sn, [iface]), (sn, [iface], True), (sn, [iface], True, False)):
try:
robot = flexivrdk.Robot(*args)
break
except TypeError:
continue
if robot is None:
robot = flexivrdk.Robot(sn)
print("Connected. Clearing fault if any...")
try:
robot.ClearFault()
except Exception as e:
print("ClearFault:", e)
mode = robot.mode()
print("mode:", mode)
states = robot.states()
print("q:", list(states.q))
print("OK — RDK Python link works. Enable RDK on teach pendant if mode/fault unexpected.")
PY
scripts/use_local_flexiv.sh
Executable
+50
View File
@@ -0,0 +1,50 @@
#!/usr/bin/env bash
# Wire local Flexiv trees under ~/文档/Flexiv_Rizon4 into this workspace.
# Avoids re-cloning RDK / sim plugin from GitHub.
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
SRC="${ROOT}/src"
TP="${ROOT}/thirdparty"
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"
if [[ ! -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
echo "ERROR: not found: ${LOCAL_FLEXIV}/flexiv_rdk"
echo "Set LOCAL_FLEXIV to your Flexiv_Rizon4 directory."
exit 1
fi
mkdir -p "${SRC}" "${TP}"
VER_LINE=$(grep -E '^project\(flexiv_rdk' "${LOCAL_FLEXIV}/flexiv_rdk/CMakeLists.txt" | head -1 || true)
echo "Linking RDK (${VER_LINE:-unknown version})..."
ln -sfn "${LOCAL_FLEXIV}/flexiv_rdk" "${SRC}/flexiv_rdk"
touch "${LOCAL_FLEXIV}/flexiv_rdk/COLCON_IGNORE"
if [[ -d "${LOCAL_FLEXIV}/flexiv_sim_plugin" ]]; then
if [[ -d "${TP}/flexiv_sim_plugin" && ! -L "${TP}/flexiv_sim_plugin" ]]; then
mv "${TP}/flexiv_sim_plugin" "${TP}/flexiv_sim_plugin.cloned.bak.$(date +%s)"
fi
ln -sfn "${LOCAL_FLEXIV}/flexiv_sim_plugin" "${TP}/flexiv_sim_plugin"
touch "${LOCAL_FLEXIV}/flexiv_sim_plugin/COLCON_IGNORE"
echo "Linked thirdparty/flexiv_sim_plugin -> ${LOCAL_FLEXIV}/flexiv_sim_plugin"
fi
# humble-v1 ROS hardware needs RDK 1.9.x (kept separately if present)
if [[ -d "${SRC}/flexiv_rdk.bak.1787646725" ]]; then
ln -sfn "${SRC}/flexiv_rdk.bak.1787646725" "${SRC}/flexiv_rdk_v1"
touch "${SRC}/flexiv_rdk.bak.1787646725/COLCON_IGNORE"
echo "ROS stack RDK v1.9.1 available at src/flexiv_rdk_v1"
fi
cat <<EOF
Done.
src/flexiv_rdk -> ${LOCAL_FLEXIV}/flexiv_rdk (your tree, currently v2.1)
thirdparty/flexiv_sim_plugin -> local sim plugin
Note: flexiv_ros2 humble-v1 expects RDK **v1.9.1**.
Build/install src/flexiv_rdk_v1 into ~/flexiv_install for flexiv_hardware.
Use your v2.1 tree for RDK C++/Python examples and Isaac-side tooling.
EOF
src/README.md
+9
View File
@@ -0,0 +1,9 @@
# src/ — ROS 2 包
详细职责见 [docs/WORKSPACE.md](../docs/WORKSPACE.md)。
**跟踪/手眼相关（本仓库维护）：** `arm_application` `arm_bringup` `arm_control` `arm_msgs`
**Flexiv 官方驱动（尽量不改）：** `flexiv_hardware` `flexiv_bringup` `flexiv_msgs` `flexiv_description` 以及 `flexiv_ros2/` 拷贝。
编译：`./scripts/build_all.sh`（先 `source scripts/env.sh`）。
src/arm_application/README.md
+39
View File
@@ -0,0 +1,39 @@
# arm_application
现场业务包。操作员用 `~/ros2/scripts/*.sh`，不要直接改这里的默认 yaml。
## 节点（console_scripts）
| 可执行文件 | 源文件 | 作用 |
|------------|--------|------|
| `realsense_color_pub` | `realsense_color_pub.py` | D405 发布 + 预览 |
| `perception_node` | `perception_node.py` | YOLO + 平面中心 → 点 |
| `servo_node` | `servo_node.py` | 100 Hz hover / 锁高 |
| `handeye_teach` | `handeye_teach.py` | 拖拽记点 |
| `handeye_calibrate` | `handeye_calibrate.py` | 走点 + 解 T |
| `handeye_waypoints` | `handeye_waypoints.py` | 旧巡航记点（可选） |
| `dh_pgea` / `dh_pgea_cli` | `dh_pgea_node.py` / `dh_pgea.py` | 左臂 PGEA |
| `dual_gripper` / `dual_gripper_cli` | `dual_gripper_node.py` / `dual_gripper_cli.py` | PGEA+RGI |
| `hole_detect` | `hole_detect_node.py` | 入孔检测 |
| `tactile` / `tactile_cli` | `tactile_node.py` / `tactile_cli.py` | Orisys 视触觉 |
| `goto_ee_plane` | `goto_ee_plane.py` | 可选：抬高再回 home（默认启动不再调用） |
| `demo_named_poses` | `demo_named_poses.py` | mock/实机点位示例 |
| `run_joint_trajectory` | `run_joint_trajectory.py` | 轨迹回放 |
库模块见包内 `__init__.py`。接口总表：[docs/INTERFACES.md](../../docs/INTERFACES.md)。
## 配置
| 文件 | 角色 |
|------|------|
| `config/perception.yaml` `servo.yaml` | 默认值，被 `site.yaml` 覆盖 |
| `config/hole_detect.yaml` | `two_stage` 缺省；被 `site.yaml hole_detect` 覆盖 |
| `config/hole_insert.yaml` | `yolo`/`hough` 缺省 |
| `config/orisys/dd03.json` | Orisys 厂家配置，路径写在 `site.yaml tactile.config_file` |
| `config/handeye_result_89.yaml` / `_91.yaml` | 该臂手眼 T（标定写入；禁止混用） |
| `config/handeye_poses_89.yaml` / `_91.yaml` | 该臂示教关节 |
| `launch/visual_servo.launch.py` | 感知 + 伺服 + 手眼 TF |
| `launch/dh_pgea.launch.py` | 左臂 PGEA |
| `launch/dual_gripper.launch.py` | 双爪 |
| `launch/hole_detect.launch.py` | 入孔检测 |
| `launch/tactile.launch.py` | 视触觉 |
src/arm_application/arm_application/__init__.py
+32
View File
@@ -0,0 +1,32 @@
"""arm_application — field nodes for Flexiv Rizon + D405 + DH grippers.
Public ROS executables (see setup.py console_scripts)
----------------------------------------------------
Camera: realsense_color_pub
Perception: perception_node
Servo: servo_node
Hand-eye: handeye_teach, handeye_waypoints, handeye_calibrate
Gripper: dh_pgea, dh_pgea_cli, dual_gripper, dual_gripper_cli
Hole detect: hole_detect
Tactile: tactile, tactile_cli
Helpers: goto_ee_plane, demo_named_poses, run_joint_trajectory
Reusable Python modules (import from your own nodes)
----------------------------------------------------
yolo_detector YOLO / TensorRT inference + pick_best
box_tracker pixel-space constant-velocity box coast
plane_feature stereo plane centroid inside a YOLO box
dh_pgea official Modbus RTU client for DH PGEA
dh_official DH Modbus client used by dual_gripper
dual_gripper PGEA left + RGI right
hole_detector yolo / hough / two_stage hole centers
orisys_tactile Orisys visuo-tactile adapter
geom_ik Rizon geometric Jacobian / DLS / hand-eye load
handeye_lib chessboard + OpenCV calibrateHandEye
handeye_realsense topic / serial helpers for D405
pid_controller PID with integral separation
kalman_filter 3D constant-velocity KF
Operators should use ``scripts/*.sh`` and ``config/site.yaml``, not these
imports. See ``docs/INTERFACES.md``.
"""
src/arm_application/arm_application/box_tracker.py
+85
View File
@@ -0,0 +1,85 @@
#!/usr/bin/env python3
"""Constant-velocity tracker for a YOLO xyxy box (pixel space).
Used so fast workpiece motion does not drop the lock: the next-frame gate
is the *predicted* box, and brief YOLO misses coast the overlay.
"""
from __future__ import annotations
from typing import Optional, Tuple
import numpy as np
def _center(xyxy: np.ndarray) -> np.ndarray:
return np.array(
[0.5 * (float(xyxy[0]) + float(xyxy[2])), 0.5 * (float(xyxy[1]) + float(xyxy[3]))],
dtype=float,
)
def _wh(xyxy: np.ndarray) -> Tuple[float, float]:
return (
max(8.0, float(xyxy[2]) - float(xyxy[0])),
max(8.0, float(xyxy[3]) - float(xyxy[1])),
)
class BoxTracker:
def __init__(
self,
*,
vmax_px_s: float = 1800.0,
v_lpf: float = 0.70,
max_dt: float = 0.25,
) -> None:
self.vmax_px_s = float(vmax_px_s)
self.v_lpf = float(np.clip(v_lpf, 0.05, 1.0))
self.max_dt = float(max_dt)
self.xyxy: Optional[np.ndarray] = None
self.v = np.zeros(2, dtype=float) # center px/s
self.t: Optional[float] = None
def reset(self) -> None:
self.xyxy = None
self.v[:] = 0.0
self.t = None
def predict(self, t: float, img_wh: Optional[Tuple[int, int]] = None) -> Optional[np.ndarray]:
if self.xyxy is None:
return None
out = self.xyxy.copy()
if self.t is None:
return out
dt = float(np.clip(t - self.t, 0.0, self.max_dt))
if dt <= 1e-4:
return out
c = _center(out) + self.v * dt
w, h = _wh(out)
x1, y1 = c[0] - 0.5 * w, c[1] - 0.5 * h
x2, y2 = c[0] + 0.5 * w, c[1] + 0.5 * h
if img_wh is not None:
iw, ih = float(img_wh[0]), float(img_wh[1])
x1 = float(np.clip(x1, 0.0, iw - 2.0))
y1 = float(np.clip(y1, 0.0, ih - 2.0))
x2 = float(np.clip(x2, x1 + 4.0, iw))
y2 = float(np.clip(y2, y1 + 4.0, ih))
return np.array([x1, y1, x2, y2], dtype=float)
def update(self, xyxy: np.ndarray, t: float) -> None:
xyxy = np.asarray(xyxy, dtype=float).reshape(4)
c = _center(xyxy)
if self.xyxy is not None and self.t is not None:
dt = float(t - self.t)
if dt > 0.28:
self.v[:] = 0.0
elif dt > 1e-3:
inst = (c - _center(self.xyxy)) / dt
n = float(np.hypot(inst[0], inst[1]))
if n > self.vmax_px_s > 0.0:
inst = inst * (self.vmax_px_s / n)
a = self.v_lpf
self.v = a * inst + (1.0 - a) * self.v
self.xyxy = xyxy.copy()
self.t = float(t)
src/arm_application/arm_application/demo_named_poses.py
+78
View File
@@ -0,0 +1,78 @@
#!/usr/bin/env python3
"""Demo: cycle named poses via /set_named_pose (works for any backend)."""
from __future__ import annotations
import time
import rclpy
from rclpy.node import Node
from arm_msgs.srv import SetNamedPose
class DemoNamedPoses(Node):
def __init__(self) -> None:
super().__init__('demo_named_poses')
self.declare_parameter(
'poses', ['home', 'ready', 'inspect', 'pos_left', 'home']
)
self.declare_parameter('velocity_scale', 0.5)
self.declare_parameter('dwell_sec', 0.3)
self.declare_parameter('loops', 1)
self.poses = list(
self.get_parameter('poses').get_parameter_value().string_array_value
or ['home', 'ready', 'inspect', 'home']
)
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
self.loops = int(self.get_parameter('loops').value)
self._cli = self.create_client(SetNamedPose, 'set_named_pose')
def run(self) -> int:
if not self._cli.wait_for_service(timeout_sec=15.0):
self.get_logger().error('/set_named_pose not available')
return 1
for loop in range(max(self.loops, 1)):
self.get_logger().info(f'loop {loop + 1}/{self.loops}')
for pose in self.poses:
req = SetNamedPose.Request()
req.pose_name = pose
req.velocity_scale = self.velocity_scale
req.acceleration_scale = self.velocity_scale
fut = self._cli.call_async(req)
deadline = time.monotonic() + 60.0
while rclpy.ok() and not fut.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error(f'timeout on {pose}')
return 2
res = fut.result()
if res is None or not res.success:
self.get_logger().error(
f'{pose} failed: {res.message if res else "no response"}'
)
return 3
self.get_logger().info(f'{pose}: {res.message}')
time.sleep(self.dwell_sec)
self.get_logger().info('demo done')
return 0
def main(args=None) -> None:
rclpy.init(args=args)
node = DemoNamedPoses()
code = 1
try:
code = node.run()
except KeyboardInterrupt:
code = 0
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
raise SystemExit(code)
if __name__ == '__main__':
main()
src/arm_application/arm_application/dh_official.py
+271
View File
@@ -0,0 +1,271 @@
"""DH-Robotics 官方 Modbus RTU 控制（非 Flexiv RDK）。
覆盖两类设备：
- PGE/PGEA 平行夹爪：初始化 0x0100，力 0x0101，位置 0x0103（0~1000‰），速度 0x0104。
- RGI 旋转夹爪：在平行夹爪寄存器基础上，增加旋转角度 0x0105、旋转速度 0x0107、
旋转力 0x0108；反馈增加旋转角度 0x0208、旋转初始化状态 0x020A、旋转状态 0x020B。
说明：
- RGI 手册示例中负角度使用“反码/one's complement”：+360 -> 0x0168，-360 -> 0xFE97。
因此这里默认按 one's complement 编解码；若后续厂家确认为 two's complement，只改
`_encode_signed` / `_decode_signed` 两处即可。
- 写位置/角度寄存器后会立即运动，没有 ChangingTek SDK 的 0x0108 触发寄存器。
"""
from __future__ import annotations
import threading
import time
from typing import Iterable
# =============================================================================
# DH-Robotics Modbus register map
# =============================================================================
# Basic control registers (holding registers, function code 03/06/10)
REG_INIT = 0x0100 # 0x01 initialize, 0xA5 fully initialize
REG_FORCE = 0x0101 # gripper closing force, 20~100 %
REG_POSITION = 0x0103 # finger reference position, 0~1000 per-mille
REG_SPEED = 0x0104 # finger open/close speed, 1~100 %
# RGI rotation control registers
REG_ROTATION_ANGLE = 0x0105 # target rotation angle, signed degree value
REG_ROTATION_SPEED = 0x0107 # rotation speed, 1~100 %
REG_ROTATION_FORCE = 0x0108 # rotation force/torque, 20~100 %
# Feedback registers
REG_INIT_STATE = 0x0200 # 0 uninitialized, 1 initialized, 2 initializing
REG_GRIPPER_STATE = 0x0201 # 0 moving, 1 reached, 2 object caught, 3 object dropped
REG_CURRENT_POSITION = 0x0202 # current finger position, per-mille
REG_ROTATION_ANGLE_FB = 0x0208 # current rotation angle, signed degree value
REG_ROTATION_INIT_STATE = 0x020A # 0 uninitialized, 1 initialized, 2 initializing
REG_ROTATION_STATE = 0x020B # 0 moving, 1 reached, 2 blocking, 3 had been blocked
# Configuration registers
REG_SAVE_PARAMS = 0x0300 # write 1 to save all parameters to flash
class DHModbusError(RuntimeError):
"""DH Modbus communication or protocol error."""
def _clamp_int(value: float | int, lo: int, hi: int) -> int:
return int(max(lo, min(hi, int(round(float(value))))))
def _encode_signed(value: float | int) -> int:
"""Encode signed degree value as DH one's-complement register value.
RGI manual example: +360 -> 0x0168, -360 -> 0xFE97.
"""
value = _clamp_int(value, -32767, 32767)
if value >= 0:
return value & 0xFFFF
return (~abs(value)) & 0xFFFF
def _decode_signed(raw: int) -> int:
"""Decode DH one's-complement register value to signed integer."""
raw = int(raw) & 0xFFFF
if raw & 0x8000:
mag = (~raw) & 0x7FFF
return -mag
return raw & 0x7FFF
class DHModbusClient:
"""Minimal thread-safe Modbus RTU client for DH-Robotics grippers."""
def __init__(
self,
port: str,
slave_id: int = 1,
baudrate: int = 115200,
timeout: float = 0.2,
*,
bytesize: int = 8,
parity: str = "N",
stopbits: int = 1,
) -> None:
try:
import minimalmodbus
import serial
except ImportError as exc: # pragma: no cover - depends on runtime env
raise DHModbusError(
"缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial"
) from exc
parity_map = {
"N": serial.PARITY_NONE,
"E": serial.PARITY_EVEN,
"O": serial.PARITY_ODD,
}
self.instrument = minimalmodbus.Instrument(port, int(slave_id))
self.instrument.serial.baudrate = int(baudrate)
self.instrument.serial.bytesize = int(bytesize)
self.instrument.serial.parity = parity_map.get(str(parity).upper(), serial.PARITY_NONE)
self.instrument.serial.stopbits = int(stopbits)
self.instrument.serial.timeout = float(timeout)
self.instrument.mode = minimalmodbus.MODE_RTU
self.instrument.clear_buffers_before_each_transaction = True
try:
self.instrument.serial.exclusive = True
except Exception:
pass
self._lock = threading.Lock()
self.port = port
def close(self) -> None:
with self._lock:
try:
self.instrument.serial.close()
except Exception:
pass
# ------------------------------------------------------------------
# Low-level register access
# ------------------------------------------------------------------
def write_register(self, address: int, value: int) -> None:
value = _clamp_int(value, 0, 0xFFFF)
last_exc: Exception | None = None
for attempt in range(3):
try:
with self._lock:
self.instrument.write_register(address, value, functioncode=6)
return
except Exception as exc:
last_exc = exc
time.sleep(0.05 * (attempt + 1))
raise DHModbusError(f"写寄存器 0x{address:04X}={value} 失败: {last_exc}") from last_exc
def read_register(self, address: int) -> int:
last_exc: Exception | None = None
for attempt in range(3):
try:
with self._lock:
return int(self.instrument.read_register(address, functioncode=3))
except Exception as exc:
last_exc = exc
time.sleep(0.05 * (attempt + 1))
hint = ""
text = str(last_exc).lower()
if "multiple access" in text or "no data" in text:
hint = f"。串口 {getattr(self, 'port', '')} 可能被其它进程占用，先执行: fuser -k {getattr(self, 'port', '/dev/ttyUSB*')}"
raise DHModbusError(f"读寄存器 0x{address:04X} 失败: {last_exc}{hint}") from last_exc
def write_registers(self, address: int, values: Iterable[int]) -> None:
vals = [_clamp_int(v, 0, 0xFFFF) for v in values]
with self._lock:
self.instrument.write_registers(address, vals)
def read_registers(self, address: int, count: int) -> list[int]:
with self._lock:
return [int(v) for v in self.instrument.read_registers(address, count, functioncode=3)]
def write_signed_register(self, address: int, value: float | int) -> None:
self.write_register(address, _encode_signed(value))
def read_signed_register(self, address: int) -> int:
return _decode_signed(self.read_register(address))
# ------------------------------------------------------------------
# Common PGE/PGEA/RGI control
# ------------------------------------------------------------------
def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
"""Initialize/homing the gripper and wait until ready."""
self.write_register(REG_INIT, 0xA5 if full else 0x01)
start = time.monotonic()
while time.monotonic() - start < timeout:
init_state = self.read_register(REG_INIT_STATE)
rot_ok = True
if rotation:
rot_ok = self.read_register(REG_ROTATION_INIT_STATE) == 1
if init_state == 1 and rot_ok:
return
time.sleep(0.1)
raise DHModbusError(f"夹爪初始化超时（>{timeout:.1f}s）")
def save_parameters(self) -> None:
self.write_register(REG_SAVE_PARAMS, 1)
def set_force_pct(self, force_pct: float | int) -> int:
value = _clamp_int(force_pct, 20, 100)
self.write_register(REG_FORCE, value)
return value
def set_speed_pct(self, speed_pct: float | int) -> int:
value = _clamp_int(speed_pct, 1, 100)
self.write_register(REG_SPEED, value)
return value
def set_position_per_mille(self, position: float | int) -> int:
value = _clamp_int(position, 0, 1000)
try:
cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
except Exception:
cmd = value
if cmd == value:
try:
cur = self.get_position_per_mille()
except Exception:
cur = value
nudge = cur + (40 if value >= cur else -40)
nudge = _clamp_int(nudge, 0, 1000)
if nudge == value:
nudge = 0 if value != 0 else 40
self.write_register(REG_POSITION, nudge)
time.sleep(0.06)
self.write_register(REG_POSITION, value)
return value
def get_position_per_mille(self) -> int:
return _clamp_int(self.read_register(REG_CURRENT_POSITION), 0, 1000)
def get_gripper_state(self) -> int:
return self.read_register(REG_GRIPPER_STATE)
def get_init_state(self) -> int:
return self.read_register(REG_INIT_STATE)
def hold_current_position(self) -> None:
"""Best-effort stop/hold for DH grippers without a dedicated stop register."""
self.set_position_per_mille(self.get_position_per_mille())
# ------------------------------------------------------------------
# RGI rotation control
# ------------------------------------------------------------------
def set_rotation_speed_pct(self, speed_pct: float | int) -> int:
value = _clamp_int(speed_pct, 1, 100)
self.write_register(REG_ROTATION_SPEED, value)
return value
def set_rotation_force_pct(self, force_pct: float | int) -> int:
value = _clamp_int(force_pct, 20, 100)
self.write_register(REG_ROTATION_FORCE, value)
return value
def set_rotation_angle_deg(self, angle_deg: float | int) -> int:
angle = _clamp_int(angle_deg, -32767, 32767)
self.write_signed_register(REG_ROTATION_ANGLE, angle)
return angle
def get_rotation_angle_raw(self) -> int:
return self.read_register(REG_ROTATION_ANGLE_FB) & 0xFFFF
def get_rotation_angle_deg(self) -> int:
return self.read_signed_register(REG_ROTATION_ANGLE_FB)
def get_rotation_init_state(self) -> int:
return self.read_register(REG_ROTATION_INIT_STATE)
def get_rotation_state(self) -> int:
return self.read_register(REG_ROTATION_STATE)
def hold_current_rotation(self) -> None:
"""Stop rotation by re-commanding the current feedback angle."""
self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw())
def hold_current_all(self) -> None:
self.hold_current_position()
self.hold_current_rotation()
src/arm_application/arm_application/dh_pgea.py
+584
View File
@@ -0,0 +1,584 @@
#!/usr/bin/env python3
"""DH Robotics PGE / PGEA Modbus RTU client.
Port of the official C++ driver:
DH-Robotics/dh_gripper_ros → DH_Modbus_Gripper
(factory maps model names containing ``PGE`` / ``PGEA`` onto this class)
Flexiv RDK ``Gripper`` cannot drive this claw: it is not a Studio Device.
Talk RS485 (USB–UART) with the same registers the DH ROS driver uses.
Registers (holding, FC03 read / FC06 write, CRC-16/Modbus LE):
0x0100 init write 0xA5
0x0101 force 20–100 %
0x0103 position 0–1000 ‰
0x0104 speed 1–100 %
0x0200 init state 0=need 1=done 2=doing
0x0201 grip state 0=moving 1=arrived 2=caught 3=dropped
0x0202 current position
"""
from __future__ import annotations
import argparse
import sys
import threading
import time
from dataclasses import dataclass
from typing import Optional
try:
import serial
except ImportError as exc: # pragma: no cover
raise SystemExit(
"pyserial missing. In conda ros_flexiv: pip install pyserial"
) from exc
# Official CRC-16/Modbus (same table as dh_modbus_gripper.cpp).
_CRC_TABLE = (
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040,
)
REG_INIT = 0x0100
REG_FORCE = 0x0101
REG_POSITION = 0x0103
REG_SPEED = 0x0104
REG_INIT_STATE = 0x0200
REG_GRIP_STATE = 0x0201
REG_CUR_POSITION = 0x0202
INIT_CMD = 0xA5
S_INIT_NOT = 0
S_INIT_FINISHED = 1
S_INIT_DOING = 2
S_GRIP_MOVING = 0
S_GRIP_ARRIVED = 1
S_GRIP_CAUGHT = 2
S_GRIP_DROPPED = 3
GRIP_STATE_NAME = {
S_GRIP_MOVING: "moving",
S_GRIP_ARRIVED: "arrived",
S_GRIP_CAUGHT: "caught",
S_GRIP_DROPPED: "dropped",
}
INIT_STATE_NAME = {
S_INIT_NOT: "need_init",
S_INIT_FINISHED: "initialized",
S_INIT_DOING: "initializing",
}
def crc16_modbus(data: bytes) -> int:
"""Official DH CRC16 (Modbus, little-endian on the wire)."""
crc = 0xFFFF
for byte in data:
crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF]
return crc
def _frame(slave: int, func: int, index: int, value: int) -> bytes:
body = bytes(
[
slave & 0xFF,
func & 0xFF,
(index >> 8) & 0xFF,
index & 0xFF,
(value >> 8) & 0xFF,
value & 0xFF,
]
)
crc = crc16_modbus(body)
return body + bytes([crc & 0xFF, (crc >> 8) & 0xFF])
@dataclass
class GripperState:
init_state: int = S_INIT_NOT
grip_state: int = S_GRIP_MOVING
position: int = 0
target_position: int = 0
target_force: int = 0
stroke_mm: float = 20.0
@property
def initialized(self) -> bool:
return self.init_state == S_INIT_FINISHED
@property
def width_mm(self) -> float:
return float(self.position) / 1000.0 * float(self.stroke_mm)
def as_tuple(self) -> tuple[float, float, float, float, float]:
return (
1.0 if self.initialized else 0.0,
float(self.grip_state),
float(self.position),
float(self.target_position),
float(self.target_force),
)
def summary(self) -> str:
return (
f"init={INIT_STATE_NAME.get(self.init_state, self.init_state)} "
f"grip={GRIP_STATE_NAME.get(self.grip_state, self.grip_state)} "
f"pos={self.position}/1000 ({self.width_mm:.2f} mm) "
f"target={self.target_position} force={self.target_force}%"
)
class DhPgeaError(RuntimeError):
pass
WIRING_HINT = (
"PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n"
" 1) 夹爪 24 V DC 已上电（指示灯亮）\n"
" 2) USB 转接头必须是 RS485（A/B），不能是 TTL 的 TX/RX\n"
" 3) A↔A、B↔B；不通就对调 A/B\n"
" 4) 先 Ctrl+C 停节点，再：bash ~/ros2/scripts/run_gripper.sh --cli scan"
)
class DhPgeaGripper:
"""Thread-safe Modbus RTU client for PGE / PGEA (official register map)."""
def __init__(
self,
port: str = "/dev/ttyUSB0",
baudrate: int = 115200,
slave_id: int = 1,
timeout_s: float = 0.25,
stroke_mm: float = 20.0,
rs485_rts: str = "",
) -> None:
self.port = port
self.baudrate = int(baudrate)
self.slave_id = int(slave_id)
self.timeout_s = float(timeout_s)
self.stroke_mm = float(stroke_mm)
mode = str(rs485_rts or "").strip().lower()
if mode in ("", "off", "none", "false", "0"):
mode = ""
if mode not in ("", "tx", "tx_inv"):
raise DhPgeaError(f"rs485_rts must be '', tx, or tx_inv, got {rs485_rts!r}")
self.rs485_rts = mode
self._ser: Optional[serial.Serial] = None
self._lock = threading.Lock()
def open(self) -> None:
if self._ser is not None and self._ser.is_open:
return
try:
self._ser = serial.Serial(
port=self.port,
baudrate=self.baudrate,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=self.timeout_s,
write_timeout=self.timeout_s,
xonxoff=False,
rtscts=False,
dsrdtr=False,
exclusive=True,
)
except Exception as exc:
raise DhPgeaError(
f"cannot open {self.port}: {exc}. "
"Need 24 V DC + RS485, and user in group dialout."
) from exc
try:
self._ser.dtr = True
except Exception:
pass
self._set_tx(False)
self._ser.reset_input_buffer()
self._ser.reset_output_buffer()
time.sleep(0.05)
def close(self) -> None:
if self._ser is not None:
try:
self._ser.close()
except Exception:
pass
self._ser = None
def __enter__(self) -> "DhPgeaGripper":
self.open()
return self
def __exit__(self, *args) -> None:
self.close()
def write_register(self, index: int, value: int, retries: int = 3) -> bool:
"""FC06 write, wait for echo (official WriteRegisterFunc)."""
frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF)
with self._lock:
for _ in range(max(1, retries)):
if self._transact(frame, 8) == frame:
return True
time.sleep(0.02)
return False
def read_register(self, index: int, retries: int = 8) -> int:
"""FC03 read one holding register (official ReadRegisterFunc)."""
req = _frame(self.slave_id, 0x03, index, 1)
last: Optional[bytes] = None
with self._lock:
for _ in range(max(1, retries)):
rsp = self._transact(req, 7)
last = rsp
if rsp is None or len(rsp) != 7:
time.sleep(0.02)
continue
if crc16_modbus(rsp[:-2]) != (rsp[5] | (rsp[6] << 8)):
continue
if rsp[0] != (self.slave_id & 0xFF) or rsp[1] != 0x03:
continue
return (rsp[3] << 8) | (rsp[4] & 0xFF)
hint = f" last={last.hex() if last else '(empty)'}"
raise DhPgeaError(
f"Modbus read 0x{index:04X} failed on {self.port} "
f"(no valid reply{hint}). baud={self.baudrate} slave={self.slave_id} "
f"rts={self.rs485_rts or 'off'}"
)
def probe(self) -> int:
"""One short read of init-state. Raises if the claw is silent."""
return self.read_register(REG_INIT_STATE, retries=2)
def _set_tx(self, tx: bool) -> None:
if not self.rs485_rts or self._ser is None:
return
try:
if self.rs485_rts == "tx":
self._ser.rts = bool(tx)
else:
self._ser.rts = not bool(tx)
except Exception:
pass
def _transact(self, frame: bytes, expect: int) -> Optional[bytes]:
if self._ser is None or not self._ser.is_open:
raise DhPgeaError("serial not open")
bit_s = 10.0 / max(1, self.baudrate)
self._set_tx(True)
if self.rs485_rts:
time.sleep(2.0 * bit_s)
self._ser.reset_input_buffer()
self._ser.write(frame)
self._ser.flush()
if self.rs485_rts:
time.sleep(len(frame) * bit_s + 0.0008)
self._set_tx(False)
deadline = time.monotonic() + self.timeout_s
buf = bytearray()
while time.monotonic() < deadline and len(buf) < expect:
chunk = self._ser.read(expect - len(buf))
if chunk:
buf.extend(chunk)
return bytes(buf) if buf else None
def initialization(self) -> bool:
return self.write_register(REG_INIT, INIT_CMD)
def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
"""Write 0xA5 then wait until init state == finished (official bringup)."""
state = self.get_init_state()
if state == S_INIT_FINISHED:
return True
if not self.initialization():
raise DhPgeaError("init write 0x0100=0xA5 failed (no Modbus echo)")
deadline = time.monotonic() + float(timeout_s)
while time.monotonic() < deadline:
state = self.get_init_state()
if state == S_INIT_FINISHED:
return True
time.sleep(poll_s)
raise DhPgeaError(
f"init timeout after {timeout_s:.0f}s (state={state}). "
"Fingers should home once on first power-up."
)
def set_target_force(self, force_pct: int) -> bool:
force_pct = int(max(20, min(100, force_pct)))
return self.write_register(REG_FORCE, force_pct)
def set_target_speed(self, speed_pct: int) -> bool:
speed_pct = int(max(1, min(100, speed_pct)))
return self.write_register(REG_SPEED, speed_pct)
def set_target_position(self, pos: int) -> bool:
pos = int(max(0, min(1000, pos)))
# 大寰固件：0x0103 写入与当前命令相同的值不会启动新运动。
# 开/关卡在 caught 时最常见——命令已是 1000，手指却停在夹持位。
try:
cmd = self.get_target_position(retries=2)
except Exception:
cmd = pos
if cmd == pos:
try:
cur = self.get_current_position(retries=2)
except Exception:
cur = pos
nudge = cur + (40 if pos >= cur else -40)
nudge = int(max(0, min(1000, nudge)))
if nudge == pos:
nudge = 0 if pos != 0 else 40
self.write_register(REG_POSITION, nudge)
time.sleep(0.06)
return self.write_register(REG_POSITION, pos)
def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
"""等到到位/夹住/松开，或超时返回最后一帧。"""
t0 = time.monotonic()
last = None
st = self.get_run_states()
while time.monotonic() - t0 < timeout_s:
st = self.get_run_states()
moved = last is not None and st.position != last
last = st.position
if st.grip_state == S_GRIP_MOVING or moved:
time.sleep(0.08)
continue
if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
return st
if st.grip_state == S_GRIP_CAUGHT:
return st
time.sleep(0.08)
return st
def get_init_state(self, retries: int = 8) -> int:
return self.read_register(REG_INIT_STATE, retries=retries)
def get_grip_state(self, retries: int = 8) -> int:
return self.read_register(REG_GRIP_STATE, retries=retries)
def get_current_position(self, retries: int = 8) -> int:
return self.read_register(REG_CUR_POSITION, retries=retries)
def get_target_position(self, retries: int = 8) -> int:
return self.read_register(REG_POSITION, retries=retries)
def get_target_force(self, retries: int = 8) -> int:
return self.read_register(REG_FORCE, retries=retries)
def get_run_states(self, retries: int = 8) -> GripperState:
return GripperState(
init_state=self.get_init_state(retries=retries),
grip_state=self.get_grip_state(retries=retries),
position=self.get_current_position(retries=retries),
target_position=self.get_target_position(retries=retries),
target_force=self.get_target_force(retries=retries),
stroke_mm=self.stroke_mm,
)
def open_jaw(self, force: int = 50, speed: int = 50) -> bool:
ok = self.set_target_force(force)
ok = self.set_target_speed(speed) and ok
return self.set_target_position(1000) and ok
def close_jaw(self, force: int = 50, speed: int = 50) -> bool:
ok = self.set_target_force(force)
ok = self.set_target_speed(speed) and ok
return self.set_target_position(0) and ok
def move(self, position: int, force: int = 50, speed: int = 50) -> bool:
ok = self.set_target_force(force)
ok = self.set_target_speed(speed) and ok
return self.set_target_position(position) and ok
def _selfcheck_crc() -> None:
"""Official short-manual example: set position 500."""
frame = _frame(1, 0x06, 0x0103, 500)
expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
if frame != expect:
raise AssertionError(f"CRC mismatch: {frame.hex()} != {expect.hex()}")
def _site_defaults() -> dict:
try:
from pathlib import Path
root = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(root / "scripts" / "lib"))
from export_site_env import load_site # type: ignore
g = (load_site().get("gripper") or {})
return {
"port": str(g.get("port") or "/dev/ttyUSB0"),
"baudrate": int(g.get("baud") or 115200),
"slave_id": int(g.get("slave_id") or 1),
"stroke_mm": float(g.get("stroke_mm") or 20.0),
"force": int(g.get("force_pct") or 50),
"speed": int(g.get("speed_pct") or 50),
"rs485_rts": str(g.get("rs485_rts") or ""),
}
except Exception:
return {
"port": "/dev/ttyUSB0",
"baudrate": 115200,
"slave_id": 1,
"stroke_mm": 20.0,
"force": 50,
"speed": 50,
"rs485_rts": "",
}
def _scan_bus(port: str) -> int:
"""Try baud × slave × RTS until one combination answers."""
bauds = (115200, 57600, 38400, 19200, 9600)
slaves = (1, 2, 3)
rts_modes = ("", "tx", "tx_inv")
print(f"scan {port} (stop the ROS node first — port is exclusive)")
for baud in bauds:
for slave in slaves:
for rts in rts_modes:
label = f"baud={baud} slave={slave} rts={rts or 'off'}"
g = DhPgeaGripper(
port=port,
baudrate=baud,
slave_id=slave,
timeout_s=0.08,
rs485_rts=rts,
)
try:
g.open()
state = g.probe()
print(f"OK {label} init_state={state}")
print(f" → site.yaml: baud: {baud} slave_id: {slave} rs485_rts: \"{rts}\"")
g.close()
return 0
except DhPgeaError:
print(f" miss {label}")
finally:
g.close()
print("no reply on any combination.", file=sys.stderr)
print(WIRING_HINT, file=sys.stderr)
return 1
def _cli(argv: Optional[list[str]] = None) -> int:
site = _site_defaults()
ap = argparse.ArgumentParser(
description="DH PGEA Modbus CLI (official register map, not Flexiv RDK)"
)
ap.add_argument("--port", default=site["port"])
ap.add_argument("--baud", type=int, default=site["baudrate"])
ap.add_argument("--slave", type=int, default=site["slave_id"])
ap.add_argument("--force", type=int, default=site["force"])
ap.add_argument("--speed", type=int, default=site["speed"])
ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE: empty | tx | tx_inv")
ap.add_argument(
"cmd",
choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"],
)
ap.add_argument("value", nargs="?", default="", help="position 0–1000 for 'pos'")
args = ap.parse_args(argv)
if args.cmd == "selfcheck":
_selfcheck_crc()
print("CRC self-check OK (01 06 01 03 01 F4 78 21)")
return 0
if args.cmd == "scan":
return _scan_bus(args.port)
grip = DhPgeaGripper(
port=args.port,
baudrate=args.baud,
slave_id=args.slave,
stroke_mm=float(site["stroke_mm"]),
rs485_rts=args.rts,
)
try:
grip.open()
except DhPgeaError as exc:
print(f"ERROR: {exc}", file=sys.stderr)
return 1
try:
if args.cmd == "probe":
st = grip.get_run_states(retries=2)
print(st.summary())
return 0
if args.cmd == "init":
print("initializing (fingers home)…")
grip.initialize()
print(grip.get_run_states().summary())
return 0
if args.cmd == "status":
print(grip.get_run_states().summary())
return 0
if args.cmd == "open":
if not grip.open_jaw(args.force, args.speed):
raise DhPgeaError("open write failed")
print(grip.wait_settle(1000).summary())
return 0
if args.cmd == "close":
if not grip.close_jaw(args.force, args.speed):
raise DhPgeaError("close write failed")
print(grip.wait_settle(0).summary())
return 0
if args.cmd == "pos":
if args.value == "":
print("usage: pos <0-1000>", file=sys.stderr)
return 2
pos = int(float(args.value))
if not grip.move(pos, args.force, args.speed):
raise DhPgeaError("position write failed")
print(grip.wait_settle(pos).summary())
return 0
except DhPgeaError as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(WIRING_HINT, file=sys.stderr)
return 1
finally:
grip.close()
return 0
def main(argv: Optional[list[str]] = None) -> int:
return _cli(argv)
if __name__ == "__main__":
raise SystemExit(main())
src/arm_application/arm_application/dh_pgea_node.py
+254
View File
@@ -0,0 +1,254 @@
#!/usr/bin/env python3
"""ROS 2 node for DH PGEA (official Modbus RTU, not Flexiv gripper).
Topics (layout matches DH-Robotics/dh_gripper_ros):
/gripper/ctrl std_msgs/Float32MultiArray
data: [initialize, position, force, speed]
initialize>0.5 → write 0xA5; else set force/speed/position
/gripper/states std_msgs/Float32MultiArray
data: [initialized, grip_state, position, target_pos, target_force]
/gripper/joint_states sensor_msgs/JointState
Services (std_srvs/Trigger):
/gripper/initialize /gripper/open /gripper/close
Do not set Flexiv bringup load_gripper:=true — that is Flexiv-GN01 only.
"""
from __future__ import annotations
import threading
from typing import Optional
import rclpy
from rcl_interfaces.msg import ParameterDescriptor
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiArrayLayout
from std_srvs.srv import Trigger
from arm_application.dh_pgea import (
DhPgeaError,
DhPgeaGripper,
GripperState,
WIRING_HINT,
)
def _param_as_str(node: Node, name: str, default: str = "") -> str:
"""String param that also accepts numeric CLI overrides (e.g. SN 26610799)."""
node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
val = node.get_parameter(name).value
if val is None or val == "":
return ""
return str(val).strip()
def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
data = list(msg.data)
while len(data) < 4:
data.append(0.0)
return float(data[0]), float(data[1]), float(data[2]), float(data[3])
class DhPgeaNode(Node):
def __init__(self) -> None:
super().__init__("dh_pgea")
self.declare_parameter("port", "/dev/ttyUSB0")
self.declare_parameter("baud", 115200)
self.declare_parameter("slave_id", 1)
self.declare_parameter("stroke_mm", 20.0)
self.declare_parameter("force_pct", 50)
self.declare_parameter("speed_pct", 50)
self.declare_parameter("auto_init", True)
self.declare_parameter("state_hz", 20.0)
self.declare_parameter("open_position", 1000)
self.declare_parameter("close_position", 0)
self._model = _param_as_str(self, "model", "PGEA-100-20-W")
self._sn = _param_as_str(self, "serial_number", "")
self._rts = _param_as_str(self, "rs485_rts", "")
self._port = str(self.get_parameter("port").value)
self._baud = int(self.get_parameter("baud").value)
self._slave = int(self.get_parameter("slave_id").value)
self._stroke = float(self.get_parameter("stroke_mm").value)
self._force = int(self.get_parameter("force_pct").value)
self._speed = int(self.get_parameter("speed_pct").value)
_auto = self.get_parameter("auto_init").value
if isinstance(_auto, str):
self._auto_init = _auto.strip().lower() in ("1", "true", "yes")
else:
self._auto_init = bool(_auto)
self._open_pos = int(self.get_parameter("open_position").value)
self._close_pos = int(self.get_parameter("close_position").value)
hz = float(self.get_parameter("state_hz").value)
self._grip = DhPgeaGripper(
port=self._port,
baudrate=self._baud,
slave_id=self._slave,
stroke_mm=self._stroke,
rs485_rts=self._rts,
)
self._state = GripperState(stroke_mm=self._stroke)
self._cmd_lock = threading.Lock()
self._pub_state = self.create_publisher(Float32MultiArray, "/gripper/states", 10)
self._pub_js = self.create_publisher(JointState, "/gripper/joint_states", 10)
self.create_subscription(Float32MultiArray, "/gripper/ctrl", self._on_ctrl, 10)
self.create_subscription(Float32, "/gripper/set_position", self._on_set_pos, 10)
self.create_service(Trigger, "/gripper/initialize", self._srv_init)
self.create_service(Trigger, "/gripper/open", self._srv_open)
self.create_service(Trigger, "/gripper/close", self._srv_close)
self.get_logger().info(
f"DH {self._model} SN={self._sn or '?'} "
f"{self._port} @{self._baud} slave={self._slave} rts={self._rts or 'off'} "
f"(official Modbus RTU; Flexiv RDK gripper API unused)"
)
try:
self._grip.open()
except DhPgeaError as exc:
self.get_logger().error(str(exc))
raise
self._online = False
try:
self._grip.probe()
self._online = True
except DhPgeaError as exc:
self.get_logger().error(str(exc))
for line in WIRING_HINT.splitlines():
self.get_logger().error(line)
self.get_logger().error("will keep retrying; open/close will fail until the bus answers")
if self._auto_init and self._online:
try:
self._grip.initialize()
self.get_logger().info("gripper initialized")
except DhPgeaError as exc:
self.get_logger().error(f"auto_init failed: {exc}")
self._online = False
period = 1.0 / max(1.0, hz)
self._timer = self.create_timer(period, self._on_timer)
def destroy_node(self) -> bool:
try:
self._grip.close()
except Exception:
pass
return super().destroy_node()
def _apply_move(self, position: int, force: Optional[int] = None, speed: Optional[int] = None) -> str:
with self._cmd_lock:
f = self._force if force is None else int(force)
s = self._speed if speed is None else int(speed)
if force is not None:
self._force = f
if speed is not None:
self._speed = s
if not self._grip.move(int(position), f, s):
raise DhPgeaError("move write failed")
return f"pos={int(position)} force={f}% speed={s}%"
def _on_ctrl(self, msg: Float32MultiArray) -> None:
initialize, position, force, speed = _as_ctrl_array(msg)
try:
if initialize > 0.5:
self._grip.initialize()
self.get_logger().info("initialize via /gripper/ctrl")
return
f = int(force) if force > 0 else self._force
s = int(speed) if speed > 0 else self._speed
info = self._apply_move(int(position), f, s)
self.get_logger().info(f"ctrl {info}")
except DhPgeaError as exc:
self.get_logger().error(str(exc))
def _on_set_pos(self, msg: Float32) -> None:
try:
info = self._apply_move(int(msg.data))
self.get_logger().info(f"set_position {info}")
except DhPgeaError as exc:
self.get_logger().error(str(exc))
def _srv_init(self, _req, resp):
try:
self._grip.initialize()
resp.success = True
resp.message = self._grip.get_run_states().summary()
except DhPgeaError as exc:
resp.success = False
resp.message = str(exc)
return resp
def _srv_open(self, _req, resp):
try:
info = self._apply_move(self._open_pos)
resp.success = True
resp.message = info
except DhPgeaError as exc:
resp.success = False
resp.message = str(exc)
return resp
def _srv_close(self, _req, resp):
try:
info = self._apply_move(self._close_pos)
resp.success = True
resp.message = info
except DhPgeaError as exc:
resp.success = False
resp.message = str(exc)
return resp
def _on_timer(self) -> None:
try:
st = self._grip.get_run_states(retries=2 if self._online else 1)
self._state = st
if not self._online:
self._online = True
self.get_logger().info("gripper bus recovered")
except DhPgeaError as exc:
self._online = False
self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
return
arr = Float32MultiArray()
arr.layout = MultiArrayLayout(
dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)],
data_offset=0,
)
arr.data = list(st.as_tuple())
self._pub_state.publish(arr)
js = JointState()
js.header.stamp = self.get_clock().now().to_msg()
js.name = ["gripper_finger1_joint"]
# Official ROS driver maps 0–1000 → ~0–0.637 rad; we publish mm as well.
js.position = [float(st.width_mm) * 0.001]
js.effort = [float(st.target_force)]
self._pub_js.publish(js)
def main(args=None) -> None:
rclpy.init(args=args)
node = None
try:
node = DhPgeaNode()
rclpy.spin(node)
except KeyboardInterrupt:
pass
except DhPgeaError:
pass
finally:
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
main()
src/arm_application/arm_application/dual_gripper.py
+803
View File
@@ -0,0 +1,803 @@
"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。
设计目标：
- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。
- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。
- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。
注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。
"""
from __future__ import annotations
import time
from abc import ABC, abstractmethod
from dataclasses import dataclass, field
from enum import Enum, auto
from pathlib import Path
from typing import Any
import numpy as np
from arm_application.dh_official import DHModbusClient, DHModbusError
class GripperType(Enum):
PARALLEL = auto() # 平行夹爪
ROTARY = auto() # 旋转夹爪
VACUUM = auto() # 真空吸盘（预留）
class GripperState(Enum):
IDLE = auto()
MOVING = auto()
GRIPPING = auto()
RELEASING = auto()
ERROR = auto()
@dataclass
class GripperStatus:
"""夹爪状态反馈。"""
state: GripperState = GripperState.IDLE
position: float = 0.0 # 平行：mm；旋转：角度 deg
force: float = 0.0 # 最近一次命令力/力矩 %
velocity: float = 0.0 # 最近一次命令速度 %
is_moving: bool = False
is_gripped: bool = False # 是否夹持到物体
connected: bool = False
initialized: bool = False
finger_position_per_mille: float = 0.0
rotation_deg: float = 0.0
gripper_state: int = -1 # DH 0x0201：0 moving,1 reached,2 caught,3 dropped
rotation_state: int = -1 # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked
error_msg: str = ""
timestamp: float = field(default_factory=time.time)
@dataclass
class GripperCommand:
"""夹爪控制指令。"""
target_position: float | None = None # 平行：mm；旋转：deg
target_force: float | None = None # 目标力/力矩 %
target_velocity: float | None = None # 目标速度 %
wait_complete: bool = True # 是否等待完成
timeout: float = 10.0 # 超时时间
class BaseGripper(ABC):
"""夹爪基类，定义统一接口。"""
name: str = "base_gripper"
gripper_type: GripperType = GripperType.PARALLEL
def __init__(self, config: dict[str, Any] | None = None) -> None:
self.cfg = config or {}
self._connected = False
self._status = GripperStatus()
@abstractmethod
def connect(self) -> None: ...
@abstractmethod
def disconnect(self) -> None: ...
@abstractmethod
def execute(self, cmd: GripperCommand) -> GripperStatus: ...
@abstractmethod
def get_status(self) -> GripperStatus: ...
def is_connected(self) -> bool:
return self._connected
def open(self, **kwargs) -> GripperStatus:
raise NotImplementedError
def close(self, **kwargs) -> GripperStatus:
raise NotImplementedError
def stop(self) -> None:
raise NotImplementedError
# =============================================================================
# 大寰平行夹爪（PGE/PGEA 系列）
# =============================================================================
@dataclass
class ParallelGripperConfig:
"""大寰平行夹爪配置。"""
port: str = "/dev/ttyUSB0"
slave_id: int = 1
baudrate: int = 115200
timeout: float = 0.2
# PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。
stroke_min: float = 0.0
stroke_max: float = 40.0
# 默认参数（DH 百分比）
default_speed: int = 50
default_force: int = 50
force_min: int = 20
force_max: int = 100
# 控制行为
auto_initialize: bool = True
init_timeout: float = 10.0
position_tolerance_per_mille: int = 15
status_poll_interval: float = 0.05
class ParallelGripper(BaseGripper):
"""大寰 PGE/PGEA 平行夹爪。"""
name = "dh_pgea_parallel"
gripper_type = GripperType.PARALLEL
def __init__(self, config: ParallelGripperConfig | None = None) -> None:
super().__init__()
self.cfg = config or ParallelGripperConfig()
self._client: DHModbusClient | None = None
self._last_position_mm = float(self.cfg.stroke_min)
self._last_force = int(self.cfg.default_force)
self._last_speed = int(self.cfg.default_speed)
def connect(self) -> None:
if self._client is not None:
self.disconnect()
self._client = DHModbusClient(
self.cfg.port,
slave_id=self.cfg.slave_id,
baudrate=self.cfg.baudrate,
timeout=self.cfg.timeout,
)
self._connected = True
self._status.connected = True
self._status.state = GripperState.IDLE
self._status.error_msg = ""
if self.cfg.auto_initialize:
self.initialize()
def disconnect(self) -> None:
if self._client is not None:
self._client.close()
self._client = None
self._connected = False
self._status.connected = False
self._status.state = GripperState.IDLE
def initialize(self, full: bool = False) -> None:
self._require_client()
assert self._client is not None
self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout)
self._status.initialized = True
def _require_client(self) -> None:
if not self._connected or self._client is None:
self._status.state = GripperState.ERROR
self._status.error_msg = "平行夹爪未连接"
raise DHModbusError(self._status.error_msg)
def _mm_to_per_mille(self, position_mm: float) -> int:
lo = float(self.cfg.stroke_min)
hi = float(self.cfg.stroke_max)
if hi <= lo:
hi = lo + 1.0
ratio = (float(position_mm) - lo) / (hi - lo)
return int(np.clip(round(ratio * 1000.0), 0, 1000))
def _per_mille_to_mm(self, position: float | int) -> float:
lo = float(self.cfg.stroke_min)
hi = float(self.cfg.stroke_max)
return lo + (float(position) / 1000.0) * (hi - lo)
def execute(self, cmd: GripperCommand) -> GripperStatus:
try:
self._require_client()
except DHModbusError:
return self._status
assert self._client is not None
position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position)
position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max))
speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100))
force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max))
self._client.set_speed_pct(speed)
self._client.set_force_pct(force)
target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm))
self._last_position_mm = position_mm
self._last_speed = speed
self._last_force = force
self._status.state = GripperState.MOVING
self._status.error_msg = ""
if cmd.wait_complete:
self._wait_complete(target_per_mille, cmd.timeout)
return self.get_status()
def _wait_complete(self, target_per_mille: int, timeout: float) -> None:
assert self._client is not None
start = time.monotonic()
time.sleep(0.05)
while time.monotonic() - start < timeout:
state = self._client.get_gripper_state()
pos = self._client.get_position_per_mille()
self._status.gripper_state = state
self._status.finger_position_per_mille = float(pos)
self._status.position = self._per_mille_to_mm(pos)
self._status.is_moving = state == 0
self._status.is_gripped = state == 2
if state in (1, 2):
self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE
return
if state == 3:
self._status.state = GripperState.ERROR
self._status.error_msg = "夹持后物体掉落"
return
if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0:
self._status.state = GripperState.IDLE
return
time.sleep(self.cfg.status_poll_interval)
self._status.state = GripperState.ERROR
self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）"
def get_status(self) -> GripperStatus:
if not self._connected or self._client is None:
self._status.connected = False
return self._status
try:
pos = self._client.get_position_per_mille()
state = self._client.get_gripper_state()
init_state = self._client.get_init_state()
self._status.connected = True
self._status.initialized = init_state == 1
self._status.finger_position_per_mille = float(pos)
self._status.position = self._per_mille_to_mm(pos)
self._status.velocity = float(self._last_speed)
self._status.force = float(self._last_force)
self._status.gripper_state = state
self._status.is_moving = state == 0
self._status.is_gripped = state == 2
if self._status.state != GripperState.ERROR:
self._status.state = GripperState.MOVING if state == 0 else (
GripperState.GRIPPING if state == 2 else GripperState.IDLE
)
self._status.timestamp = time.time()
except Exception as exc:
self._status.state = GripperState.ERROR
self._status.error_msg = str(exc)
self._status.connected = False
return self._status
def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
# DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。
return self.execute(GripperCommand(
target_position=float(self.cfg.stroke_max),
target_force=force or self.cfg.default_force,
target_velocity=speed or self.cfg.default_speed,
**kwargs,
))
def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
return self.execute(GripperCommand(
target_position=float(self.cfg.stroke_min),
target_force=force or self.cfg.default_force,
target_velocity=speed or self.cfg.default_speed,
**kwargs,
))
def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None,
**kwargs) -> GripperStatus:
return self.execute(GripperCommand(
target_position=position_mm,
target_force=force or self.cfg.default_force,
target_velocity=speed or self.cfg.default_speed,
**kwargs,
))
def set_force(self, force_pct: int) -> None:
self._require_client()
assert self._client is not None
self._last_force = self._client.set_force_pct(force_pct)
def set_speed(self, speed_pct: int) -> None:
self._require_client()
assert self._client is not None
self._last_speed = self._client.set_speed_pct(speed_pct)
def stop(self) -> None:
if self._client is not None:
self._client.hold_current_position()
self._status.state = GripperState.IDLE
# =============================================================================
# 大寰旋转夹爪（RGI-100-30-O-S-M1）
# =============================================================================
@dataclass
class RotaryGripperConfig:
"""大寰 RGI 旋转夹爪配置。"""
port: str = "/dev/ttyUSB1"
slave_id: int = 1
baudrate: int = 115200
timeout: float = 0.2
# RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。
angle_min: float = -32767.0
angle_max: float = 32767.0
max_step_deg: float = 30000.0 # rotate_revolutions 分段旋转的最大单步角度
# DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。
finger_open_per_mille: int = 1000
finger_close_per_mille: int = 0
# 默认参数（DH 百分比）
default_speed: int = 50 # 旋转速度 %
default_torque: int = 50 # 旋转力/力矩 %
default_finger_speed: int = 50 # 手指速度 %
default_finger_force: int = 50 # 手指夹持力 %
torque_min: int = 20
torque_max: int = 100
# 控制行为
auto_initialize: bool = True
init_timeout: float = 15.0 # RGI 初始化包含旋转找零，时间更长
angle_tolerance_deg: float = 1.0
status_poll_interval: float = 0.05
class RotaryGripper(BaseGripper):
"""大寰 RGI-100-30-O-S-M1 旋转夹爪。"""
name = "dh_rgi_rotary"
gripper_type = GripperType.ROTARY
def __init__(self, config: RotaryGripperConfig | None = None) -> None:
super().__init__()
self.cfg = config or RotaryGripperConfig()
self._client: DHModbusClient | None = None
self._last_angle = 0.0
self._last_torque = int(self.cfg.default_torque)
self._last_speed = int(self.cfg.default_speed)
self._last_finger_force = int(self.cfg.default_finger_force)
self._last_finger_speed = int(self.cfg.default_finger_speed)
self._last_finger_per_mille = int(self.cfg.finger_open_per_mille)
def connect(self) -> None:
if self._client is not None:
self.disconnect()
self._client = DHModbusClient(
self.cfg.port,
slave_id=self.cfg.slave_id,
baudrate=self.cfg.baudrate,
timeout=self.cfg.timeout,
)
self._connected = True
self._status.connected = True
self._status.state = GripperState.IDLE
self._status.error_msg = ""
if self.cfg.auto_initialize:
self.initialize()
def disconnect(self) -> None:
if self._client is not None:
self._client.close()
self._client = None
self._connected = False
self._status.connected = False
self._status.state = GripperState.IDLE
def initialize(self, full: bool = False) -> None:
self._require_client()
assert self._client is not None
self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout)
self._status.initialized = True
def _require_client(self) -> None:
if not self._connected or self._client is None:
self._status.state = GripperState.ERROR
self._status.error_msg = "旋转夹爪未连接"
raise DHModbusError(self._status.error_msg)
def execute(self, cmd: GripperCommand) -> GripperStatus:
"""执行旋转指令；cmd.target_position 单位为度。"""
return self.rotate_to(
self._last_angle if cmd.target_position is None else float(cmd.target_position),
speed=cmd.target_velocity,
torque=cmd.target_force,
wait_complete=cmd.wait_complete,
timeout=cmd.timeout,
)
def _wait_complete(self, target_angle: float, timeout: float) -> None:
assert self._client is not None
start = time.monotonic()
start_angle = float(self._client.get_rotation_angle_deg())
expected = abs(float(target_angle) - start_angle) + 30.0
settled = 0
last_angle = start_angle
time.sleep(0.05)
while time.monotonic() - start < timeout:
rot_state = self._client.get_rotation_state()
angle = float(self._client.get_rotation_angle_deg())
self._status.rotation_state = rot_state
self._status.rotation_deg = angle
self._status.position = angle
self._status.is_moving = rot_state == 0
if abs(angle - start_angle) > expected:
self._client.hold_current_rotation()
self._status.state = GripperState.ERROR
self._status.error_msg = (
f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止"
)
return
if rot_state in (1, 2):
self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING
if rot_state == 2:
self._status.error_msg = "旋转受阻（blocking）"
return
if rot_state == 3:
self._status.state = GripperState.ERROR
self._status.error_msg = "旋转曾受阻且未到目标位"
return
if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg:
settled += 1
if settled >= 3:
self._status.state = GripperState.IDLE
self._status.is_moving = False
return
elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg:
settled += 1
if settled >= 8 and abs(angle - float(target_angle)) <= 15.0:
self._status.state = GripperState.IDLE
self._status.is_moving = False
return
else:
settled = 0
last_angle = angle
time.sleep(self.cfg.status_poll_interval)
self._client.hold_current_rotation()
self._status.state = GripperState.ERROR
self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止"
def get_status(self) -> GripperStatus:
if not self._connected or self._client is None:
self._status.connected = False
return self._status
try:
finger_pos = self._client.get_position_per_mille()
finger_state = self._client.get_gripper_state()
init_state = self._client.get_init_state()
rot_angle = self._client.get_rotation_angle_deg()
rot_state = self._client.get_rotation_state()
rot_init = self._client.get_rotation_init_state()
self._status.connected = True
self._status.initialized = init_state == 1 and rot_init == 1
self._status.finger_position_per_mille = float(finger_pos)
self._status.rotation_deg = float(rot_angle)
self._status.position = float(rot_angle)
self._status.velocity = float(self._last_speed)
self._status.force = float(self._last_torque)
self._status.gripper_state = finger_state
self._status.rotation_state = rot_state
self._status.is_moving = finger_state == 0 or rot_state == 0
self._status.is_gripped = finger_state == 2
if self._status.state != GripperState.ERROR:
if finger_state == 2 or rot_state == 2:
self._status.state = GripperState.GRIPPING
elif finger_state == 0 or rot_state == 0:
self._status.state = GripperState.MOVING
else:
self._status.state = GripperState.IDLE
self._status.timestamp = time.time()
except Exception as exc:
self._status.state = GripperState.ERROR
self._status.error_msg = str(exc)
self._status.connected = False
return self._status
# -----------------------------
# 手指夹持
# -----------------------------
def move_fingers(self, position_per_mille: int, force: int | None = None,
speed: int | None = None, wait_complete: bool = True,
timeout: float = 10.0) -> GripperStatus:
try:
self._require_client()
except DHModbusError:
return self._status
assert self._client is not None
target = int(np.clip(position_per_mille, 0, 1000))
f = int(np.clip(force or self.cfg.default_finger_force, 20, 100))
s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100))
self._client.set_speed_pct(s)
self._client.set_force_pct(f)
self._client.set_position_per_mille(target)
self._last_finger_per_mille = target
self._last_finger_force = f
self._last_finger_speed = s
if wait_complete:
start = time.monotonic()
time.sleep(0.05)
while time.monotonic() - start < timeout:
state = self._client.get_gripper_state()
pos = self._client.get_position_per_mille()
self._status.gripper_state = state
self._status.finger_position_per_mille = float(pos)
if state in (1, 2):
break
if state == 3:
self._status.state = GripperState.ERROR
self._status.error_msg = "夹持后物体掉落"
break
time.sleep(self.cfg.status_poll_interval)
return self.get_status()
def open_fingers(self, **kwargs) -> GripperStatus:
return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs)
def close_fingers(self, **kwargs) -> GripperStatus:
return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs)
# -----------------------------
# 旋转
# -----------------------------
def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None,
wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus:
try:
self._require_client()
except DHModbusError:
return self._status
assert self._client is not None
angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max))
s = int(np.clip(speed or self.cfg.default_speed, 1, 100))
t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max))
self._client.set_rotation_speed_pct(s)
self._client.set_rotation_force_pct(t)
target = self._client.set_rotation_angle_deg(angle)
self._last_angle = float(target)
self._last_speed = s
self._last_torque = t
self._status.state = GripperState.MOVING
self._status.error_msg = ""
if wait_complete:
self._wait_complete(float(target), timeout)
return self.get_status()
def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus:
current = self.get_status().rotation_deg
return self.rotate_to(current + float(delta_deg), **kwargs)
def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus:
"""旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。"""
remaining = float(revolutions) * 360.0
status = self.get_status()
max_step = abs(float(self.cfg.max_step_deg)) or 30000.0
while abs(remaining) > 1e-6:
before = float(self.get_status().rotation_deg)
step = float(np.clip(remaining, -max_step, max_step))
status = self.rotate_to(before + step, **kwargs)
if status.state == GripperState.ERROR:
return status
after = float(status.rotation_deg)
achieved = after - before
if abs(achieved) < max(1.0, abs(step) * 0.2):
status.state = GripperState.ERROR
status.error_msg = (
"旋转未达到预期步进，可能超出角度寄存器范围或反馈异常："
f"step={step:.3f}, before={before:.3f}, after={after:.3f}"
)
return status
remaining -= achieved
return status
def home_rotation(self, **kwargs) -> GripperStatus:
return self.rotate_to(0.0, **kwargs)
def set_rotation_speed(self, speed_pct: int) -> None:
self._require_client()
assert self._client is not None
self._last_speed = self._client.set_rotation_speed_pct(speed_pct)
def set_rotation_torque(self, torque_pct: int) -> None:
self._require_client()
assert self._client is not None
self._last_torque = self._client.set_rotation_force_pct(torque_pct)
def set_finger_speed(self, speed_pct: int) -> None:
self._require_client()
assert self._client is not None
self._last_finger_speed = self._client.set_speed_pct(speed_pct)
def set_finger_force(self, force_pct: int) -> None:
self._require_client()
assert self._client is not None
self._last_finger_force = self._client.set_force_pct(force_pct)
def open(self, **kwargs) -> GripperStatus:
"""对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。"""
return self.open_fingers(**kwargs)
def close(self, **kwargs) -> GripperStatus:
"""对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。"""
return self.close_fingers(**kwargs)
def stop(self) -> None:
if self._client is not None:
self._client.hold_current_all()
self._status.state = GripperState.IDLE
# =============================================================================
# 双臂夹爪管理器
# =============================================================================
@dataclass
class DualArmGripperConfig:
"""双臂夹爪配置。"""
left_arm_sn: str = "Rizon4-063989"
right_arm_sn: str = "Rizon4-063991"
left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
class DualArmGripperManager:
"""双臂夹爪管理器：统一控制左右臂夹爪。"""
def __init__(self, config: DualArmGripperConfig | None = None) -> None:
self.cfg = config or DualArmGripperConfig()
self._left: ParallelGripper | None = None
self._right: RotaryGripper | None = None
@property
def left(self) -> ParallelGripper:
if self._left is None:
self._left = ParallelGripper(self.cfg.left_gripper)
return self._left
@property
def right(self) -> RotaryGripper:
if self._right is None:
self._right = RotaryGripper(self.cfg.right_gripper)
return self._right
def connect_all(self) -> None:
self.left.connect()
self.right.connect()
def disconnect_all(self) -> None:
if self._left:
self._left.disconnect()
if self._right:
self._right.disconnect()
def initialize_all(self, full: bool = False) -> None:
self.left.initialize(full=full)
self.right.initialize(full=full)
def get_all_status(self) -> dict[str, GripperStatus]:
return {
"left": self.left.get_status(),
"right": self.right.get_status(),
}
def emergency_stop(self) -> None:
self.left.stop()
self.right.stop()
def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
"""从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。"""
import yaml
data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {}
left = data.get("left_arm", {}) or {}
right = data.get("right_arm", {}) or {}
grip = data.get("gripper", {}) or {}
parallel = grip.get("parallel", {}) or {}
rotary = grip.get("rotary", {}) or {}
left_cfg = ParallelGripperConfig(
port=left.get("gripper_port", "/dev/ttyUSB0"),
slave_id=int(left.get("gripper_slave_id", 1)),
baudrate=int(left.get("gripper_baudrate", 115200)),
stroke_min=float(parallel.get("stroke_min", 0.0)),
stroke_max=float(parallel.get("stroke_max", 40.0)),
default_speed=int(parallel.get("default_speed", 50)),
default_force=int(parallel.get("default_force", 50)),
force_min=int(parallel.get("force_min", 20)),
force_max=int(parallel.get("force_max", 100)),
auto_initialize=bool(parallel.get("auto_initialize", True)),
)
right_cfg = RotaryGripperConfig(
port=right.get("gripper_port", "/dev/ttyUSB1"),
slave_id=int(right.get("gripper_slave_id", 1)),
baudrate=int(right.get("gripper_baudrate", 115200)),
angle_min=float(rotary.get("angle_min", -32767.0)),
angle_max=float(rotary.get("angle_max", 32767.0)),
max_step_deg=float(rotary.get("max_step_deg", 30000.0)),
finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)),
finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)),
default_speed=int(rotary.get("default_speed", 50)),
default_torque=int(rotary.get("default_torque", 50)),
default_finger_speed=int(rotary.get("default_finger_speed", 50)),
default_finger_force=int(rotary.get("default_finger_force", 50)),
torque_min=int(rotary.get("torque_min", 20)),
torque_max=int(rotary.get("torque_max", 100)),
auto_initialize=bool(rotary.get("auto_initialize", True)),
)
return DualArmGripperConfig(
left_arm_sn=left.get("robot_sn", "Rizon4-063989"),
right_arm_sn=right.get("robot_sn", "Rizon4-063991"),
left_gripper=left_cfg,
right_gripper=right_cfg,
)
def load_from_site(data: dict) -> DualArmGripperConfig:
"""Build DualArmGripperConfig from ~/ros2/config/site.yaml."""
robots = list((data.get("robot") or {}).get("robots") or [])
left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
if not left:
left = next((r for r in robots if "063989" in str(r.get("sn") or "")), {})
if not right:
right = next((r for r in robots if "063991" in str(r.get("sn") or "")), {})
g = data.get("gripper") or {}
parallel = g.get("parallel") or {}
rotary = g.get("rotary") or {}
lg = left.get("gripper") or {}
rg = right.get("gripper") or {}
stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0)
left_cfg = ParallelGripperConfig(
port=str(lg.get("port") or "/dev/ttyUSB0"),
slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1),
baudrate=int(lg.get("baud") or g.get("baud") or 115200),
stroke_min=float(parallel.get("stroke_min") or 0.0),
stroke_max=stroke,
default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50),
default_force=int(parallel.get("default_force") or g.get("force_pct") or 50),
force_min=int(parallel.get("force_min") or 20),
force_max=int(parallel.get("force_max") or 100),
auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))),
init_timeout=float(parallel.get("init_timeout") or 10.0),
)
right_cfg = RotaryGripperConfig(
port=str(rg.get("port") or "/dev/ttyUSB1"),
slave_id=int(rg.get("slave_id") or 1),
baudrate=int(rg.get("baud") or g.get("baud") or 115200),
angle_min=float(rotary.get("angle_min") or -32767.0),
angle_max=float(rotary.get("angle_max") or 32767.0),
max_step_deg=float(rotary.get("max_step_deg") or 30000.0),
finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000),
finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0),
default_speed=int(rotary.get("default_speed") or 50),
default_torque=int(rotary.get("default_torque") or 50),
default_finger_speed=int(rotary.get("default_finger_speed") or 50),
default_finger_force=int(rotary.get("default_finger_force") or 50),
torque_min=int(rotary.get("torque_min") or 20),
torque_max=int(rotary.get("torque_max") or 100),
auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))),
init_timeout=float(rotary.get("init_timeout") or 15.0),
)
return DualArmGripperConfig(
left_arm_sn=str(left.get("sn") or "Rizon4-063989"),
right_arm_sn=str(right.get("sn") or "Rizon4-063991"),
left_gripper=left_cfg,
right_gripper=right_cfg,
)
src/arm_application/arm_application/dual_gripper_cli.py
+142
View File
@@ -0,0 +1,142 @@
#!/usr/bin/env python3
"""CLI for DH PGEA (left) and RGI (right). Loads ports from site.yaml."""
from __future__ import annotations
import argparse
import os
import sys
import time
from pathlib import Path
import yaml
from arm_application.dual_gripper import DualArmGripperManager, load_from_site
def _site_path() -> Path:
env = os.environ.get("SITE_CONFIG", "").strip()
if env:
return Path(env).expanduser()
return Path.home() / "ros2" / "config" / "site.yaml"
def _print_status(label: str, status) -> None:
print(
f" {label}: state={status.state.name} "
f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
f"gripped={status.is_gripped} init={status.initialized} "
f"err={status.error_msg or '-'}"
)
def _normalize_arm(raw: str) -> str:
t = (raw or "").strip().lower()
if t in ("89", "left", "l", "063989"):
return "left"
if t in ("91", "right", "r", "063991"):
return "right"
if t in ("both", "dual", "all"):
return "both"
raise SystemExit(f"unknown --arm {raw!r} (use 89|91|left|right|both)")
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
gripper = manager.left
print(f"=== PGEA parallel ({gripper.cfg.port}) ===")
gripper.connect()
_print_status("connected", gripper.get_status())
if args.test in ("init", "probe", "all"):
if args.test != "probe":
gripper.initialize(full=args.init_full)
_print_status("init", gripper.get_status())
if args.test in ("open", "all"):
_print_status("open", gripper.open(force=args.force, speed=args.speed))
if args.test in ("close", "all"):
_print_status("close", gripper.close(force=args.force, speed=args.speed))
if args.test in ("move", "pos") and args.pos is not None:
_print_status("move", gripper.move_to(args.pos, force=args.force, speed=args.speed))
if args.test in ("status", "probe"):
_print_status("status", gripper.get_status())
def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
gripper = manager.right
print(f"=== RGI rotary ({gripper.cfg.port}) ===")
gripper.connect()
_print_status("connected", gripper.get_status())
if args.test in ("init", "all"):
gripper.initialize(full=args.init_full)
_print_status("init", gripper.get_status())
if args.test in ("open", "all"):
_print_status("open", gripper.open_fingers(force=args.force, speed=args.speed))
if args.test in ("close", "all"):
_print_status("close", gripper.close_fingers(force=args.force, speed=args.speed))
if args.test in ("move", "pos") and args.pos is not None:
_print_status("fingers", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
if args.test in ("rotate", "rotate_to", "all"):
if args.revolutions is not None:
_print_status(
"revolutions",
gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
)
else:
angle = args.angle if args.angle is not None else 90.0
_print_status("rotate", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
if args.test == "rotate_by":
delta = args.angle if args.angle is not None else 45.0
_print_status("rotate_by", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
if args.test in ("status", "probe"):
_print_status("status", gripper.get_status())
def main() -> int:
parser = argparse.ArgumentParser(description="DH PGEA / RGI CLI (site.yaml ports)")
parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
parser.add_argument(
"--test",
default="status",
help="probe|init|open|close|move|pos|rotate|rotate_by|status",
)
parser.add_argument("--pos", type=float, default=None, help="PGEA mm or RGI finger 0-1000")
parser.add_argument("--angle", type=float, default=None)
parser.add_argument("--revolutions", type=float, default=None)
parser.add_argument("--speed", type=int, default=25)
parser.add_argument("--force", type=int, default=33)
parser.add_argument("--init-full", action="store_true")
parser.add_argument("--site", default="")
args = parser.parse_args()
site = Path(args.site).expanduser() if args.site else _site_path()
if not site.is_file():
print(f"ERROR: site.yaml missing: {site}", file=sys.stderr)
return 1
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
manager = DualArmGripperManager(load_from_site(data))
arm = _normalize_arm(str(args.arm))
print(f"site={site} arm={arm} test={args.test}")
try:
if arm == "left":
test_parallel(manager, args)
elif arm == "right":
test_rotary(manager, args)
else:
test_parallel(manager, args)
test_rotary(manager, args)
except KeyboardInterrupt:
print("interrupt — stop")
manager.emergency_stop()
return 130
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
return 1
finally:
manager.disconnect_all()
time.sleep(0.05)
return 0
if __name__ == "__main__":
raise SystemExit(main())
src/arm_application/arm_application/dual_gripper_node.py
+318
View File
@@ -0,0 +1,318 @@
"""ROS 2 dual DH gripper node (PGEA left + RGI right). Uses arm_msgs, not Flexiv RDK."""
from __future__ import annotations
import os
import threading
from pathlib import Path
import rclpy
import yaml
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from arm_application.dual_gripper import (
DualArmGripperManager,
GripperState,
load_dual_arm_gripper_config,
load_from_site,
)
from arm_msgs.msg import GripperCommand as GripperCommandMsg
from arm_msgs.msg import GripperStatus as GripperStatusMsg
from arm_msgs.srv import GripperExecute
def _site_path() -> Path:
env = os.environ.get("SITE_CONFIG", "").strip()
if env:
return Path(env).expanduser()
return Path.home() / "ros2" / "config" / "site.yaml"
def _as_bool(val, default: bool = True) -> bool:
if val is None:
return default
if isinstance(val, str):
return val.strip().lower() in ("1", "true", "yes", "on")
return bool(val)
class DualGripperNode(Node):
def __init__(self) -> None:
super().__init__("dual_gripper_driver")
self.declare_parameter("config_file", "")
self.declare_parameter("auto_connect", True)
self.declare_parameter("allow_offline", True)
self.declare_parameter("publish_hz", 10.0)
self.declare_parameter("sides", "left,right")
self._cb_group = ReentrantCallbackGroup()
self._locks = {"left": threading.Lock(), "right": threading.Lock()}
sides_raw = str(self.get_parameter("sides").value or "left,right")
self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
if not self._sides:
self._sides = ["left", "right"]
cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
if cfg_path:
self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
else:
site = _site_path()
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
self._manager = DualArmGripperManager(load_from_site(data))
self.get_logger().info(f"gripper config from site.yaml: {site}")
self._connect_errors: dict[str, str] = {}
if _as_bool(self.get_parameter("auto_connect").value, True):
self._connect_all()
self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
if "left" in self._sides:
self.create_subscription(
GripperCommandMsg,
"~/left/command",
lambda msg: self._on_command("left", msg),
10,
callback_group=self._cb_group,
)
self.create_service(
GripperExecute,
"~/left/execute",
lambda req, resp: self._on_execute("left", req, resp),
callback_group=self._cb_group,
)
if "right" in self._sides:
self.create_subscription(
GripperCommandMsg,
"~/right/command",
lambda msg: self._on_command("right", msg),
10,
callback_group=self._cb_group,
)
self.create_service(
GripperExecute,
"~/right/execute",
lambda req, resp: self._on_execute("right", req, resp),
callback_group=self._cb_group,
)
period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
self.create_timer(period, self._publish_status, callback_group=self._cb_group)
self.get_logger().info(
f"dual_gripper_driver ready sides={self._sides} "
"~/left|right/command status execute"
)
def _connect_all(self) -> None:
mapping = (("left", self._manager.left), ("right", self._manager.right))
for side, gripper in mapping:
if side not in self._sides:
continue
try:
gripper.connect()
self.get_logger().info(f"{side} gripper connected: {gripper.cfg.port}")
except Exception as exc:
self._connect_errors[side] = str(exc)
self.get_logger().error(f"{side} gripper connect failed: {exc}")
if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
raise RuntimeError(f"gripper connect failed: {self._connect_errors}")
def _gripper(self, side: str):
return self._manager.left if side == "left" else self._manager.right
def _execute(self, side: str, msg: GripperCommandMsg):
gripper = self._gripper(side)
command = (msg.command or "").strip().lower()
timeout = float(msg.timeout or 10.0)
wait = bool(msg.wait_complete)
if side in self._connect_errors and not gripper.is_connected():
status = gripper.get_status()
status.error_msg = self._connect_errors[side]
return status
if command == "connect":
gripper.connect()
self._connect_errors.pop(side, None)
return gripper.get_status()
if command == "init":
gripper.initialize(full=False)
return gripper.get_status()
if command == "init_full":
gripper.initialize(full=True)
return gripper.get_status()
if command == "stop":
gripper.stop()
return gripper.get_status()
if side == "left":
if command == "open":
return gripper.open(
force=int(msg.force) if msg.force else None,
speed=int(msg.speed) if msg.speed else None,
wait_complete=wait,
timeout=timeout,
)
if command == "close":
return gripper.close(
force=int(msg.force) if msg.force else None,
speed=int(msg.speed) if msg.speed else None,
wait_complete=wait,
timeout=timeout,
)
if command == "move":
return gripper.move_to(
float(msg.position),
force=int(msg.force) if msg.force else None,
speed=int(msg.speed) if msg.speed else None,
wait_complete=wait,
timeout=timeout,
)
if command == "set_force":
gripper.set_force(int(msg.force))
return gripper.get_status()
if command == "set_speed":
gripper.set_speed(int(msg.speed))
return gripper.get_status()
else:
if command == "open":
return gripper.open_fingers(
force=int(msg.force) if msg.force else None,
speed=int(msg.speed) if msg.speed else None,
wait_complete=wait,
timeout=timeout,
)
if command == "close":
return gripper.close_fingers(
force=int(msg.force) if msg.force else None,
speed=int(msg.speed) if msg.speed else None,
wait_complete=wait,
timeout=timeout,
)
if command == "move":
return gripper.move_fingers(
int(msg.position),
force=int(msg.force) if msg.force else None,
speed=int(msg.speed) if msg.speed else None,
wait_complete=wait,
timeout=timeout,
)
if command in ("rotate", "rotate_to"):
return gripper.rotate_to(
float(msg.position),
speed=int(msg.speed) if msg.speed else None,
torque=int(msg.force) if msg.force else None,
wait_complete=wait,
timeout=timeout,
)
if command == "rotate_by":
return gripper.rotate_by(
float(msg.position),
speed=int(msg.speed) if msg.speed else None,
torque=int(msg.force) if msg.force else None,
wait_complete=wait,
timeout=timeout,
)
if command == "rotate_revolutions":
return gripper.rotate_revolutions(
float(msg.position),
speed=int(msg.speed) if msg.speed else None,
torque=int(msg.force) if msg.force else None,
wait_complete=wait,
timeout=timeout,
)
if command == "home_rotation":
return gripper.home_rotation(
speed=int(msg.speed) if msg.speed else None,
torque=int(msg.force) if msg.force else None,
wait_complete=wait,
timeout=timeout,
)
if command == "set_force":
gripper.set_rotation_torque(int(msg.force))
return gripper.get_status()
if command == "set_speed":
gripper.set_rotation_speed(int(msg.speed))
return gripper.get_status()
if command == "set_finger_force":
gripper.set_finger_force(int(msg.force))
return gripper.get_status()
if command == "set_finger_speed":
gripper.set_finger_speed(int(msg.speed))
return gripper.get_status()
status = gripper.get_status()
status.state = GripperState.ERROR
status.error_msg = f"unsupported command {command!r} (side={side})"
return status
def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
with self._locks[side]:
status = self._execute(side, msg)
if status.state == GripperState.ERROR:
self.get_logger().warn(f"{side} command {msg.command!r} failed: {status.error_msg}")
def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
with self._locks[side]:
status = self._execute(side, request.command)
response.success = status.state != GripperState.ERROR and status.connected
response.message = status.error_msg or "OK"
response.status = self._to_status_msg(side, status)
return response
def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
gripper = self._gripper(side)
msg = GripperStatusMsg()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = f"{side}_gripper"
msg.name = gripper.name
msg.gripper_type = gripper.gripper_type.name.lower()
msg.state = status.state.name
msg.connected = bool(status.connected)
msg.initialized = bool(status.initialized)
msg.is_moving = bool(status.is_moving)
msg.is_gripped = bool(status.is_gripped)
msg.position = float(status.position)
msg.finger_position_per_mille = float(status.finger_position_per_mille)
msg.rotation_deg = float(status.rotation_deg)
msg.gripper_state = int(status.gripper_state)
msg.rotation_state = int(status.rotation_state)
msg.velocity = float(status.velocity)
msg.force = float(status.force)
msg.error_msg = status.error_msg or ""
return msg
def _publish_status(self) -> None:
if "left" in self._sides:
left_status = self._manager.left.get_status()
if "left" in self._connect_errors and not left_status.connected:
left_status.error_msg = self._connect_errors["left"]
self._left_pub.publish(self._to_status_msg("left", left_status))
if "right" in self._sides:
right_status = self._manager.right.get_status()
if "right" in self._connect_errors and not right_status.connected:
right_status.error_msg = self._connect_errors["right"]
self._right_pub.publish(self._to_status_msg("right", right_status))
def destroy_node(self) -> bool:
try:
self._manager.disconnect_all()
finally:
return super().destroy_node()
def main() -> None:
rclpy.init()
node = DualGripperNode()
executor = MultiThreadedExecutor(num_threads=4)
try:
rclpy.spin(node, executor=executor)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
src/arm_application/arm_application/geom_ik.py
+135
View File
@@ -0,0 +1,135 @@
#!/usr/bin/env python3
"""TF-based geometric Jacobian + damped least-squares position IK step."""
from __future__ import annotations
from typing import List, Optional, Sequence, Tuple
import numpy as np
# URDF axes expressed in each child link frame (Rizon4).
RIZON_LINK_AXES = [
(0.0, 0.0, 1.0), # joint1 -> link1
(0.0, 1.0, 0.0), # joint2 -> link2
(0.0, 0.0, 1.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
]
def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
x, y, z, w = [float(v) for v in q]
n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
x, y, z, w = x / n, y / n, z / n, w / n
return np.array(
[
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
],
dtype=float,
)
def geometric_jacobian(
link_R: Sequence[np.ndarray],
link_p: Sequence[np.ndarray],
ee_p: np.ndarray,
axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
) -> np.ndarray:
"""Return 6x7 geometric Jacobian in base frame (linear then angular)."""
n = len(axes_local)
J = np.zeros((6, n), dtype=float)
pee = np.asarray(ee_p, dtype=float).reshape(3)
for i in range(n):
R = np.asarray(link_R[i], dtype=float).reshape(3, 3)
p = np.asarray(link_p[i], dtype=float).reshape(3)
z = R @ np.asarray(axes_local[i], dtype=float).reshape(3)
J[:3, i] = np.cross(z, pee - p)
J[3:, i] = z
return J
def position_jacobian(
link_R: Sequence[np.ndarray],
link_p: Sequence[np.ndarray],
ee_p: np.ndarray,
axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
) -> np.ndarray:
"""Return 3x7 translational Jacobian in base frame."""
return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :]
def dls_position_step(
J: np.ndarray,
dp_base: np.ndarray,
*,
damping: float = 0.05,
max_dq: float = 0.05,
) -> np.ndarray:
"""Solve J dq = dp with damped least squares; clip |dq|."""
dp = np.asarray(dp_base, dtype=float).reshape(3)
JT = J.T
A = J @ JT + (damping ** 2) * np.eye(3)
dq = JT @ np.linalg.solve(A, dp)
dq = np.clip(dq, -abs(max_dq), abs(max_dq))
return dq
def dls_twist(
J6: np.ndarray,
v_base: np.ndarray,
*,
w_base: Optional[np.ndarray] = None,
damping: float = 0.08,
max_qdot: float = 0.80,
ori_weight: float = 1.2,
) -> np.ndarray:
"""Solve J qdot = [v; w] with orientation held (w≈0 by default)."""
dx = np.zeros(6, dtype=float)
dx[:3] = np.asarray(v_base, dtype=float).reshape(3)
if w_base is not None:
dx[3:] = np.asarray(w_base, dtype=float).reshape(3)
w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
Jw = J6 * w[:, None]
dxw = dx * w
JT = Jw.T
A = Jw @ JT + (damping ** 2) * np.eye(6)
qdot = JT @ np.linalg.solve(A, dxw)
return np.clip(qdot, -abs(max_qdot), abs(max_qdot))
def dls_translate_hold_ori(
J6: np.ndarray,
dp_base: np.ndarray,
*,
damping: float = 0.08,
max_dq: float = 0.05,
ori_weight: float = 1.2,
) -> np.ndarray:
"""Translate EE by dp while keeping orientation (wx,wy,wz ≈ 0)."""
dx = np.zeros(6, dtype=float)
dx[:3] = np.asarray(dp_base, dtype=float).reshape(3)
w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
Jw = J6 * w[:, None]
dxw = dx * w
JT = Jw.T
A = Jw @ JT + (damping ** 2) * np.eye(6)
dq = JT @ np.linalg.solve(A, dxw)
return np.clip(dq, -abs(max_dq), abs(max_dq))
def load_handeye_T(path: str) -> np.ndarray:
"""Load 4x4 from handeye_result.yaml (T_4x4 list)."""
import yaml
with open(path, 'r', encoding='utf-8') as f:
data = yaml.safe_load(f)
if not isinstance(data, dict) or 'T_4x4' not in data:
raise ValueError(f'No T_4x4 in hand-eye file: {path}')
T = np.array(data['T_4x4'], dtype=float)
if T.shape != (4, 4):
raise ValueError(f'T_4x4 shape {T.shape}, expected (4,4)')
return T
src/arm_application/arm_application/goto_ee_plane.py
+451
View File
@@ -0,0 +1,451 @@
#!/usr/bin/env python3
"""Startup return-to-initial before perception/servo starts.
Sequence:
[A] if the flange is below servo.safe_z_m, lift it STRAIGHT UP first
(closed-loop Z hops) so the home move can't sweep walls/fixtures;
[B] call /set_named_pose (Flexiv named pose, default "home") — the arm
returns to the Flexiv initial pose with Flexiv's own planner;
[C] hold (pin) so the servo takes over from a clean, stationary pose.
"""
from __future__ import annotations
import sys
import time
from pathlib import Path
from typing import List, Optional, Tuple
import numpy as np
import rclpy
from arm_msgs.srv import SetNamedPose
from control_msgs.action import FollowJointTrajectory
from rclpy.action import ActionClient
from rclpy.node import Node
from sensor_msgs.msg import JointState
from tf2_ros import Buffer, TransformListener
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from arm_application.geom_ik import (
RIZON_LINK_AXES,
dls_translate_hold_ori,
dls_twist,
geometric_jacobian,
quat_xyzw_to_R,
)
def _load_ee_cfg() -> dict:
root = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(root / "scripts" / "lib"))
from export_site_env import load_site, resolve_robot # type: ignore
data = load_site()
servo = (data.get("servo") or {})
robot = resolve_robot(data)
ee_z = float(servo.get("ee_z_m") or 0.24)
return {
"prefix": str(robot.get("joint_prefix") or "Rizon4-063989_"),
"base": str(robot.get("base_frame") or "Rizon4-063989_base_link"),
"ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"),
"z": ee_z,
# Lift straight up to this Z before the home move (avoid hitting
# walls/fixtures); then return to the Flexiv named initial pose.
"safe_z": float(servo.get("safe_z_m") or max(0.40, ee_z + 0.10)),
"home_pose": str(servo.get("home_pose_name") or "home"),
"home_speed": float(servo.get("home_pose_speed") or 0.30),
"v_z": float(servo.get("v_max_z") or 0.04),
# only used by the (currently unused) verticalize planner path
"kp": float(servo.get("ee_hold_kp") or 2.0),
"w_max": float(servo.get("ee_w_max") or 0.50),
}
def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
pt = JointTrajectoryPoint()
pt.positions = [float(v) for v in q.tolist()]
pt.velocities = [float(v) for v in qdot.tolist()]
pt.accelerations = [0.0] * int(q.size)
ns = int(round(max(0.0, float(t_sec)) * 1e9))
pt.time_from_start.sec = ns // 1_000_000_000
pt.time_from_start.nanosec = ns % 1_000_000_000
return pt
class GotoEePlane(Node):
def __init__(self, cfg: dict) -> None:
super().__init__("goto_ee_plane")
self.cfg = cfg
prefix = cfg["prefix"]
self.joint_names = [f"{prefix}joint{i}" for i in range(1, 8)]
self.link_frames = [f"{prefix}link{i}" for i in range(1, 8)]
self._js: Optional[JointState] = None
self.create_subscription(JointState, "joint_states", self._on_js, 10)
self._tf = Buffer()
TransformListener(self._tf, self, spin_thread=True)
self._cli = ActionClient(
self, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory"
)
def _on_js(self, msg: JointState) -> None:
self._js = msg
def _spin(self, dt: float = 0.05) -> None:
rclpy.spin_once(self, timeout_sec=dt)
def _lookup(self, parent: str, child: str) -> Optional[np.ndarray]:
try:
tf = self._tf.lookup_transform(
parent, child, rclpy.time.Time(),
timeout=rclpy.duration.Duration(nanoseconds=80_000_000),
)
except Exception:
return None
t = tf.transform.translation
o = tf.transform.rotation
T = np.eye(4)
T[:3, :3] = quat_xyzw_to_R([o.x, o.y, o.z, o.w])
T[:3, 3] = [t.x, t.y, t.z]
return T
def _q(self) -> Optional[np.ndarray]:
if self._js is None:
return None
m = {n: p for n, p in zip(self._js.name, self._js.position)}
try:
return np.array([float(m[n]) for n in self.joint_names], dtype=float)
except KeyError:
return None
def _jacobian(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
link_R: List[np.ndarray] = []
link_p: List[np.ndarray] = []
for fr in self.link_frames:
Tl = self._lookup(self.cfg["base"], fr)
if Tl is None:
return None
link_R.append(Tl[:3, :3])
link_p.append(Tl[:3, 3])
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
z_now = R[:, 2]
z_des = np.array([0.0, 0.0, -1.0])
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
ang = float(np.arccos(c))
if ang < 0.02:
return np.zeros(3), ang
axis = np.cross(z_now, z_des)
n = float(np.linalg.norm(axis))
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
mag = float(np.clip(self.cfg["kp"] * ang, -self.cfg["w_max"], self.cfg["w_max"]))
return axis * mag, ang
def _target_err(
self, p: np.ndarray, R: np.ndarray, target: np.ndarray, verticalize: bool
) -> Tuple[np.ndarray, float, float]:
dp = np.asarray(target, dtype=float).reshape(3) - np.asarray(p, dtype=float).reshape(3)
_, ang = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
xy = float(np.hypot(dp[0], dp[1]))
return dp, ang, xy
def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
t0 = time.monotonic()
q = None
Tee = None
while time.monotonic() - t0 < timeout_s:
self._spin(0.05)
q = self._q()
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
if q is not None and Tee is not None and self._jacobian(Tee[:3, 3]) is not None:
return q, Tee
return q, Tee
def _send_action(self, traj: JointTrajectory, wait_s: float) -> bool:
goal = FollowJointTrajectory.Goal()
goal.trajectory = traj
fut = self._cli.send_goal_async(goal)
t0 = time.monotonic()
while not fut.done() and time.monotonic() - t0 < 4.0:
self._spin(0.05)
handle = fut.result() if fut.done() else None
if handle is None or not handle.accepted:
print(" ERROR: JTC rejected the height trajectory", flush=True)
return False
res = handle.get_result_async()
t0 = time.monotonic()
last = 0.0
while not res.done() and time.monotonic() - t0 < wait_s:
self._spin(0.05)
now = time.monotonic()
if now - last > 0.4:
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
if Tee is not None:
p = Tee[:3, 3]
print(
f" moving ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f}) "
f"z_err={(self.cfg['z'] - p[2]) * 1000:+.0f}mm",
flush=True,
)
last = now
if not res.done():
print(" WARN: JTC result timeout — checking TF anyway", flush=True)
return False
return True
def _plan_and_send(
self,
q0: np.ndarray,
Tee: np.ndarray,
target: np.ndarray,
verticalize: bool,
tag: str = "",
max_seg: float = 0.08,
v: float = 0.05,
) -> bool:
"""Plan and execute ONE short cartesian segment (≤ max_seg) toward
target. Long moves are handled by the caller in closed-loop hops,
because the Jacobian is frozen at the current pose — over >10 cm the
linearization error makes single-shot plans land far off target."""
p = Tee[:3, 3].copy()
R = Tee[:3, :3]
dp, ang, _ = self._target_err(p, R, target, verticalize)
dist = float(np.linalg.norm(dp))
if dist > max_seg: # clamp this hop; caller loops with fresh TF
dp = dp * (max_seg / dist)
dist = max_seg
print(
f" plan {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm "
f"tilt={ang * 180 / np.pi:.1f}deg",
flush=True,
)
J = self._jacobian(p)
if J is None:
print("ERROR: missing link TF for Jacobian", flush=True)
return False
q = q0.copy()
waypoints = [q.copy()]
remain = dp.copy()
w_step, _ = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
n_max = int(np.ceil(dist / 0.012)) + 4
for _ in range(n_max):
if float(np.linalg.norm(remain)) < 0.0015 and (
(not verticalize) or ang < 0.03
):
break
nseg = max(1, int(np.ceil(float(np.linalg.norm(remain)) / 0.012)))
step = remain / nseg
if verticalize and ang > 0.03:
qdot = dls_twist(
J, step / 0.12, w_base=w_step, damping=0.08, max_qdot=0.45
)
dq = np.clip(qdot * 0.12, -0.06, 0.06)
else:
dq = dls_translate_hold_ori(J, step, damping=0.08, max_dq=0.06)
if float(np.max(np.abs(dq))) < 1e-5:
break
q = q + dq
waypoints.append(q.copy())
remain = remain - (J[:3, :] @ dq)
ang = max(0.0, ang - 0.05)
planned = dist - float(np.linalg.norm(remain))
if len(waypoints) < 2 or planned < 0.4 * dist:
print(
f" WARN {tag}: planner stalled "
f"(planned {planned*1000:.0f}/{dist*1000:.0f}mm) — skip hop",
flush=True,
)
return False
# joint-excursion sanity gate: refuse wild plans
if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0:
print(f" ERROR {tag}: joint excursion >1 rad — abort hop", flush=True)
return False
duration = max(1.6, dist / max(min(v, 0.06), 1e-3))
n = len(waypoints) - 1
traj = JointTrajectory()
traj.header.stamp.sec = 0
traj.header.stamp.nanosec = 0
traj.joint_names = list(self.joint_names)
traj.points.append(_pt(waypoints[0], np.zeros(7), 0.08))
zeros = np.zeros(7)
for i, qi in enumerate(waypoints[1:], start=1):
t = 0.08 + duration * (i / n)
qdot = zeros if i == n else (waypoints[min(i + 1, n)] - qi) / max(
duration / n, 0.05
)
qdot = np.clip(qdot, -0.40, 0.40)
traj.points.append(_pt(qi, qdot, t))
# force zero velocity at the last point
traj.points[-1].velocities = [0.0] * 7
print(
f" send JTC action {tag} {n} steps T={duration:.1f}s",
flush=True,
)
return self._send_action(traj, wait_s=duration + 6.0)
def _goto(
self,
q: np.ndarray,
Tee: np.ndarray,
target: np.ndarray,
verticalize: bool,
tol_m: float,
tol_tilt: float,
tag: str,
v: float,
max_hops: int = 8,
) -> Tuple[bool, np.ndarray, np.ndarray]:
"""Closed-loop point-to-point: plan ≤8 cm hop → execute → re-measure
TF → repeat, until within tolerance. Robust to Jacobian linearization
error over long distances."""
for hop in range(max_hops):
p = Tee[:3, 3]
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
dist = float(np.linalg.norm(dp))
if dist < tol_m and ((not verticalize) or ang < tol_tilt):
return True, q, Tee
print(
f" {tag} hop{hop + 1} remain={dist*1000:.0f}mm "
f"tilt={ang * 180 / np.pi:.1f}deg",
flush=True,
)
if not self._plan_and_send(q, Tee, target, verticalize, tag, v=v):
return False, q, Tee
t_end = time.monotonic() + 0.5
while time.monotonic() < t_end:
self._spin(0.05)
q2, Tee2 = self._wait_state(2.0)
if q2 is None or Tee2 is None:
print("ERROR: lost state during move", flush=True)
return False, q, Tee
q, Tee = q2, Tee2
p = Tee[:3, 3]
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
ok = float(np.linalg.norm(dp)) < (tol_m * 2.0) and (
(not verticalize) or ang < (tol_tilt * 2.0)
)
return ok, q, Tee
def _pin(self, q: np.ndarray) -> None:
if not self._cli.wait_for_server(timeout_sec=3.0):
return
traj = JointTrajectory()
traj.header.stamp.sec = 0
traj.header.stamp.nanosec = 0
traj.joint_names = list(self.joint_names)
traj.points = [_pt(q, np.zeros(7), 0.08), _pt(q, np.zeros(7), 0.45)]
self._send_action(traj, wait_s=3.0)
def _named_pose(self, name: str, speed: float) -> bool:
"""Call Flexiv /set_named_pose and wait until the arm is stationary."""
cli = self.create_client(SetNamedPose, "/set_named_pose")
if not cli.wait_for_server(timeout_sec=8.0):
print("ERROR: /set_named_pose unavailable — start run_real.sh", flush=True)
return False
req = SetNamedPose.Request()
req.pose_name = name
req.velocity_scale = float(min(max(speed, 0.05), 1.0))
req.acceleration_scale = 0.0
print(f" [B] set_named_pose '{name}' speed={req.velocity_scale:.2f}", flush=True)
fut = cli.call_async(req)
t0 = time.monotonic()
while not fut.done() and time.monotonic() - t0 < 90.0:
self._spin(0.1)
if not fut.done():
print("ERROR: set_named_pose timeout", flush=True)
return False
resp = fut.result()
print(
f" [B] resp success={resp.success} msg={resp.message}",
flush=True,
)
if not resp.success:
return False
# wait until the flange stops moving (Δp < 1 mm over ~0.8 s)
last = None
still = 0.0
t0 = time.monotonic()
while time.monotonic() - t0 < 30.0:
self._spin(0.2)
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
if Tee is not None:
p = Tee[:3, 3]
if last is not None and float(np.linalg.norm(p - last)) < 0.001:
still += 0.2
if still >= 0.8:
return True
else:
still = 0.0
last = p.copy()
print(" WARN: still-moving wait timeout — continue anyway", flush=True)
return True
def run(self, timeout_s: float = 25.0) -> int:
cfg = self.cfg
q, Tee = self._wait_state(4.0)
if q is None or Tee is None:
print("ERROR: no joint_states / TF — start run_real.sh", flush=True)
return 2
if not self._cli.wait_for_server(timeout_sec=8.0):
print("ERROR: JTC action unavailable — start run_real.sh", flush=True)
return 3
p = Tee[:3, 3]
safe_z = float(cfg["safe_z"])
print(
f"=== return to Flexiv initial pose '{cfg['home_pose']}' "
f"safe_z={safe_z*1000:.0f}mm ===",
flush=True,
)
print(
f" now ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
flush=True,
)
# --- Phase A: pure-Z lift to the safe height before the home move ---
if p[2] < safe_z - 0.01:
lift_tgt = np.array([p[0], p[1], safe_z], dtype=float)
print(f" [A] lift Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True)
ok, q, Tee = self._goto(
q, Tee, lift_tgt, False, 0.010, 0.0, "[A]lift", v=0.05
)
if not ok:
print("ERROR: lift phase failed — pin here", flush=True)
self._pin(q)
return 4
# --- Phase B: Flexiv named pose → real initial pose, good posture ---
if not self._named_pose(cfg["home_pose"], cfg["home_speed"]):
print("ERROR: named-pose return failed — pin here", flush=True)
self._pin(q)
return 4
# --- Phase C: hold so the servo takes over from a stationary pose ---
q, Tee = self._wait_state(2.0)
if q is not None:
self._pin(q)
if Tee is not None:
p = Tee[:3, 3]
print(
f" at initial pose ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
flush=True,
)
return 0
def main() -> int:
cfg = _load_ee_cfg()
rclpy.init()
node = GotoEePlane(cfg)
try:
return node.run()
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
raise SystemExit(main())

Some files were not shown because too many files have changed in this diff Show More