GR-EndEffector：二指夹爪

1. 概述

GR-EndEffector 的运行时程序是 EndEffectorCore。它通过 DDS 接收末端执行器命令， 再通过 Modbus-RTU 控制二指平行夹爪，并将夹爪状态重新发布到 DDS。

PGC 每侧被建模为一个自由度（DOF=1）：一个位置值表示夹板目标位置。左右夹爪通常分别 连接到独立的 USB-RS485 适配器；单夹爪和双夹爪配置均受支持。

项目	说明
运行时程序	EndEffectorCore
DDS Domain ID	123
设备厂商/型号	dh_robotics / PGC
传输协议	Modbus-RTU over RS485
每侧自由度	1
命令 group	left_end_effector、right_end_effector
推荐硬件命令/状态轮询频率	20 Hz

2. 数据链路

Text

PGC 夹爪
  <-> USB-RS485 适配器
  <-> EndEffectorCore（Modbus-RTU）
  <-> end_effector_state

DDS 客户端
  -> end_effector_cmd
  -> EndEffectorCore
  -> PGC 动作

使用 DDS 案例前，必须先启动并保持 EndEffectorCore 运行。每个 USB-RS485 适配器只能由一个进程访问；修改串口配置或重新启动运行时前，应先停止正在 使用对应串口的程序。

运行环境

EndEffectorCore 在机器人上以系统用户运行。本页的 DDS 客户端脚本以普通用户运行即可，无需 root/sudo；但必须在创建任何 DDS participant 前设置 FASTDDS_BUILTIN_TRANSPORTS=UDPv4（否则同机共享内存传输会收不到数据或数据间歇性丢失）。下文 Python 示例已内置该设置。

3. 硬件与配置

3.1 串口参数

PGC 的默认通信参数如下：

参数	值
波特率	115200
数据位	8
停止位	1
校验位	N（无校验）
Modbus slave ID	1

将左右夹爪分别连接到两个 USB-RS485 适配器，并使用稳定的设备路径：

Bash

ls -l /dev/serial/by-id/

配置中应填写 /dev/serial/by-id/...，不要依赖可能随拔插变化的 /dev/ttyUSB*。每个 适配器和 slave ID 只能对应一个 PGC 设备。

获取当前主机的 serial_port

示例配置中的串口名称只对应示例设备，不能直接复制到另一台主机。插入 USB-RS485 适配器后，在目标主机上执行：

Bash

ls -l /dev/serial/by-id/
readlink -f /dev/serial/by-id/usb-FTDI_*

/dev/serial/by-id/ 下的符号链接名称包含 USB 适配器厂商和硬件序列号；将完整的链接路径 复制到 hardware_config.json 的 serial_port 字段。若适配器不是 FTDI，不要使用上面的 通配符命令，直接使用 ls -l 输出的完整路径。需要同时查看厂商、型号和序列号时，可以 执行：

Bash

for port in /dev/serial/by-id/*; do
    [ -e "$port" ] || continue
    printf '%s -> %s\n' "$port" "$(readlink -f "$port")"
    udevadm info --query=property --name="$port" | \
        grep -E '^(ID_VENDOR|ID_MODEL|ID_SERIAL)='
done

还可以使用以下命令确认 USB 适配器是否被内核识别：

Bash

lsusb
sudo dmesg --ctime | grep -Ei 'FTDI|ttyUSB|usb serial|cp210|ch341' | tail -50

拔插适配器后重新执行 ls -l /dev/serial/by-id/，新增或消失的链接就是该适配器对应的 路径。不要把 /dev/ttyUSB0、/dev/ttyUSB1 当作永久标识，它们可能因拔插顺序改变。 如果系统没有生成 by-id 链接，可以临时查看 /dev/serial/by-path/；该路径依赖 USB 物理插口，换插口后可能变化，部署时应重新确认。

确认串口对应左手还是右手

serial_port 只标识 USB-RS485 适配器，不包含“左手”或“右手”信息；即使只看到一个 串口链接，也只能说明当前接入了一个适配器，不能仅凭名称判断它连接的是哪一侧。因此， 推荐在首次部署时使用“单侧接入 + 小行程动作”建立映射：

1. 停止 EndEffectorCore 和其他串口程序，记录当前 /dev/serial/by-id/ 列表。如果 EndEffectorCore 由 fourierpc 或其他进程监管，必须先暂停或停止监管进程，否则子进程 可能被自动重新启动并继续占用串口。
2. 只保留一只 PGC 及其 USB-RS485 适配器连接，重新执行 ls -l /dev/serial/by-id/， 记录唯一的 serial_port。
3. 将该路径暂时写入 left_hand.serial_port，将 left_hand.enabled 设为 true，并 将 right_hand.enabled 设为 false。启动 EndEffectorCore，确认初始化成功且没有 串口超时。
4. 确认夹爪周围安全后，运行本页“单独下发 PGC 指令”中的示例代码时，只保留 与当前启用侧对应的单侧动作，先发送 left_end_effector 的小行程目标，再观察实际 运动的是左手还是右手。单侧配置下不要继续执行示例中的另一侧、双侧和恢复动作。 示例配置使用归一化位置时，可依次发送 [0.7] 和 [1.0]；使用原始位置时则发送 [700] 和 [1000]。
5. 将该路径写入观察到的那一侧（left_hand 或 right_hand）的 serial_port，并将 另一侧暂时设置为 "enabled": false。停止并重新启动 EndEffectorCore，再连接第二 只 PGC，重复步骤 2 至 4，最后补全双侧配置。

如果不能断开其中一只手，也可以在双设备连接时逐侧启用配置：先将候选路径写入 left_hand.serial_port、禁用右侧，启动运行时并通过本页示例发送一个安全的小行程命令； 观察运动侧后再确定映射。确认映射前不要直接运行双侧动作，以免左右配置颠倒。仅查看 串口文件名无法判断左右手，必须结合物理接线标签、线缆追踪或一次受控的小行程动作完成确认。

3.2 PGC 配置文件

DEB 包安装后，以下 PGC 配置示例位于：

Text

/opt/fftai/End_effector/bin/node_config/examples/hardware_config_pgc_dual.json
/opt/fftai/End_effector/bin/node_config/examples/hardware_config_pgc_right.json

将对应示例复制为运行时配置，并替换实际适配器路径。双 PGC 配置示例：

Bash

sudo cp /opt/fftai/End_effector/bin/node_config/examples/hardware_config_pgc_dual.json \
   /opt/fftai/End_effector/bin/node_config/hardware_config.json

每侧 PGC 配置字段如下：

字段	含义
enabled	是否启用该侧设备
vendor	固定为dh_robotics
model	固定为PGC
transport	固定为modbus_rtu
serial_port	该侧 USB-RS485 的稳定路径
baud_rate、data_bits、stop_bits、parity	串口格式
slave_id	PGC 的 Modbus 地址
command_rate_hz	PGC 命令写入的运行时频率
poll_rate_hz	PGC 状态读取的运行时频率
default_speed	命令未提供有效速度时使用的速度，范围1..100
default_force	命令未提供有效力度时使用的力度，范围20..100
position_input_normalized	位置输入和状态输出是否使用0..1 归一化数值
response_timeout_ms	单次 Modbus 请求超时时间

command_rate_hz 和 poll_rate_hz 是 PGC 硬件 worker 的频率，不是 DDS 发布频率。DDS 可以以更高频率接收命令，但运行时只保留最新待执行命令；未经硬件压测，不建议将 PGC 频率设置得高于已验证的 20 Hz。

位置单位由 position_input_normalized 决定：

配置值	输入命令	发布状态
true	0..1	0..1
false	0..1000	0..1000

PGC 寄存器始终使用原始位置范围 0..1000。运行时会在 DDS 数值与寄存器数值之间进行 换算。PGC 不提供实际速度和实际夹持力反馈，因此 end_effector_state 中对应的 velocity 和 effort 固定为 0，不能将其当作力传感器或接触检测结果。

3.3 Modbus 寄存器

EndEffectorCore 使用以下寄存器完成初始化、运动控制和状态采集：

地址	用途	方向	取值
0x0100	初始化	写	1：普通初始化
0x0101	目标夹持力	写	20..100
0x0103	目标位置	写	0..1000
0x0104	目标速度	写	1..100
0x0200	初始化状态	读	1：初始化完成
0x0201	夹持状态	读	0：运动中，1：到位未夹持，2：已夹持，3：掉落
0x0202	实际位置	读	0..1000

运行时只读取 0x0200..0x0202 的状态，不把 0x0201 映射为 DDS 的新字段。需要使用 夹持状态时，应通过日志或单独的 Modbus 诊断工具读取。正常启动只写入初始化值 1，不 应在每次启动时写入完整标定值 0xA5；正常控制循环也不写入保存配置寄存器。

4. DDS 接口

4.1 Topic

方向	Topic	消息类型	说明
订阅	end_effector_cmd	fourier_msgs_pod/msg/EndEffectorGroupCmd	接收左右夹爪命令
发布	end_effector_state	fourier_msgs_pod/msg/EndEffectorState	发布左右夹爪状态
发布	end_effector_metainfo	fourier_msgs/msg/BaseDataType	发布左右夹爪的设备元数据

命令必须使用精确的 group 名称：

Text

left_end_effector
right_end_effector

只控制一侧时，在 EndEffectorGroupCmd 中仅提供对应的 group 命令，另一侧的命令位置应 保持为 NaN，这样另一侧不会收到新的目标。双侧同时控制时，再分别提供 left_end_effector 和 right_end_effector 两个 group 的命令。具体消息构造方式见下方 DDS 示例。

PGC 每侧只消费命令数组的第一个元素。DDS 消息中的数组是固定宽度时，后续元素不会被 PGC 使用，但第一个位置值必须有效。只发送位置而未提供速度或力度时，运行时使用该侧 配置中的 default_speed 和 default_force。

状态数组按启用设备顺序拼接：左侧状态在前，右侧状态在后。双 PGC 配置的 state_size 为 2；单 PGC 配置的 state_size 为 1。

5. 上层控制案例

5.1 启动 EndEffectorCore

从 DEB 包安装目录启动，使相对路径 node_config/node_config.json 和 node_config/hardware_config.json 能被正确加载：

Bash

cd /opt/fftai/End_effector/bin
./EndEffectorCore

确认日志显示左右启用的 PGC 初始化完成，并且没有串口超时。保持该终端运行，再打开 另一个终端执行 DDS 客户端案例。

测试前置条件：不要在测试期间启动 AuroraCore。

AuroraCore 会以固定频率向 end_effector_cmd 发布上层命令。多个 DDS publisher 共享同一 topic 时，AuroraCore 的新命令可能覆盖本页示例发送的目标，表现为日志已经 收到测试命令但夹爪不动作，或夹爪很快回到上层命令的位置。设备重启后，系统服务可能会 自动启动 AuroraCore，因此开始测试前应确认它没有运行或没有发布末端命令。只启动并 保持本页的 EndEffectorCore 即可。

如果 AuroraCore 已由进程监管器启动，可在确认机械设备处于安全状态后临时暂停它：

Bash

AURORA_PID=$(ps -eo pid=,stat=,comm= | \
  awk '$3=="AuroraCore" && $2 !~ /^Z/ {print $1; exit}')
test -n "$AURORA_PID" || { echo "AuroraCore 未运行"; exit 0; }
sudo kill -STOP "$AURORA_PID"

保持该终端打开，在另一个终端运行 DDS 测试。确认测试结束且设备处于安全状态后，回到 原终端恢复 AuroraCore：

Bash

sudo kill -CONT "$AURORA_PID"

测试前应确认 AURORA_PID 不为空，暂停后在 EndEffectorCore 日志中确认不再持续出现 来自上层的重复命令。测试结束后必须恢复 AuroraCore，以免影响正常上层控制。

注意：暂停时间不宜过长。 进程监管器（fourierpc）通过心跳监控 AuroraCore；被 STOP 冻结的进程会因心跳超时被监管器终止并用新实例替换。此时 kill -CONT 会作用在已退出的旧进程上，但新实例会自动启动并恢复对机器人的控制。恢复后应读取 FSM 状态确认机器人回到预期状态。

以下四个案例相互独立，均直接使用 fourierdds_py，不需要额外的上层控制服务。运行案例 前请先启动 EndEffectorCore，并确保没有其他程序持续向相关 topic 发布或订阅数据。

5.2 单独下发 PGC 指令

下面的代码只向一个指定侧下发一次位置指令。示例假设 position_input_normalized=true；如果配置为 false，请将 POSITION 改为 700 或 1000 等原始范围值。速度和力度保持为 NaN，由该侧配置中的 default_speed 和 default_force 提供。确认夹爪周围安全，并根据实际安装确认 0.0 和 1.0 的开合方向。

Python

import math
import os
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # 机器人上运行必须设置

from fourierdds_py import DDSInterface, PublisherQosProfile
import fourier_msgs_pod.msg.RobotJointCmd as RobotJointCmd

DOMAIN_ID = 123
SIDE = "left"                 # 改为 "right" 可测试右侧
POSITION = 0.7                # normalized=true: 0..1; false: 0..1000
GROUP_NAMES = ("left_end_effector", "right_end_effector")
GROUP_INDEX = {"left": 0, "right": 1}


def set_string(char_array, value):
    data = value.encode("utf-8")
    if len(data) >= 64:
        raise ValueError("group 名称必须小于 64 字节")
    for index, byte in enumerate(data):
        char_array[index] = byte
    char_array[len(data)] = 0


def clear_command(command):
    # PGC 每侧只使用第一个元素；其余固定宽度元素保持无效。
    invalid = [math.nan] * len(command.position())
    command.position(invalid)
    command.velocity(invalid)
    command.acceleration(invalid)
    command.effort(invalid)


def set_position(command, value):
    clear_command(command)
    positions = [math.nan] * len(command.position())
    positions[0] = float(value)
    command.position(positions)
    # 速度和力度保持 NaN，由 hardware_config.json 中的默认值提供。


dds = DDSInterface(domain_id=DOMAIN_ID)
publisher = dds.create_publisher(
    RobotJointCmd.EndEffectorGroupCmdPubSubType,
    "end_effector_cmd",
    qos_profile=PublisherQosProfile.default(),
)

try:
    # 等待 DDS 发现完成后再发布第一条命令。
    time.sleep(3.0)
    message = RobotJointCmd.EndEffectorGroupCmd()
    for index, group_name in enumerate(GROUP_NAMES):
        set_string(message.group_names()[index].content(), group_name)
        clear_command(message.commands()[index])

    set_position(message.commands()[GROUP_INDEX[SIDE]], POSITION)
    print(f"publishing {SIDE} position={POSITION}", flush=True)
    for attempt in range(3):
        publisher.publish(message)
        if attempt < 2:
            time.sleep(0.1)
finally:
    dds.close()

消息中的两个 group 名称仍需按固定结构填充，但未选中侧的所有数组元素均为 NaN， 因此不会收到新的目标。测试期间如果 EndEffectorCore 日志中同一侧的位置在测试目标和 其他数值之间反复切换，应先停止其他 end_effector_cmd publisher。

5.3 完整 DDS 运动验收

下面的完整示例适用于左右两侧均启用的双 PGC 配置，依次执行左侧单独运动、右侧单独 运动、双侧同时运动，最后将两侧恢复到 1.0。程序会订阅状态并等待目标位置到达； 如果只启用一侧，请使用上面的单独下发示例，不要运行本示例中的另一侧和双侧动作。 示例假设 position_input_normalized=true；若配置为 false，请将位置值改为对应的 原始范围值（例如 500、0 和 1000）。确认夹爪周围安全，并根据实际安装确认 0.0 和 1.0 的开合方向。

Python

import math
import os
import threading
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # 机器人上运行必须设置

from fourierdds_py import DDSInterface, PublisherQosProfile, SubscriberQosProfile
import fourier_msgs_pod.msg.RobotJointCmd as RobotJointCmd
import fourier_msgs_pod.msg.RobotJointState as RobotJointState

DOMAIN_ID = 123
GROUP_NAMES = ("left_end_effector", "right_end_effector")
GROUP_INDEX = {"left": 0, "right": 1}


def set_string(char_array, value):
    data = value.encode("utf-8")
    if len(data) >= 64:
        raise ValueError("group 名称必须小于 64 字节")
    for index, byte in enumerate(data):
        char_array[index] = byte
    char_array[len(data)] = 0


def clear_command(command):
    # PGC 每侧只使用第一个元素；其余固定宽度元素保持无效。
    invalid = [math.nan] * len(command.position())
    command.position(invalid)
    command.velocity(invalid)
    command.acceleration(invalid)
    command.effort(invalid)


def set_position(command, value):
    clear_command(command)
    positions = [math.nan] * len(command.position())
    positions[0] = float(value)
    command.position(positions)
    # 速度和力度留空，由 hardware_config.json 中的默认值提供。


dds = DDSInterface(domain_id=DOMAIN_ID)
state_condition = threading.Condition()
last_state = {"left": math.nan, "right": math.nan}
state_samples = 0


def on_state(message):
    global state_samples
    size = int(message.state_size())
    positions = message.position()
    with state_condition:
        if size >= 1:
            last_state["left"] = float(positions[0])
        if size >= 2:
            last_state["right"] = float(positions[1])
        state_samples += 1
        if state_samples == 1 or state_samples % 20 == 0:
            print(
                f"state_size={size}, left={last_state['left']:.3f}, "
                f"right={last_state['right']:.3f}",
                flush=True,
            )
        state_condition.notify_all()


dds.create_subscription(
    RobotJointState.EndEffectorStatePubSubType,
    "end_effector_state",
    on_state,
    qos_profile=SubscriberQosProfile.default(),
)
publisher = dds.create_publisher(
    RobotJointCmd.EndEffectorGroupCmdPubSubType,
    "end_effector_cmd",
    qos_profile=PublisherQosProfile.default(),
)


def publish_and_wait(message, targets, label, timeout=10.0):
    for command in message.commands():
        clear_command(command)
    for side, position in targets.items():
        set_position(message.commands()[GROUP_INDEX[side]], position)

    print(f"publishing {label}: {targets}", flush=True)
    for attempt in range(3):
        publisher.publish(message)
        if attempt < 2:
            time.sleep(0.1)

    def reached():
        return all(
            math.isfinite(last_state[side])
            and abs(last_state[side] - position) <= 0.03
            for side, position in targets.items()
        )

    with state_condition:
        if not state_condition.wait_for(reached, timeout):
            raise RuntimeError(
                f"{label} timeout: targets={targets}, state={dict(last_state)}"
            )
    print(f"{label} reached: {dict(last_state)}", flush=True)


try:
    # 等待 DDS 发现完成后再发布第一条命令。
    time.sleep(3.0)
    message = RobotJointCmd.EndEffectorGroupCmd()
    for index, group_name in enumerate(GROUP_NAMES):
        set_string(message.group_names()[index].content(), group_name)
        clear_command(message.commands()[index])

    publish_and_wait(message, {"left": 0.5}, "left-only")
    publish_and_wait(message, {"right": 0.5}, "right-only")
    publish_and_wait(message, {"left": 0.0, "right": 0.0}, "dual")
    publish_and_wait(message, {"left": 1.0, "right": 1.0}, "restore")
finally:
    dds.close()

预期输出会显示每个阶段的 reached 结果以及两侧的最终位置。若出现超时，先检查 EndEffectorCore 日志中的实际目标、PGC 初始化状态和 AuroraCore publisher 是否已暂停。

5.4 查看夹爪状态数据

下面的代码订阅 end_effector_state，打印 state_size 以及每个已启用 PGC 的位置、 速度和力度。状态数组按启用设备顺序排列，PGC 每侧占一个元素；PGC 不提供实际速度和 夹持力反馈，因此速度和力度按约定为 0。单侧配置时，将 ENABLED_SIDES 改为实际启用 的一侧，例如 ("right",)。

Python

import os
import signal
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # 机器人上运行必须设置

from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs_pod.msg.RobotJointState as RobotJointState

DOMAIN_ID = 123
ENABLED_SIDES = ("left", "right")  # 单侧配置示例：("right",)

dds = DDSInterface(domain_id=DOMAIN_ID)


def on_state(message):
    state_size = int(message.state_size())
    positions = message.position()
    velocities = message.velocity()
    efforts = message.effort()
    print(f"state_size={state_size}", flush=True)

    for index, side in enumerate(ENABLED_SIDES[:state_size]):
        print(
            f"  {side}_end_effector: "
            f"position={float(positions[index]):.6g}, "
            f"velocity={float(velocities[index]):.6g}, "
            f"effort={float(efforts[index]):.6g}",
            flush=True,
        )


dds.create_subscription(
    RobotJointState.EndEffectorStatePubSubType,
    "end_effector_state",
    on_state,
    qos_profile=SubscriberQosProfile.default(),
)

running = {"value": True}


def _stop(signum, frame):
    running["value"] = False
    print(f"\nsignal {signum} received, stopping ...", flush=True)


# fastdds 的原生信号处理会吞掉 Ctrl+C，必须显式覆盖才能正常退出
signal.signal(signal.SIGINT, _stop)
signal.signal(signal.SIGTERM, _stop)

try:
    print("Subscribed to 'end_effector_state', waiting for data...", flush=True)
    while running["value"]:
        time.sleep(0.1)
finally:
    dds.close()

5.5 查看设备元数据

end_effector_metainfo 以 BaseDataType.arg_string 发布左右两侧的元数据，每侧占四个 字符串，顺序为 name、type、driver_version、hardware_version。PGC 的正常返回值 通常为 dh_robotics、PGC、libmodbus 和 unknown；未启用的一侧为空字符串。

Python

import os
import signal
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # 机器人上运行必须设置

from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs.msg.BaseStructType as BaseStructType

DOMAIN_ID = 123
FIELDS = ("name", "type", "driver_version", "hardware_version")

dds = DDSInterface(domain_id=DOMAIN_ID)


def on_metainfo(message):
    values = message.arg_string()
    size = values.size() if hasattr(values, "size") else len(values)
    print("--- End Effector Metainfo ---", flush=True)
    for side, offset in (("left", 0), ("right", 4)):
        print(f"  {side}_end_effector:", flush=True)
        for index, field in enumerate(FIELDS):
            position = offset + index
            value = values[position] if position < size else ""
            print(f"    {field}: {value or '(empty)'}", flush=True)


dds.create_subscription(
    BaseStructType.BaseDataTypePubSubType,
    "end_effector_metainfo",
    on_metainfo,
    qos_profile=SubscriberQosProfile.default(),
)

running = {"value": True}


def _stop(signum, frame):
    running["value"] = False
    print(f"\nsignal {signum} received, stopping ...", flush=True)


# fastdds 的原生信号处理会吞掉 Ctrl+C，必须显式覆盖才能正常退出
signal.signal(signal.SIGINT, _stop)
signal.signal(signal.SIGTERM, _stop)

try:
    print("Subscribed to 'end_effector_metainfo', waiting for data...", flush=True)
    while running["value"]:
        time.sleep(0.1)
finally:
    dds.close()

6. 验收与故障排查

建议按以下顺序验收：

1. ls -l /dev/serial/by-id/ 能看到每个适配器的稳定路径。
2. EndEffectorCore 初始化完成且持续发布 end_effector_state。
3. 状态数组的 state_size 与启用的 PGC 数量一致，每侧占一个位置元素。
4. 发送单侧命令时，未选中的一侧保持原位置。
5. 发送双侧命令时，两侧均达到目标位置，再恢复到安全位置。

常见问题：

• Connection timed out：检查 PGC 电源、RS485 的 A/B 线、波特率/校验位、slave ID 和 serial_port，并确认没有其他进程占用端口。
• no ... PGC state received：确认 EndEffectorCore 已启动、配置文件已加载，且 DDS Domain ID 为 123。
• 日志显示收到命令但夹爪不动：检查 group 名称是否精确匹配，PGC 是否已完成初始化， 以及 default_speed（1..100）和 default_force（20..100）是否有效。
• 两侧一起运动：检查客户端是否同时提供了两个 group；单侧测试时只保留目标侧的命令。
• 不要把 velocity=0 或 effort=0 当作 PGC 的实际反馈；PGC 的这两个状态字段按约定 始终为 0。