GR-EndEffector

1. 概述

GR-EndEffector 是 Fourier 灵巧手的 DDS 桥接模块。运行时应用 EndEffectorCore 从 DDS 接收手部关节命令，通过物理手 SDK 写入 PVC 命令，并将手部状态、触觉数据、设备元信息和 HAL 错误码重新发布到 DDS。

当前启动路径会直接初始化 JointDataNode。FDH6 与 FDH12 插件路径在默认运行时配置中不会加载。

元素	数值
运行时应用名	`EndEffectorCore`
Domain ID	`123`
支持硬件	FDH-6 与 FDH-12 灵巧手
默认设备	左手、右手均启用
默认 IP	左手：`192.168.137.19`；右手：`192.168.137.39`
Group 名称	`left_end_effector`、`right_end_effector`

2. 运行流程

Text用户 DDS publisher
  -> end_effector_cmd
  -> JointDataNode 命令回调
  -> 单手 PVC 命令缓存
  -> 单手 worker 循环
  -> DexHand SDK set_pvc() / get_pvc()
  -> end_effector_state, end_effector_tactile, end_effector_metainfo, hal_endeffector_error_code

每个启用的手都有一个约 10 ms 周期的 worker 循环。worker 会从 SDK 读取 PVC 状态，写入最新 PVC 命令；如果检测到设备为 FDH-12，还会读取触觉矩阵；同时会读取原始硬件错误码。

3. DDS 接口

3.1 Topics

Topic	消息类型	频率	说明
`end_effector_cmd`	`fourier_msgs_pod/msg/EndEffectorGroupCmd`	接收触发	左右手关节命令

发布

Topic	消息类型	频率	说明
`end_effector_state`	`fourier_msgs_pod/msg/EndEffectorState`	100 Hz	拼接后的手部关节状态：位置、速度、力矩 / 电流通道
`end_effector_tactile`	`fourier_msgs/msg/GripperSensorData`	100 Hz	两个触觉 surface，每只手一个；FDH-12 会填充数据
`end_effector_metainfo`	`std_msgs/msg/BaseDataType`	1 Hz	设备名称、类型、驱动版本和硬件版本
`hal_endeffector_error_code`	`fourier_msgs/msg/ErrorCodes`	10 Hz	HAL 格式的末端错误码列表

3.2 命令字段

EndEffectorGroupCmd 通过 group_names 路由命令。运行时只消费 group 名称精确等于 left_end_effector 或 right_end_effector 的条目。

字段	用途
`group_names[i]`	目标手部 group 名称
`commands[i].position[j]`	第 `j` 个关节的目标位置
`commands[i].velocity[j]`	第 `j` 个关节的目标速度
`commands[i].acceleration[j]`	命令类型中存在该字段；当前运行时不会拷贝到 SDK PVC 缓存
`commands[i].effort[j]`	第 `j` 个关节的目标力矩 / 前馈值

3.3 状态字段

EndEffectorState 以扁平数组发布，不再按左右手分组：

字段	用途
`position`	当前关节位置
`velocity`	当前关节速度
`effort`	PVC 数据中的当前力矩 / 电流通道
`state_size`	`left_dof + right_dof`

数组前 left_dof 个元素对应左手，随后 right_dof 个元素对应右手。

3.4 元信息字段

end_effector_metainfo 通过 BaseDataType.arg_string 发布 8 个位置：

下标	含义
`0`	左手设备名称，例如 `fdhv1` 或 `fdhv2`
`1`	左手设备类型
`2`	左手驱动版本
`3`	左手硬件版本
`4`	右手设备名称
`5`	右手设备类型
`6`	右手驱动版本
`7`	右手硬件版本

当 SDK 返回的设备名称为 fdhv2 时，运行时将该手识别为 FDH-12，并读取其触觉矩阵。

3.5 触觉字段

end_effector_tactile 始终发布两个 TactileSensorSurface 条目：

字段	数值
`sensor_count`	`2`
`tactile_sensors[0].sensor_id`	`0`
`tactile_sensors[0].sensor_name`	`left_end_effector`
`tactile_sensors[1].sensor_id`	`1`
`tactile_sensors[1].sensor_name`	`right_end_effector`
`uint8_data.rows`、`uint8_data.cols`、`uint8_data.data`	有数据时为按行展开的触觉矩阵

如果某只手不支持触觉，对应矩阵会保持为空。

3.6 错误码字段

hal_endeffector_error_code 发布 ErrorCodes，其中 source = 3。每个非状态类硬件错误都会被转换到 ErrorCode.high32：

Textbits [31:28]  子模块，当前为 0
bits [27:12]  原始硬件错误码
bits [11:4]   实例 ID：0x13 左手，0x27 右手
bits [3:0]    严重级别：1 状态，2 警告，3 故障

ErrorCode.low32 固定为 0。

4. 运行环境假设

下方示例假设机器人已经随产品交付并完成 EndEffectorCore 运行时部署，用户通过 DDS 客户端与正在运行的模块交互。

运行客户端 DDS 示例前，请确认：

机器人侧 EndEffectorCore 运行时已经启动。
客户端环境包含 fourierdds_py 和已生成的 Fourier 消息包。
客户端可以在 DDS domain 123 上通信。
左右手 IP 已在交付部署过程中完成配置。

5. 基础示例

5.1 发布手部命令

该示例会填充双手 group 名称，等待 DDS 发现完成，然后向双手发布相同目标位置。

Pythonimport time

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

DOMAIN_ID = 123
DOF = 12
GROUP_NAMES = ("left_end_effector", "right_end_effector")

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

def set_positions(cmd, positions):
    for i, position in enumerate(positions):
        cmd.position()[i] = float(position)
        cmd.velocity()[i] = 0.0
        cmd.acceleration()[i] = 0.0
        cmd.effort()[i] = 0.0

dds = DDSInterface(domain_id=DOMAIN_ID)
pub = dds.create_publisher(
    RobotJointCmd.EndEffectorGroupCmdPubSubType,
    "end_effector_cmd",
    qos_profile=PublisherQosProfile.default(),
)
time.sleep(3)

msg = RobotJointCmd.EndEffectorGroupCmd()
set_string(msg.group_names()[0].content(), GROUP_NAMES[0])
set_string(msg.group_names()[1].content(), GROUP_NAMES[1])

target = [0.5] * DOF
set_positions(msg.commands()[0], target)
set_positions(msg.commands()[1], target)

pub.publish(msg)

5.2 订阅手部状态

Pythonimport time

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

dds = DDSInterface(domain_id=123)

def on_state(msg):
    size = msg.state_size()
    pos = msg.position()
    vel = msg.velocity()
    eff = msg.effort()
    print(
        f"state_size={size} "
        f"joint[0]: pos={pos[0]:.4f}, vel={vel[0]:.4f}, effort={eff[0]:.4f}"
    )

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

while True:
    time.sleep(0.1)

5.3 订阅设备元信息

Pythonimport time

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

DDS_DOMAIN_ID = 123
FIELDS = ("name", "type", "driver_ver", "hw_ver")

dds = DDSInterface(domain_id=DDS_DOMAIN_ID)

def on_metainfo(msg):
    values = msg.arg_string()
    size = values.size() if hasattr(values, "size") else len(values)

    for side, offset in (("left", 0), ("right", 4)):
        print(f"{side} hand:")
        for index, field in enumerate(FIELDS):
            pos = offset + index
            value = values[pos] if pos < size else ""
            print(f"  {field}: {value or '(empty)'}")

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

while True:
    time.sleep(0.1)

5.4 订阅触觉数据

Pythonimport time

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

dds = DDSInterface(domain_id=123)

def on_tactile(msg):
    sensors = msg.tactile_sensors()
    count = sensors.size() if hasattr(sensors, "size") else len(sensors)
    print(f"sensor_count={msg.sensor_count()}")

    for index in range(count):
        surface = sensors[index]
        matrix = surface.uint8_data()
        rows = matrix.rows()
        cols = matrix.cols()
        data = matrix.data()

        print(f"[{surface.sensor_id()}] {surface.sensor_name()}: {rows}x{cols}")
        if rows == 0 or cols == 0:
            continue

        for row in range(rows):
            values = [data[row * cols + col] for col in range(min(cols, 16))]
            print("  " + " ".join(f"{value:3d}" for value in values))

dds.create_subscription(
    TactileSensor.GripperSensorDataPubSubType,
    "end_effector_tactile",
    on_tactile,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

5.5 订阅 HAL 错误码

Pythonimport time

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

SEVERITY_MAP = {
    1: "STATUS",
    2: "WARNING",
    3: "FAULT",
}

INSTANCE_MAP = {
    0x13: "LEFT",
    0x27: "RIGHT",
}

dds = DDSInterface(domain_id=123)

def decode_high32(high32):
    return {
        "raw_code": (high32 >> 12) & 0xFFFF,
        "instance_id": (high32 >> 4) & 0xFF,
        "severity": high32 & 0x0F,
    }

def on_error_codes(msg):
    codes = msg.error_codes()
    count = codes.size() if hasattr(codes, "size") else len(codes)
    print(f"source={msg.source()} count={count}")

    if count == 0:
        print("  no active end-effector errors")
        return

    for index in range(count):
        item = codes[index]
        decoded = decode_high32(item.high32())
        hand = INSTANCE_MAP.get(decoded["instance_id"], "UNKNOWN")
        severity = SEVERITY_MAP.get(decoded["severity"], "UNKNOWN")
        print(
            f"[{index}] hand={hand} severity={severity} "
            f"raw=0x{decoded['raw_code']:04X} "
            f"high32=0x{item.high32():08X} low32=0x{item.low32():08X}"
        )

dds.create_subscription(
    ErrorCodes.ErrorCodesPubSubType,
    "hal_endeffector_error_code",
    on_error_codes,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

6. 故障排查与注意事项

6.1 Group 名称必须精确匹配

命令通过 group_names 路由。如果字符串与 left_end_effector 或 right_end_effector 不一致，运行时会忽略对应命令条目。

6.2 等待 DDS 发现完成

创建 publisher 后，请等待约 3 秒再发布第一条命令，以便 DDS 发现完成。

6.3 检查电源与网络

如果运行时提示未发现设备，请检查手部电源、急停状态，以及到配置 IP 的网络连通性。

Bashping 192.168.137.19
ping 192.168.137.39

6.4 触觉数据依赖 FDH-12

即使某只手不支持触觉，触觉 topic 也可能仍然发布两个 surface，但该手对应的矩阵为空。

6.5 命令值需保持在硬件限制内

示例中使用 0.0 表示张开、1.0 表示闭合。向特定硬件发送其它位置值前，请先确认该硬件的有效范围。

1. 概述​

2. 运行流程​

3. DDS 接口​

3.1 Topics​

订阅​

发布​

3.2 命令字段​

3.3 状态字段​

3.4 元信息字段​

3.5 触觉字段​

3.6 错误码字段​

4. 运行环境假设​

5. 基础示例​

5.1 发布手部命令​

5.2 订阅手部状态​

5.3 订阅设备元信息​

5.4 订阅触觉数据​

5.5 订阅 HAL 错误码​

6. 故障排查与注意事项​

6.1 Group 名称必须精确匹配​

6.2 等待 DDS 发现完成​

6.3 检查电源与网络​

6.4 触觉数据依赖 FDH-12​

6.5 命令值需保持在硬件限制内​