多段笛卡尔运动完整示例

本示例演示使用 move_sequence() 组合一段 move_point 和一段 move_line：先用 base frame 到点运动将左臂末端抬高 3 cm，再用 tool frame 直线段做小位移后返回。目标均由当前末端位姿加有限偏移生成，位置单位为 m，四元数顺序为 xyzw。

涉及状态： JointStand、upper_body_tele_task

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

Python

import math
import os
import time

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

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession

Z_OFFSET = 0.03            # base frame 抬升（m）
TOOL_DELTA = [0.0, 0.0, 0.02]  # tool frame 小位移（m）


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}")


def normalized_pose(pose):
    pose = list(pose)
    norm = math.sqrt(sum(q * q for q in pose[3:]))
    return pose[:3] + [q / norm for q in pose[3:]]


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_pose = normalized_pose(client.get_cartesian_state("left_manipulator", "pose"))
    print(f"start pose: {[round(v, 4) for v in start_pose]}")

    up_pose = start_pose[:2] + [start_pose[2] + Z_OFFSET] + start_pose[3:]

    seq = movetools.create_move_sequence_builder(groups=["left_manipulator"])

    # 段 1：base frame 到点抬升 3 cm
    seq.add_move_point(
        {"left_end_effector_link": up_pose},
        expect_vel=150,
        frame="base",
    )
    # 段 2：tool frame 直线小位移
    seq.add_move_line(
        {"left_end_effector_link": TOOL_DELTA + [0.0, 0.0, 0.0, 1.0]},
        expect_vel=120,
        frame="tool",
    )
    # 段 3：回到起始位姿
    seq.add_move_point(
        {"left_end_effector_link": start_pose},
        expect_vel=150,
        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()

    end_pose = normalized_pose(client.get_cartesian_state("left_manipulator", "pose"))
    print(f"restored z delta: {round(end_pose[2] - start_pose[2], 4)}")
    print("move_sequence complete")

finally:
    if started:
        movetools.stop()
    if client.get_fsm_state() != 3:
        client.set_fsm_state(3)   # 回到策略站立状态
        wait_fsm(client, 3)
    client.close()