多段关节运动完整示例

本示例演示使用 move_sequence() 一次提交两段关节空间运动。所有段共享同一组 groups，每段通过 seq.add_move_joint() 添加。

涉及状态： PD Stand、WBC、RemoteState

涉及接口： create_move_sequence_builder()、seq.add_move_joint()、move_sequence()

Python

import time

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


waist_zero_q = np.array([0.0, 0.0, 0.0])
left_pre_q = np.array([0.7, 0.6, 0.1, -1.5, 0.0, 0.0, 0.0])
right_pre_q = np.array([0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0])
left_end_q = np.array([-0.7, 0.6, 0.4, -1.2, 0.0, 0.0, 0.0])
right_end_q = np.array([-0.7, -0.6, -0.4, -1.2, 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()

    seq = movetools.create_move_sequence_builder(
        groups=["waist", "left_manipulator", "right_manipulator"],
    )
    seq.add_move_joint(
        {
            "waist": waist_zero_q,
            "left_manipulator": left_pre_q,
            "right_manipulator": right_pre_q,
        },
        expect_vel=300,
    )
    seq.add_move_joint(
        {
            "waist": waist_zero_q,
            "left_manipulator": left_end_q,
            "right_manipulator": right_end_q,
        },
        expect_vel=300,
    )

    input("Press Enter to run move_sequence...")
    result = movetools.move_sequence(seq)
    check_result(result, "move_sequence")

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

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