跳到主要内容

动作编排实践

该文档以一个完整的运控-操作一体化场景为例，演示如何使用GR-Controller运动控制与 MoveTools 上肢操作编排为一个完整流程。

其中运动控制场景涵盖状态机切换、速度指令设置、机器人姿态控制等。 上肢操作场景涵盖关节空间运动、笛卡尔到点、笛卡尔直线和多段连续运动四种运动接口，以及 base frame 与 world frame 两种参考系的切换。

涉及接口： set_fsm_state()、set_upper_fsm_state()、set_velocity_source()、set_velocity()、set_stand_pose()、move_joint()、move_point()、move_line()、move_sequence()、convert_target_poses()

涉及状态： PD Stand → WBC Policy→ RemoteState

前置条件​

  1. GR-Controller（AuroraCore）已在机器人侧运行。
  2. AuroraClient 已成功连接机器人，可读写状态。
  3. 机器人处于安全的初始站立姿态，周围无障碍物。

场景概览​

整个流程分为以下阶段：

阶段操作说明
1连接机器人创建 AuroraClient 和 MoveToolsSession
2切换状态机PD Stand → WBC policy，上肢切到 act state
3下肢运动设置速度源，向前行走
4move_joint启动 MoveTools，双臂移动到预备位姿，腰部归零
5move_point双臂运动到抓取目标（base frame）
6move_line双臂沿 world frame z 轴直线抬升 10 cm
7姿态控制 + 下肢运动降低站姿高度，后退，恢复站姿
8move_sequence双臂矩形路径（move_point + 多段 move_line）
9退出停止 MoveTools，关闭连接

常量定义​

Python
import numpy as np

LEFT_ARM_PRE_Q  = np.array([ 0.7,  0.6,  0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)
RIGHT_ARM_PRE_Q = np.array([ 0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)

LEFT_ARM_START_Q  = np.array([-0.7,  0.6,  0.4, -1.2, 0.0, 0.0, 0.0], dtype=np.float64)
RIGHT_ARM_START_Q = np.array([-0.7, -0.6, -0.4, -1.2, 0.0, 0.0, 0.0], dtype=np.float64)

WAIST_ZERO_Q = np.array([0.0, 0.0, 0.0], dtype=np.float64)

LEFT_GRASP_POSE  = [0.42372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
RIGHT_GRASP_POSE = [0.42372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]

笛卡尔位姿格式为 [x, y, z, qx, qy, qz, qw]，位置单位为 m，四元数顺序为 xyzw。

辅助函数​

Python
def check_move_result(result, name):
    if not result.success:
        detail = ""
        if result.failed_segment_index >= 0:
            detail += f", segment={result.failed_segment_index}"
        if result.failed_sample_index >= 0:
            detail += f", sample={result.failed_sample_index}, t={result.failed_sample_time_s:.4f}s"
        raise RuntimeError(f"[{name}] planning failed: {result.code} {result.message}{detail}")


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

check_move_result() 在规划失败时立即抛出异常，并附带失败段和采样点信息，便于排查具体问题。wait_and_check() 等待已入队轨迹执行完成并检查后台线程异常。

阶段 1：连接机器人​

Python
import time
from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession

client = AuroraClient.get_instance(
    domain_id=123,
    robot_name="gr3",
    namespace=None,
    is_ros_compatible=None,
)
time.sleep(1.0)
print("AuroraClient connected successfully.")

movetools = MoveToolsSession(client, robot_name="gr3")

AuroraClient.get_instance() 后等待 1 秒，确保 DDS 发现完成后再进行后续状态切换。

阶段 2：切换状态机​

MoveTools 不直接管理机器人状态机。启动 MoveTools 前，需要通过 AuroraClient 将机器人切换到可接管状态。

Python
# 切换到 PD Stand
client.set_fsm_state(2)
time.sleep(1.0)

# 切换到 WBC
client.set_fsm_state(3)
time.sleep(0.2)
状态fsm_state 值说明
PD Stand2直立稳定，准备进入 WBC
WBC3全身控制，允许上肢接管

阶段 3：下肢运动​

在将上肢控制权交给 MoveTools 之前，先完成下肢行走。

Python
# 设置速度源为导航模式
client.set_velocity_source(2)
time.sleep(0.2)

# 上肢切到 act state，使上肢随动
client.set_upper_fsm_state(1)
time.sleep(2.0)

# 向前行走 2 秒
client.set_velocity(1.0, 0.0, 0.0, duration=2.0)
time.sleep(2.5)

set_velocity_source(2) 将速度控制权交给外部导航命令。set_upper_fsm_state(1) 将上肢切到随动状态，使行走过程中上肢自然协同摆动。

阶段 4：move_joint — 启动 MoveTools 并运动到预备位姿​

下肢行走完成后，将上肢切到 RemoteState，启动 MoveTools，然后立即使用 move_joint() 将双臂和腰部运动到预备位姿。

Python
# 上肢切到 RemoteState，启动 MoveTools
client.set_upper_fsm_state(2)
time.sleep(0.5)
movetools.start()

result = movetools.move_joint(
    groups=["left_manipulator", "right_manipulator", "waist"],
    target_q={
        "left_manipulator":  LEFT_ARM_PRE_Q,
        "right_manipulator": RIGHT_ARM_PRE_Q,
        "waist":             WAIST_ZERO_Q,
    },
    expect_vel=300,
)
check_move_result(result, "move_joint")
wait_and_check(movetools, "move_joint")

movetools.start() 读取一帧当前机器人状态初始化内部规划状态，并启动MoveTools。此后所有运动规划请求均通过 MoveTools 下发。groups 同时包含双臂和腰部，三组关节在同一规划请求中并行运动。expect_vel=300 表示使用 30% 配置上限速度，适合初始接近阶段。

注意： set_upper_fsm_state(2) 和 movetools.start() 必须在下肢速度命令结束后执行，否则下肢运动期间上肢被 RemoteState 接管可能导致姿态异常。

阶段 5：move_point — 笛卡尔到点（base frame）​

使用 move_point() 将双臂末端运动到抓取目标位姿。末端路径不保证为直线。

Python
result = movetools.move_point(
    groups=["waist", "left_manipulator", "right_manipulator"],
    target_poses={
        "left_end_effector_link":  list(LEFT_GRASP_POSE),
        "right_end_effector_link": list(RIGHT_GRASP_POSE),
    },
    expect_vel=300,
    frame="base",
)
check_move_result(result, "move_point")
wait_and_check(movetools, "move_point")

frame="base" 表示目标位姿相对 base_link 解释，与 AuroraClient.get_cartesian_state() 返回的坐标系一致。groups 中加入 waist 使腰部参与 IK，扩大末端可达范围。

阶段 6：move_line — 笛卡尔直线（world frame）​

使用 move_line() 让双臂末端在 world frame 下沿 z 轴直线抬升 10 cm。需要先读取当前末端位姿并转换到 world frame。

Python
# 读取当前末端 base frame 位姿
left_base  = client.get_cartesian_state("left_manipulator",  "pose")
right_base = client.get_cartesian_state("right_manipulator", "pose")

# 转换到 world frame
world_poses = movetools.convert_target_poses(
    {
        "left_end_effector_link":  left_base,
        "right_end_effector_link": right_base,
    },
    from_frame="base",
    to_frame="world",
)

# 在 world frame 下抬升 10 cm
left_world  = list(world_poses["left_end_effector_link"])
right_world = list(world_poses["right_end_effector_link"])
left_world[2]  += 0.1
right_world[2] += 0.1

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_move_result(result, "move_line")
wait_and_check(movetools, "move_line")

frame="world" 使末端在世界坐标系下保持方向约束沿直线运动，适合对机器人姿态变化不敏感但需要空间精度的场景。expect_vel=100 使用较低速度，确保直线运动精度。

参考系选择： base 系目标随机器人姿态变化；world 系目标在空间中固定。直线抬升等需要绝对空间高度约束的任务应使用 world frame。

阶段 7：站姿调整与后退​

MoveTools 运行期间，仍可通过 AuroraClient 调整站姿高度和下肢速度。上肢保持 RemoteState 接管，不受影响。

Python
# 降低站姿 20 cm
client.set_stand_pose(delta_z=-0.2, delta_pitch=0.0, delta_yaw=0.0)
time.sleep(1.0)

# 向后行走 2 秒
client.set_velocity(vx=-1.0, vy=0.0, yaw=0.0, duration=2.0)
time.sleep(0.5)

# 恢复正常站姿高度
client.set_stand_pose(delta_z=0.0, delta_pitch=0.0, delta_yaw=0.0)
time.sleep(2.0)

set_stand_pose() 调整机器人整体站立高度，delta_z 单位为 m。set_velocity() 在下肢仍保持 WBC 状态下下发速度命令，与上肢 MoveTools 控制相互独立。

阶段 8：move_sequence — 矩形路径​

使用 move_sequence() 一次提交多段连续运动：先用 move_point 到达起点，再用四段 move_line 走一个矩形路径。

Python
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]

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

# 第一段：move_point 到达矩形起点
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",
)

# 第二到五段：move_line 依次走矩形路径
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",
    )

result = movetools.move_sequence(seq)
check_move_result(result, "move_sequence")
wait_and_check(movetools, "move_sequence")

move_sequence() 在提交时对所有段一次性规划，段与段之间自动连续过渡，不会有停顿。第一段使用 move_point（expect_vel=100）慢速到达确定起点，后续直线段使用较高速度（expect_vel=200）。

段类型起点 → 终点说明
1move_point当前位置 → pose_a关节空间到达矩形起点
2move_linepose_a → pose_b沿 x 轴向内直线
3move_linepose_b → pose_c沿 z 轴向上直线
4move_linepose_c → pose_d沿 x 轴向外直线
5move_linepose_d → pose_a沿 z 轴向下直线，回到起点

阶段 9：退出​

Python
movetools.stop()
client.close()

movetools.stop() 停止后台 bridge 线程。建议放在 finally 块中确保异常时也能正常退出。

Python
try:
    # ... 运动逻辑
finally:
    movetools.stop()
    client.close()

完整示例​

Python
import argparse
import time

import numpy as np

from fourier_aurora_client import AuroraClient
from movetools_session import MoveToolsSession


LEFT_ARM_PRE_Q   = np.array([ 0.7,  0.6,  0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)
RIGHT_ARM_PRE_Q  = np.array([ 0.7, -0.6, -0.1, -1.5, 0.0, 0.0, 0.0], dtype=np.float64)
WAIST_ZERO_Q     = np.array([0.0, 0.0, 0.0], dtype=np.float64)
LEFT_GRASP_POSE  = [0.42372,  0.3, 0.186122, -0.05, -0.74, -0.04, 0.67]
RIGHT_GRASP_POSE = [0.42372, -0.3, 0.186122,  0.05, -0.74,  0.04, 0.67]


def check_move_result(result, name):
    if not result.success:
        detail = ""
        if result.failed_segment_index >= 0:
            detail += f", segment={result.failed_segment_index}"
        if result.failed_sample_index >= 0:
            detail += f", sample={result.failed_sample_index}, t={result.failed_sample_time_s:.4f}s"
        raise RuntimeError(f"[{name}] planning failed: {result.code} {result.message}{detail}")


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


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-d", dest="domain_id", type=int, default=123)
    parser.add_argument("--robot-name", type=str, default="gr3")
    args = parser.parse_args()

    client = AuroraClient.get_instance(
        domain_id=args.domain_id,
        robot_name=args.robot_name,
        namespace=None,
        is_ros_compatible=None,
    )
    time.sleep(1.0)

    movetools = MoveToolsSession(client, robot_name=args.robot_name)

    try:
        # 状态机切换
        client.set_fsm_state(2)
        time.sleep(1.0)
        client.set_fsm_state(3)
        time.sleep(0.2)

        # 下肢行走
        client.set_velocity_source(2)
        time.sleep(0.2)
        client.set_upper_fsm_state(1)
        time.sleep(2.0)
        client.set_velocity(1.0, 0.0, 0.0, duration=2.0)
        time.sleep(2.5)

        # 启动 MoveTools
        client.set_upper_fsm_state(2)
        time.sleep(0.5)
        movetools.start()

        # move_joint
        result = movetools.move_joint(
            groups=["left_manipulator", "right_manipulator", "waist"],
            target_q={
                "left_manipulator":  LEFT_ARM_PRE_Q,
                "right_manipulator": RIGHT_ARM_PRE_Q,
                "waist":             WAIST_ZERO_Q,
            },
            expect_vel=300,
        )
        check_move_result(result, "move_joint")
        wait_and_check(movetools, "move_joint")

        # move_point
        result = movetools.move_point(
            groups=["waist", "left_manipulator", "right_manipulator"],
            target_poses={
                "left_end_effector_link":  list(LEFT_GRASP_POSE),
                "right_end_effector_link": list(RIGHT_GRASP_POSE),
            },
            expect_vel=300,
            frame="base",
        )
        check_move_result(result, "move_point")
        wait_and_check(movetools, "move_point")

        # move_line (world frame)
        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.1
        right_world[2] += 0.1
        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_move_result(result, "move_line")
        wait_and_check(movetools, "move_line")

        # 站姿调整 + 下肢后退
        client.set_stand_pose(delta_z=-0.2, delta_pitch=0.0, delta_yaw=0.0)
        time.sleep(1.0)
        client.set_velocity(vx=-1.0, vy=0.0, yaw=0.0, duration=2.0)
        time.sleep(0.5)
        client.set_stand_pose(delta_z=0.0, delta_pitch=0.0, delta_yaw=0.0)
        time.sleep(2.0)

        # move_sequence
        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]

        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",
            )
        result = movetools.move_sequence(seq)
        check_move_result(result, "move_sequence")
        wait_and_check(movetools, "move_sequence")

        print("Demo finished successfully.")

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


if __name__ == "__main__":
    main()

常见问题​

movetools规划失败查看原因​

check_move_result() 会打印 failed_segment_index 和 failed_sample_index，可以定位是哪一段、哪个采样点导致 IK 失败。常见原因是目标位姿超出可达范围或与当前姿态差距过大。

下肢移动后上肢目标偏移​

base frame 目标随机器人下肢移动。如果下肢运动期间上肢正在执行笛卡尔运动，目标的实际空间位置会随之漂移。解决方案：

  • 等待下肢完全停稳（速度命令结束后加足够 time.sleep）再下发上肢运动。

move_sequence 中某段规划失败​

check_move_result() 中检查 result.failed_segment_index，确认是哪一段失败。常见原因是相邻 move_line 段的连接处速度或关节状态不连续，可尝试降低 expect_vel 或调整中间路径点。

推荐参考​

  • modules/gr-controller/index.md
  • modules/gr-manipulation/index.md