多段笛卡尔运动完整示例

本示例演示使用 move_sequence() 组合一段 move_point 和多段 move_line。示例中的末端位姿均按 base frame 解释，位置单位为 m，四元数顺序为 xyzw。

涉及状态： PD Stand、WBC、RemoteState

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

Python

import time

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession


left_pose_a = [0.38372, 0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
right_pose_a = [0.38372, -0.3, 0.186122, 0.05, -0.74, 0.04, 0.67]
left_pose_b = [0.28372, 0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
right_pose_b = [0.28372, -0.3, 0.186122, 0.05, -0.74, 0.04, 0.67]
left_pose_c = [0.28372, 0.3, 0.386122, -0.05, -0.74, -0.04, 0.67]
right_pose_c = [0.28372, -0.3, 0.386122, 0.05, -0.74, 0.04, 0.67]
left_pose_d = [0.38372, 0.3, 0.386122, -0.05, -0.74, -0.04, 0.67]
right_pose_d = [0.38372, -0.3, 0.386122, 0.05, -0.74, 0.04, 0.67]


def check_result(result, name):
    if not result.success:
        detail = f"segment={result.failed_segment_index}" if result.failed_segment_index >= 0 else ""
        raise RuntimeError(f"{name} failed: {result.code} {result.message} {detail}")


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=["left_manipulator", "right_manipulator"],
    )
    seq.add_move_point(
        target_poses={
            "left_end_effector_link": left_pose_a,
            "right_end_effector_link": right_pose_a,
        },
        expect_vel=100,
        frame="base",
    )

    for left_pose, right_pose in [
        (left_pose_b, right_pose_b),
        (left_pose_c, right_pose_c),
        (left_pose_d, right_pose_d),
        (left_pose_a, right_pose_a),
    ]:
        seq.add_move_line(
            {
                "left_end_effector_link": left_pose,
                "right_end_effector_link": right_pose,
            },
            expect_vel=200,
            frame="base",
        )

    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()