GR-Actuator

1. 概述

GR-Actuator 是 DDS 与 Fourier 机器人本体执行器之间的接口。它通过 DDS 接收本体关节命令，驱动腿部、腰部、头部和手臂执行器，并将本体关节状态和执行器故障通过 DDS 发布。

以 GR3 为例，本模块覆盖机器人本体 31 个自由度：双腿 12 DOF、腰部 3 DOF、头部 2 DOF、双臂 14 DOF。请注意：左右灵巧手不属于 GR-Actuator。

元素	数值
Domain ID	123
周期	关节状态 1000 Hz 发布，本体控制循环约 1 ms
硬件	GR3 本体关节执行器，共 31 DOF
Group 字段	left_leg、right_leg、waist、head、left_manipulator、right_manipulator

2. DDS 接口服务

2.1 Topics

订阅

Topic	消息类型	说明
robot_joint_cmd	JointGroupCmd	本体控制组关节命令

发布

Topic	消息类型	频率	说明
robot_joint_state	RobotJointState	1000 Hz	当前本体关节状态（位置、速度、力矩）
hal_actuator_error_code	ErrorCodes	事件触发；持续故障最多约 10 Hz	执行器故障码

服务

Service	Request / Response	说明
set_actuator_params	SetActuatorParams_Request / SetActuatorParams_Response	设置控制组 PID 参数
set_actuator_behavior_enable	SetActuatorBehavior_Request / SetActuatorBehavior_Response	使能或禁用控制组

2.2 IDL 定义

JointGroupCmd

Idl

struct PvatData7 {
    double position[7];      // 目标位置，前 N 个自由度有效
    double velocity[7];      // 目标速度
    double acceleration[7];  // 目标加速度
    double effort[7];        // 目标力矩或前馈力矩
};

struct JointGroupCmd {
    HeaderPod header;
    TracePointList trace_points;
    ShortString group_names[6];  // 例如 "left_manipulator"
    PvatData7 commands[6];       // 与 group_names 一一对应
};

GR3 常用 group_names 为 left_leg、right_leg、waist、head、left_manipulator、right_manipulator。每个控制组最多 7 个自由度，commands[i] 只读取对应控制组实际自由度数量内的数据。

RobotJointState

Idl

struct RobotJointState {
    HeaderPod header;
    TracePointList trace_points;
    double position[32];  // 当前关节位置
    double velocity[32];  // 当前关节速度
    double effort[32];    // 当前关节力矩
    uint8 state_size;
};

GR3 本体状态按整机关节顺序写入数组。消费端通常读取前 31 个元素；灵巧手状态不在该消息内。

SetActuatorParams

Idl

enum ControlMode {
    POSITION,
    VELOCITY,
    TORQUE,
    PD
};

struct ControlParameters {
    double pos_kp[7];
    double pos_ki[7];
    double pos_kd[7];
    double pd_kp[7];
    double pd_kd[7];
};

struct SetActuatorParams_Request {
    HeaderPod header;
    boolean force_reset;      // 保留字段
    ShortString group_name;   // 目标控制组
    ControlMode motor_mode;   // POSITION 或 PD
    ControlParameters params;
};

struct SetActuatorParams_Response {
    HeaderPod header;
    LongString message;
    boolean success;
};

当前服务端支持 POSITION 和 PD 参数写入，结果以 success 为准。

SetActuatorBehavior

Idl

struct ControlBehavior {
    boolean enable;  // true 使能，false 禁用
    uint8 motor_mode;
};

struct SetActuatorBehavior_Request {
    HeaderPod header;
    boolean force_reset;       // 保留字段
    ShortString group_name;    // 目标控制组
    ControlBehavior behavior;
};

struct SetActuatorBehavior_Response {
    HeaderPod header;
    LongString message;
    boolean success;
};

当前公开服务为 set_actuator_behavior_enable，只使用 behavior.enable。

ErrorCodes

Idl

struct ErrorCode {
    uint32 high32;
    uint32 low32;
};

struct ErrorCodes {
    std_msgs::msg::Header header;
    uint8 source;                  // 1 = 执行器
    sequence<ErrorCode> error_codes;
};

3. 基础示例

3.1 — 本体控制命令发布

Python

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

DOMAIN_ID = 123
GROUP_NAME = "left_manipulator"
DOF = 7

def short_string(value):
    data = value.encode("utf-8")
    if len(data) > 64:
        raise ValueError("控制组名称超过 64 字节")
    msg = RobotJointCmd.ShortString()
    msg.content(data + b"\0" * (64 - len(data)))
    return msg

dds = DDSInterface(domain_id=DOMAIN_ID)
pub = dds.create_publisher(
    pub_sub_type=RobotJointCmd.JointGroupCmdPubSubType(),
    topic_name="robot_joint_cmd",
    qos_profile=PublisherQosProfile.best_effort(),
)
time.sleep(3)

msg = RobotJointCmd.JointGroupCmd()
msg.group_names([short_string(GROUP_NAME)] + [RobotJointCmd.ShortString() for _ in range(5)])

cmd = RobotJointCmd.PvatData__7()
for i in range(DOF):
    cmd.position()[i] = 0.0
    cmd.velocity()[i] = 0.0
    cmd.acceleration()[i] = 0.0
    cmd.effort()[i] = 0.0
msg.commands([cmd] + [RobotJointCmd.PvatData__7() for _ in range(5)])

pub.publish(msg)

3.2 — 本体关节状态订阅

Python

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

dds = DDSInterface(domain_id=123)

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

dds.create_subscription(
    pub_sub_type=RobotJointState.RobotJointStatePubSubType(),
    topic_name="robot_joint_state",
    callback=on_state,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

4. 进阶 —— Demo 与工具

4.1 Demo：使能控制组并设置 PD 参数

Python

import time
from fourierdds_py import DDSInterface
import fourier_msgs_pod.srv.SetActuatorBehavior as SetActuatorBehavior
import fourier_msgs_pod.srv.SetActuatorParams as SetActuatorParams

DOMAIN_ID = 123
GROUP_NAME = "left_manipulator"
DOF = 7

def short_string(module, value):
    data = value.encode("utf-8")
    msg = module.ShortString()
    msg.content(data + b"\0" * (64 - len(data)))
    return msg

dds = DDSInterface(domain_id=DOMAIN_ID)
enable_client = dds.create_client(
    req_type=SetActuatorBehavior.SetActuatorBehavior_RequestPubSubType(),
    res_type=SetActuatorBehavior.SetActuatorBehavior_ResponsePubSubType(),
    service_name="set_actuator_behavior_enable",
)
params_client = dds.create_client(
    req_type=SetActuatorParams.SetActuatorParams_RequestPubSubType(),
    res_type=SetActuatorParams.SetActuatorParams_ResponsePubSubType(),
    service_name="set_actuator_params",
)
time.sleep(3)

enable_req = SetActuatorBehavior.SetActuatorBehavior_Request()
behavior = SetActuatorBehavior.ControlBehavior()
behavior.enable(True)
enable_req.behavior(behavior)
enable_req.group_name(short_string(SetActuatorBehavior, GROUP_NAME))
enable_res = enable_client.send_request(enable_req, wait_ms=1000)
if enable_res is None or not enable_res.success():
    raise RuntimeError("set_actuator_behavior_enable failed")

params_req = SetActuatorParams.SetActuatorParams_Request()
params_req.group_name(short_string(SetActuatorParams, GROUP_NAME))
params_req.motor_mode(SetActuatorParams.ControlMode_PD)
params = SetActuatorParams.ControlParameters()
params.pd_kp([80.0] * DOF)
params.pd_kd([4.0] * DOF)
params_req.params(params)
params_res = params_client.send_request(params_req, wait_ms=1000)
if params_res is None or not params_res.success():
    raise RuntimeError("set_actuator_params failed")

4.2 Demo：关节控制完整demo

Python

import time
from fourierdds_py import DDSInterface, PublisherQosProfile, SubscriberQosProfile
import fourier_msgs_pod.msg.RobotJointCmd as RobotJointCmd
import fourier_msgs_pod.msg.RobotJointState as RobotJointState
import fourier_msgs_pod.srv.SetActuatorBehavior as SetActuatorBehavior
import fourier_msgs_pod.srv.SetActuatorParams as SetActuatorParams

DOMAIN_ID = 123
GROUP_NAME = "left_manipulator"
DOF = 7

# 你指定的 KP 和 KD 参数列表
left_manipulator_kp = [400.0, 200.0, 200.0, 200.0, 50.0, 50.0, 50.0]
left_manipulator_kd = [20.0, 10.0, 10.0, 10.0, 2.5, 2.5, 2.5]

# 全局变量，用于存储订阅到的当前关节位置
current_joint_positions = None

def short_string(module, value):
    data = value.encode("utf-8")
    if len(data) > 64:
        raise ValueError("控制组名称超过 64 字节")
    msg = module.ShortString()
    msg.content(data + b"\0" * (64 - len(data)))
    return msg

# 订阅者回调函数：不断更新当前机械臂的真实位置
def on_state_callback(msg):
    global current_joint_positions
    current_joint_positions = list(msg.position())


# ==========================================
# 0. 初始化 DDS 节点与通信接口
# ==========================================
print("正在初始化 DDS 节点...")
dds = DDSInterface(domain_id=DOMAIN_ID)

# 创建 Service 客户端
enable_client = dds.create_client(
    req_type=SetActuatorBehavior.SetActuatorBehavior_RequestPubSubType(),
    res_type=SetActuatorBehavior.SetActuatorBehavior_ResponsePubSubType(),
    service_name="set_actuator_behavior_enable",
)
params_client = dds.create_client(
    req_type=SetActuatorParams.SetActuatorParams_RequestPubSubType(),
    res_type=SetActuatorParams.SetActuatorParams_ResponsePubSubType(),
    service_name="set_actuator_params",
)

# 创建 Topic 订阅者与发布者
dds.create_subscription(
    pub_sub_type=RobotJointState.RobotJointStatePubSubType(),
    topic_name="robot_joint_state",
    callback=on_state_callback,
    qos_profile=SubscriberQosProfile.default(),
)
pub = dds.create_publisher(
    pub_sub_type=RobotJointCmd.JointGroupCmdPubSubType(),
    topic_name="robot_joint_cmd",
    qos_profile=PublisherQosProfile.best_effort(),
)

time.sleep(3) # 等待 DDS 拓扑发现
print("DDS 初始化完毕。\n")


# ==========================================
# 阶段 1: 电机使能与 PD 参数配置
# ==========================================
input(">>> 键盘按下 [Enter] 键开始：【电机使能】与【设置 PD 参数】...")

# 1.1 发送使能请求
print("正在使能控制组...")
enable_req = SetActuatorBehavior.SetActuatorBehavior_Request()
behavior = SetActuatorBehavior.ControlBehavior()
behavior.enable(True)
enable_req.behavior(behavior)
enable_req.group_name(short_string(SetActuatorBehavior, GROUP_NAME))
enable_res = enable_client.send_request(enable_req, wait_ms=1000)
if enable_res is None or not enable_res.success():
    raise RuntimeError("控制组使能失败，程序终止！")
print("-> 控制组使能成功。")

# 1.2 发送 PD 参数请求
print("正在设置自定义 PD 参数...")
params_req = SetActuatorParams.SetActuatorParams_Request()
params_req.group_name(short_string(SetActuatorParams, GROUP_NAME))
params_req.motor_mode(SetActuatorParams.ControlMode_PD)

params = SetActuatorParams.ControlParameters()
params.pd_kp(left_manipulator_kp)
params.pd_kd(left_manipulator_kd)
params_req.params(params)

params_res = params_client.send_request(params_req, wait_ms=1000)
if params_res is None or not params_res.success():
    raise RuntimeError("控制组参数设置失败，程序终止！")
print("-> PD 参数设置成功。\n")


# ==========================================
# 阶段 2: 获取当前真实位置
# ==========================================
print("正在获取机械臂当前真实位置，请稍候...")
for _ in range(30):
    if current_joint_positions is not None:
        break
    time.sleep(0.1)

if current_joint_positions is None:
    raise RuntimeError("无法获取机械臂状态，请检查主控通信或话题名称！")

start_pos = current_joint_positions[20]
target_pos = -1.4  # 目标位置

print(f"-> 成功锁定当前状态！")
print(f"   第 21 个电机(left_elbow_pitch_joint)当前位置: {start_pos:.4f}")
print(f"   插值目标位置: {target_pos:.4f}\n")


# ==========================================
# 阶段 3: 分步安全控制运动
# ==========================================
input(f">>> 确认机械臂周围安全后，按下 [Enter] 键开始：【从 {start_pos:.4f} 平滑插值到 {target_pos:.4f}】...")

# 构建基础控制消息
msg = RobotJointCmd.JointGroupCmd()
msg.group_names([short_string(RobotJointCmd, GROUP_NAME)] + [RobotJointCmd.ShortString() for _ in range(5)])

cmd = RobotJointCmd.PvatData__7()
# 将所有轴的目标指令初始化为当前接收到的真实位置，防止其它轴突变
for i in range(DOF):
    cmd.position()[i] = current_joint_positions[i]
    cmd.velocity()[i] = 0.0
    cmd.acceleration()[i] = 0.0
    cmd.effort()[i] = 0.0
msg.commands([cmd] + [RobotJointCmd.PvatData__7() for _ in range(5)])

# 插值参数
DURATION = 2.0       # 2秒运动时间
STEPS = 100          # 100步
INTERVAL = DURATION / STEPS

print("正在执行平滑插值轨迹...")
for step in range(STEPS + 1):
    alpha = step / STEPS
    current_target = start_pos + (target_pos - start_pos) * alpha

    # 仅修改第 4 个电机 (索引为 3) 的目标位置
    msg.commands()[0].position()[3] = current_target

    # 发送 DDS 指令
    pub.publish(msg)
    time.sleep(INTERVAL)

print(f"\n[完成] 插值运动结束，第 21 个电机(left_elbow_pitch_joint)已成功平滑到达目标位置: {target_pos}")

4.3 工具：关节状态查看器

Python

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

dds = DDSInterface(domain_id=123)

def on_state(msg):
    pos = list(msg.position())
    vel = list(msg.velocity())
    eff = list(msg.effort())
    print("pos[0..6] =", " ".join(f"{value:.4f}" for value in pos[:7]))
    print("vel[0..6] =", " ".join(f"{value:.4f}" for value in vel[:7]))
    print("eff[0..6] =", " ".join(f"{value:.4f}" for value in eff[:7]))

dds.create_subscription(
    pub_sub_type=RobotJointState.RobotJointStatePubSubType(),
    topic_name="robot_joint_state",
    callback=on_state,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

4.4 工具：执行器故障订阅

Python

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

dds = DDSInterface(domain_id=123)

def on_error(msg):
    print(f"source={msg.source()} count={msg.error_codes().size()}")
    for i in range(msg.error_codes().size()):
        code = msg.error_codes()[i]
        print(f"high32=0x{code.high32():08X}, low32=0x{code.low32():08X}")

dds.create_subscription(
    pub_sub_type=ErrorCodes.ErrorCodesPubSubType(),
    topic_name="hal_actuator_error_code",
    callback=on_error,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

5. 故障排查与注意事项

5.1 Group 名称必须精确匹配

命令通过 group_names 路由。GR3 常用控制组为 left_leg、right_leg、waist、head、left_manipulator、right_manipulator。名称不匹配时，该组命令不会生效。

5.2 留 3 秒给 DDS 发现

创建 publisher 或 client 后建议等待约 3 秒。立即发送首个命令时，订阅端或服务端可能还没有完成发现。

5.3 执行器上电后再发送命令

如果运行时仍在等待 hal_fscb_status，说明执行器供电链路尚未满足激活条件。此时命令不会下发到底层执行器。

5.4 关节目标单位

position 单位为 rad，velocity 单位为 rad/s，effort 为力矩或前馈力矩。目标位置应遵守机器人本体关节限位。