关节空间运动完整示例

本示例演示使用 MoveTools 执行上身关节空间点到点运动：连接 GR-Controller，切换到上肢可接管状态，启动 MoveTools，然后调用 move_joint() 让双臂做相对当前姿态的小偏移运动并回到起点。目标由当前读取的关节位置加有限偏移生成，不使用硬编码绝对位姿（机器人零位不固定）。

涉及状态： JointStand、upper_body_tele_task

涉及接口： AuroraClient.get_instance()、set_fsm_state()、MoveToolsSession.start()、move_joint()、wait_until_idle()

Python

import os
import time

import numpy as np

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

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession

ARMS = ("left_manipulator", "right_manipulator")
JOINT_OFFSET = 0.05  # 单关节偏移（rad），保持在小幅范围内


def wait_fsm(client, expected, timeout_s=8.0):
    deadline = time.monotonic() + timeout_s
    while client.get_fsm_state() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"state {expected} not reached")
        time.sleep(0.05)


def check_result(result, name):
    if not result.success:
        raise RuntimeError(f"{name} failed: {result.code} {result.message}")


client = AuroraClient.get_instance(
    domain_id=123,  # 必须与当前 AuroraCore 的 DDS domain 一致
    robot_name="grw",
)
movetools = MoveToolsSession(client, robot_name="grw")
started = False

try:
    input("Press Enter to switch to JointStand...")
    client.set_fsm_state(1)
    wait_fsm(client, 1)
    time.sleep(2.0)

    input("Press Enter to switch to upper_body_tele_task...")
    client.set_fsm_state(4)
    wait_fsm(client, 4)
    time.sleep(1.0)

    movetools.start()
    started = True

    # 读取当前关节位置作为起点；腰部只回发当前值，不做偏移
    start = {arm: list(client.get_group_state(arm, key="position")) for arm in ARMS}
    waist_q = list(client.get_group_state("waist", key="position"))
    print(f"start positions: {start}")

    target = {arm: list(q) for arm, q in start.items()}
    for arm in ARMS:
        target[arm][0] += JOINT_OFFSET

    input("Press Enter to run move_joint...")
    result = movetools.move_joint(
        groups=["waist", "left_manipulator", "right_manipulator"],
        target_q={
            "waist": np.array(waist_q),
            "left_manipulator": np.array(target["left_manipulator"]),
            "right_manipulator": np.array(target["right_manipulator"]),
        },
        expect_vel=150,
    )
    check_result(result, "move_joint")

    movetools.wait_until_idle()
    movetools.raise_if_error()
    moved = {arm: list(client.get_group_state(arm, key="position")) for arm in ARMS}
    print(f"joint0 delta: {[round(moved[a][0] - start[a][0], 4) for a in ARMS]}")

    input("Press Enter to return to the start pose...")
    result = movetools.move_joint(
        groups=["waist", "left_manipulator", "right_manipulator"],
        target_q={
            "waist": np.array(waist_q),
            "left_manipulator": np.array(start["left_manipulator"]),
            "right_manipulator": np.array(start["right_manipulator"]),
        },
        expect_vel=150,
    )
    check_result(result, "move_joint back")
    movetools.wait_until_idle()
    movetools.raise_if_error()
    print("move_joint complete")

finally:
    if started:
        movetools.stop()
    if client.get_fsm_state() != 3:
        client.set_fsm_state(3)   # 回到策略站立状态，避免停在状态 4 导致手臂下垂
        wait_fsm(client, 3)
    client.close()