关节空间运动完整示例

本示例演示使用 MoveTools 执行双臂关节空间点到点运动：连接 GR-Controller，切换到上肢可接管状态，启动 MoveTools，然后调用 move_joint() 下发左右臂目标关节角。

涉及状态： PD Stand、WBC、RemoteState

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

Python

import time

import numpy as np
from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession


left_target_q = np.array([0.7, 0.6, 0.1, -1.5, 0.0, 0.0, 0.0])
right_target_q = np.array([0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0])


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, robot_name="gr3")
movetools = MoveToolsSession(client, robot_name="gr3")

try:
    input("Press Enter to switch to PD Stand...")
    client.set_fsm_state(2)
    time.sleep(1.0)

    input("Press Enter to switch to WBC and RemoteState...")
    client.set_fsm_state(3)
    time.sleep(0.5)
    client.set_upper_fsm_state(2)
    time.sleep(0.5)

    movetools.start()

    input("Press Enter to run move_joint...")
    result = movetools.move_joint(
        groups=["left_manipulator", "right_manipulator"],
        target_q={
            "left_manipulator": left_target_q,
            "right_manipulator": right_target_q,
        },
        expect_vel=300,
    )
    check_result(result, "move_joint")

    movetools.wait_until_idle()
    movetools.raise_if_error()
    print("move_joint complete")

finally:
    movetools.stop()
    client.close()