关节控制完整示例

本示例演示 GR-W 关节组控制的完整工作流程：进入 upper_body_tele_task（状态 4），读取当前关节位置，用线性插值让双臂做一个相对当前姿态的小偏移，最后精确回到起始位置。

运行前必读

在创建任何 DDS participant 前设置 export FASTDDS_BUILTIN_TRANSPORTS=UDPv4（见连接说明）。
本示例不使用硬编码绝对目标位姿：机器人每次上电的关节零位可能不同，公开示例一律以"当前读取值 + 有限差分"生成目标。单关节偏移不要超过 ±0.03 rad。
upper_body_tele_task 期望连续流式的关节命令（示例以 100 Hz 发送）。停止发送后，未被命令的关节会因重力缓慢下垂（1 s 内可下垂约 0.09 rad）；要么持续回发全部受控组，要么尽快切回状态 3。
腰部（waist，5 自由度）在本示例中只原样回发当前值，不做偏移插值。腰部安全默认位置为 [-1.25, 2.15, -0.918, 0, 0]，禁止将腰部全部回零。

Pythonimport os
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")

from fourier_aurora_client import AuroraClient

ARM_OFFSET = 0.03      # 单关节偏移上限（rad）
STEPS = 200            # 插值步数
DT = 0.01              # 命令周期 10 ms（100 Hz）


def wait_fsm(client, expected, timeout=4.0):
    deadline = time.monotonic() + timeout
    while client.get_fsm_state() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"状态未在 {timeout}s 内切换到 {expected}")
        time.sleep(0.05)


def interpolate(client, current, target, steps=STEPS, dt=DT):
    for index in range(steps + 1):
        ratio = index / steps
        cmd = {group: [a + (b - a) * ratio for a, b in zip(current[group], target[group])]
               for group in target}
        client.set_group_cmd(cmd)
        time.sleep(dt)


client = AuroraClient.get_instance(domain_id=123, robot_name="grw")
original = None
try:
    time.sleep(1.0)

    # Step 1: 从状态 3 进入 upper_body_tele_task（状态 4），并回读确认
    input("Press Enter to set FSM to upper_body_tele_task (state 4)...")
    if not client.set_fsm_state(4):
        raise RuntimeError("状态切换请求未受理")
    wait_fsm(client, 4)

    # Step 2: 读取当前各控制组位置
    original = {
        "left_manipulator":  client.get_group_state("left_manipulator", key="position"),
        "right_manipulator": client.get_group_state("right_manipulator", key="position"),
        "waist":             client.get_group_state("waist", key="position"),
    }
    assert len(original["left_manipulator"]) == 7
    assert len(original["right_manipulator"]) == 7
    assert len(original["waist"]) == 5
    print(f"original positions: {original}")

    # Step 3: 目标 = 当前值 + 有限偏移（仅双臂 joint 0），腰部保持当前值
    input("Press Enter to move both arm joint0 by +0.03 rad...")
    target = {group: list(values) for group, values in original.items()}
    target["left_manipulator"][0] += ARM_OFFSET
    target["right_manipulator"][0] += ARM_OFFSET

    interpolate(client, original, target)     # 去程
    time.sleep(0.3)

    # Step 4: 回到起始位置
    input("Press Enter to return to the original position...")
    start = {
        group: client.get_group_state(group, key="position") for group in original
    }
    interpolate(client, start, original)      # 回程
    print("Joint control demonstration complete")
finally:
    if original is not None:
        try:
            now = {group: client.get_group_state(group, key="position") for group in original}
            interpolate(client, now, original, steps=100)
        except Exception as error:
            print(f"recovery interpolation failed: {error}")
    if client.get_fsm_state() != 3:
        client.set_fsm_state(3)               # 回到策略站立状态
        wait_fsm(client, 3)
    client.close()

行为说明：

状态 3 → 4 切换受理后约 50 ms 内状态反馈更新；状态 4 下 get_upper_fsm_state() 仍为 0（Default），set_upper_fsm_state() 在状态 3/4 下均返回 False，上半身遥操作一律通过整机状态 4 进入。
双臂 joint 0 命令 +0.03 rad，命令流结束后实际到达约 +0.024 ~ +0.026 rad（存在小的跟踪滞后）。
get_group_motion_state() 在 100 Hz 流式插值期间保持 0（idle），不能用它判断遥操作命令引起的运动是否完成。