同构臂遥操作系统

Feetech 舵机 Leader 臂 → Gen72-B Follower 臂（含 Lebai LMG-90 夹爪）的实时遥操作。

---

硬件配置

角色	设备	连接方式
Leader 臂	Feetech 舵机臂（7 关节 + 夹爪舵机）	USB → 本机
Follower 臂	睿曼 Gen72-B	TCP 192.168.1.19:8080
Follower 夹爪	乐白 Lebai LMG-90	Gen72 工具端 RS485（115200bps，Modbus RTU）

Gen72 网页配置要求：末端外设设备 → RM_ARM+ 协议 → 打开，115200bps

---

文件结构

teleop/
├── teleop.py          # 主程序（启动遥操作）
├── joint_mapping.py   # 全部可调参数（标定值、IP、夹爪）
├── check_leader.py    # 诊断工具：实时查看 Leader 臂 J1-J8 角度
└── README.md

config.toml（位于 ~/DorobotTool/）控制 Leader 臂采样频率。

---

系统架构

Leader 臂（Feetech 舵机）
    │  USB
    ▼
DorobotTool（Rust 守护进程）
    │  Zenoh 发布，publish_hz（默认 200Hz）
    │  topic: dora/v1/std/iot/dual_arms/joint_state
    │  格式: bincode <u64, [f32;8], [f32;8]>（timestamp + left + right）
    ▼
teleop.py（Python）
    ├── Zenoh 回调：解析 J1-J7 关节弧度 + J8 夹爪角度 → 映射到 Follower 目标值
    └── 200Hz 发送线程
            ├── rm_movej_canfd → Gen72 关节控制（7 轴）
            └── rm_write_single_register（每 20 帧一次，≈10Hz）→ 夹爪

---

快速启动

前提

1. Leader 臂 USB 已连接，DorobotTool 数据库中已记录设备（名称 mainarmtest）
2. Gen72 已开机，IP 192.168.1.19，RM_ARM+ 协议已启用

步骤

终端 1：启动 DorobotTool

cd ~/DorobotTool
RUST_LOG=info ./target/release/dorobot start

等待日志出现 Transitioning from Idle -> Active。

终端 2：启动遥操作

conda activate pico
cd ~/DorobotTool/teleop
python teleop.py

启动后约 1-2 秒，Follower 臂平滑追上 Leader 臂当前位置，随后实时跟随。

停止

任意终端按 Ctrl+C。

---

诊断工具

查看 Leader 臂实时角度（用于标定、确认数据流）：

conda activate pico
python ~/DorobotTool/teleop/check_leader.py

显示 J1-J8 的实时角度（度），J8 为夹爪舵机。若数值不更新，说明 DorobotTool 未运行或 USB 未连接。

---

可调参数

延迟 / 响应速度

参数	文件	默认值	说明
MAX_VEL_DEG_PER_SEC	teleop.py 第 64 行	300.0	Follower 追赶速度上限（°/s）。越大延迟越低、启动越快；越小启动越平滑
publish_hz	config.toml	200	Leader 臂采样频率。修改后需重新编译并重启 DorobotTool

publish_hz 修改后重编命令：

pkill -f dorobot && sleep 2
cd ~/DorobotTool && cargo build --release
RUST_LOG=info ./target/release/dorobot start

连接地址

在 joint_mapping.py 中修改：

GEN72_IP   = "192.168.1.19"   # Follower 臂 IP
GEN72_PORT = 8080              # Follower 臂端口

关节映射（joint_mapping.py）

JOINT_OFFSETS_RAD — 零点偏置

使 Leader 臂竖直时对应 Gen72 零位（竖直）。

JOINT_OFFSETS_RAD = [0.0052, -0.0209, 0.0332, -2.8483, 0.0, -1.4835, -0.0332]

重新标定方法：

1. 将 Leader 臂摆到与 Gen72 竖直姿态对应的位置
2. 运行 check_leader.py，记录 J1-J7 读数（度）
3. JOINT_OFFSETS_RAD[i] = -deg2rad(读数[i])

JOINT_SCALES — 方向与比例

符号控制方向（不可随意改变），绝对值控制比例。

JOINT_SCALES = [-1.0, -1.1798, -1.0, 1.0417, -1.0, -0.9252, -1.5478]

• J1/J3/J5：±1.0，1:1 映射（Leader 转多少度 Follower 转多少度）
• J2/J7：按范围比例缩放（Leader 全程对应 Follower 全程）
• J4/J6：由实测标定点两点拟合

重新标定某关节（两点拟合，以 J4 为例）：

1. 测量两个对应点：(leader_A°, follower_A°) 和 (leader_B°, follower_B°)
2. 在 Python 中计算（保留原符号）：

   import math
   # 两点拟合
   scale = (math.radians(follower_B) - math.radians(follower_A)) \
           / (math.radians(leader_B) - math.radians(leader_A))
   offset = math.radians(follower_A) / scale - math.radians(leader_A)
   # scale 符号应与原值相同，若相反则说明标定点选错了

JOINT_LIMITS_RAD — 硬件限位

Gen72-B 各关节实际可达范围（弧度），超出时自动截断，防止触发保护模式。

JOINT_LIMITS_RAD = [
    (-3.1067, 3.1067),   # J1 ±178°
    (-1.8326, 1.8326),   # J2 ±105°
    (-3.1067, 3.1067),   # J3 ±178°
    (-3.1067, 0.9599),   # J4 -178° ~ +55°
    (-3.1067, 3.1067),   # J5 ±178°
    (-1.5708, 2.0944),   # J6 -90° ~ +120°（实测）
    (-3.1067, 3.1067),   # J7 ±178°
]

夹爪映射（joint_mapping.py）

GRIPPER_ZERO_DEG = 46.0              # Leader J8 对应夹爪全关时的角度（°）
GRIPPER_SCALE    = 100.0 / (46.0 - 3.2)  # ≈ 2.3364

映射公式：follower_gripper = clip(GRIPPER_SCALE × (GRIPPER_ZERO_DEG - leader_j8°), 0, 100)

重新标定：找到两个对应点：

• 点 A：Leader J8 在某角度时夹爪全关（follower=0）
• 点 B：Leader J8 在另一角度时夹爪全开（follower=100）

GRIPPER_ZERO_DEG = A          # 全关时 Leader J8 角度（°）
GRIPPER_SCALE    = 100.0 / abs(A - B)
# 若方向反了，在 teleop.py 中把公式中 (GRIPPER_ZERO_DEG - j8_deg) 改为 (j8_deg - GRIPPER_ZERO_DEG)

---

常见问题

Follower 臂没有响应

1. 确认 DorobotTool 正在运行（check_leader.py 有数据更新）
2. 确认 Gen72 IP 正确（joint_mapping.py 中 GEN72_IP）
3. 确认 Gen72 已开机并可 ping 通

启动时 Follower 臂快速移动

正常现象：速率限制器从 Follower 臂当前位置出发追赶 Leader 臂，约 1-2 秒完成。 若觉得太快可降低 MAX_VEL_DEG_PER_SEC。

延迟过高

• 提高 MAX_VEL_DEG_PER_SEC（只改 teleop.py，重启脚本生效）
• 提高 publish_hz（改 config.toml，需重编 DorobotTool）

Follower 臂断线（ret=-2）

脚本自动等待 15 秒后重连。若频繁断线，检查：

• Gen72 J6 是否超出 -90°~+120° 范围（可能触发硬件保护）
• 网络稳定性

夹爪无响应

1. 确认 Gen72 网页中 RM_ARM+ 协议已开启（115200bps）
2. 确认 RS485 线缆已连接至 Gen72 工具端口
3. 确认 Gen72 工具端已输出 24V（启动时自动设置）

DorobotTool 启动提示 "Already running"

pkill -f dorobot && sleep 2
cd ~/DorobotTool && RUST_LOG=info ./target/release/dorobot start