跳到主要内容

关节组控制

set_group_cmd()​

Python
set_group_cmd(
    position_cmd: Dict[str, list[float]],
    velocity_cmd: Dict[str, list[float]] | None = None,
    torque_cmd: Dict[str, list[float]] | None = None
)

向一个或多个关节组发送位置、速度、力矩命令。position_cmd 为必填，其余为可选。命令立即生效，建议使用插值避免急剧跳变。

参数类型说明
position_cmdDict[str, list[float]]group 名称 → 关节目标位置列表（rad）
velocity_cmdDict[str, list[float]] | Nonegroup 名称 → 关节目标速度列表（rad/s）
torque_cmdDict[str, list[float]] | Nonegroup 名称 → 关节目标力矩列表（Nm）

合法 group 名称（取决于机器人型号，以 hardware.json 为准）：

waist（5 自由度）、head（2 自由度）、left_manipulator（7 自由度）、right_manipulator（7 自由度）、left_end_effector、right_end_effector

其中两个末端执行器可作为原始命令的对象，并可通过 get_group_state() 读取一维状态；但当前 get_group_motion_state() 和 get_group_motor_cfg() 仅支持 waist、head、left_manipulator、right_manipulator，不能传入 left_end_effector 或 right_end_effector。

危险

控制 waist 时必须发送 5 个关节值。安全默认位置为 [-1.25, 2.15, -0.918, 0, 0]，禁止将腰部全部回零。

示例：带插值的上半身控制

目标由"当前读取值 + 有限差分"生成，不使用硬编码绝对姿态（机器人零位不固定；单关节偏移不要超过 ±0.03 rad）。关节命令需要在状态 4（upper_body_tele_task）下连续流式发送：

Python
import os
import time

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

from fourier_aurora_client import AuroraClient

client = AuroraClient.get_instance(domain_id=123, robot_name="grw")
try:
    time.sleep(1.0)
    if not client.set_fsm_state(4):
        raise RuntimeError("状态切换请求未受理")
    deadline = time.monotonic() + 4.0
    while client.get_fsm_state() != 4:
        if time.monotonic() >= deadline:
            raise RuntimeError("未切换到 upper_body_tele_task")
        time.sleep(0.05)

    init_pose = client.get_group_state("left_manipulator", key="position")
    target_pose = list(init_pose)
    target_pose[0] += 0.03
    steps = 200

    for i in range(steps + 1):                    # 去程
        pose = [s + (t - s) * i / steps for s, t in zip(init_pose, target_pose)]
        client.set_group_cmd({"left_manipulator": pose})
        time.sleep(0.01)
    for i in range(steps + 1):                    # 回到起始位置
        pose = [t + (s - t) * i / steps for s, t in zip(init_pose, target_pose)]
        client.set_group_cmd({"left_manipulator": pose})
        time.sleep(0.01)
finally:
    if client.get_fsm_state() == 4:
        client.set_fsm_state(3)                   # 回到策略站立状态
    client.close()

get_group_state()​

Python
get_group_state(group_name: str, key: str = 'position') -> list[float]

读取指定关节组的当前状态。

参数类型说明
group_namestr关节组名称
keystr返回数据类型，见下表
key 值说明
'position'关节位置（rad），默认
'velocity'关节速度（rad/s）
'effort'关节力矩（Nm）

示例

Python
import os
import time

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

from fourier_aurora_client import AuroraClient

client = AuroraClient.get_instance(domain_id=123, robot_name="grw")
try:
    time.sleep(1.0)  # 等待状态数据到达
    pos = client.get_group_state("waist", key="position")
    vel = client.get_group_state("waist", key="velocity")
    print(f"waist position: {pos}")
    print(f"waist velocity: {vel}")
finally:
    client.close()

get_group_motion_state()​

Python
get_group_motion_state(group_name: str) -> int

读取指定关节组的当前运动状态。

返回值说明
0空闲（idle）
1运动中（moving）
2停止中（stopping）
3错误（error）
GR-W 限制

在状态 4 以 100 Hz 流式发送插值命令期间，left_manipulator 的运动状态始终为 0（idle）。该返回值不反映遥操作命令引起的运动，不能用它等待"运动完成"；运动是否结束应以命令流结束和 get_group_state() 的位置收敛为准。

示例

Python
import os
import time

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

from fourier_aurora_client import AuroraClient

client = AuroraClient.get_instance(domain_id=123, robot_name="grw")
try:
    time.sleep(1.0)  # 等待状态数据到达
    state = client.get_group_motion_state("left_manipulator")
    print(f"motion state: {state} (0=idle)")
finally:
    client.close()