笛卡尔直线完整示例

本示例演示使用 move_line() 让双臂末端沿 world frame 的 z 方向直线抬升 5 cm。示例先用 move_point() 到达确定起点，再读取当前末端 base frame 位姿，转换到 world frame 后构造直线目标。

涉及状态： PD Stand、WBC、RemoteState

涉及接口： move_point()、move_line()、convert_target_poses()、get_cartesian_state()

Python

import time

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession


left_start_pose = [0.469241, 0.305731, 0.186122, -0.05, -0.74, -0.04, 0.67]
right_start_pose = [0.44372, -0.213381, 0.186122, 0.05, -0.74, 0.04, 0.67]


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


def wait_and_check(movetools, name):
    movetools.wait_until_idle()
    movetools.raise_if_error()
    print(f"{name} complete")


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 move to the Cartesian start pose...")
    result = movetools.move_point(
        groups=["waist", "left_manipulator", "right_manipulator"],
        target_poses={
            "left_end_effector_link": left_start_pose,
            "right_end_effector_link": right_start_pose,
        },
        expect_vel=300,
        frame="base",
    )
    check_result(result, "prepare move_point")
    wait_and_check(movetools, "prepare move_point")

    left_base = client.get_cartesian_state("left_manipulator", "pose")
    right_base = client.get_cartesian_state("right_manipulator", "pose")
    world_poses = movetools.convert_target_poses(
        {
            "left_end_effector_link": left_base,
            "right_end_effector_link": right_base,
        },
        from_frame="base",
        to_frame="world",
    )

    left_world = list(world_poses["left_end_effector_link"])
    right_world = list(world_poses["right_end_effector_link"])
    left_world[2] += 0.05
    right_world[2] += 0.05

    input("Press Enter to run move_line...")
    result = movetools.move_line(
        groups=["waist", "left_manipulator", "right_manipulator"],
        target_poses={
            "left_end_effector_link": left_world,
            "right_end_effector_link": right_world,
        },
        expect_vel=100,
        frame="world",
    )
    check_result(result, "move_line")
    wait_and_check(movetools, "move_line")

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