任务配置

task 块以声明式方式定义一段完整的任务流程——由一系列 动作（action） 顺序执行，最终由一组 目标条件（goal） 验收。TaskManager 在运行时将配置转换为可执行的 Task 实例。

源码：fastsim/task/task_cfg.py（TaskConfig）、fastsim/task/task.py（Task）

执行语义

text

Task.execute()
  │
  ├─ for action in actions:
  │     action.act()                        ← 执行动作
  │     action.goal.is_satisfied()          ← 动作后检查内置目标
  │     [失败?] → 重试机制 / 交互调试        ← 见 RetryConfig
  │
  └─ for goal in goals:
        goal.is_satisfied()                 ← 最终验收

所有 actions 按列表顺序依次执行。每个 action 可选地携带一个 goal_config，在该 action 执行后立即校验；task.goals 是在所有 actions 完成后执行的最终验收条件。

TaskConfig 字段

字段	类型	默认值	说明
`name`	`str`	—	任务名称
`description`	`str`	—	任务描述
`actions`	`list[ActionConfig]`	`[]`	动作列表，顺序执行
`goals`	`list[GoalConfig]`	`[]`	最终目标列表，全部满足才算任务成功
`retry_config`	`RetryConfig`	`null`	全局重试配置，作用于所有 action（各 action 可用自身 `retry_config` 覆盖）

ActionConfig 字段

每个 action 通过 stereotype 确定具体的行为类型（如 pick、place、move、navigate），由 ActionController.execute_action() 实例化并执行。

源码：fastsim/extensions/common/action/action.py

字段	类型	默认值	说明
`stereotype`	`str`	—	动作类型（如 `pick` / `place` / `move` / `grasp` / `navigate`）
`name`	`str`	—	动作名称
`description`	`str`	—	动作描述
`robot_name`	`str`	—	执行该动作的机器人实例名
`goal_config`	`GoalConfig`	`null`	动作执行后的即时校验目标（可选）
`visualization_config`	`VisualizationConfig`	—	可视化开关（规划路径、末端位姿等）
`wait_robot_stopped`	`bool`	`false`	执行前是否等待机器人静止
`wait_robot_stopped_timeout`	`float`	`3.0`	等待超时时间（秒）
`save_snapshot`	`bool`	`false`	执行前是否保存场景快照
`snapshot_dir`	`str`	`null`	快照保存目录
`retry_config`	`RetryConfig`	`null`	该动作的重试配置（覆盖 task 级别配置）
`extra_parameters`	`dict`	`{}`	具体 stereotype 的扩展参数（由各 Action 子类解释）

GoalConfig 字段

GoalConfig 通过 stereotype 确定条件类型，例如 pose（末端位姿到达）、on_top（物体叠放）、inside（物体在容器内）、grasped（抓取成功）。

源码：fastsim/extensions/common/goal/goal.py

通用字段（所有 goal 共有）

字段	类型	默认值	必填	说明
`stereotype`	`str`	—	是	目标类型，必须是 `pose` / `on_top` / `inside` / `grasped` / `articulated_joint_state` 之一
`name`	`str`	`"no_name_provided"`	是	目标名称（在成功/失败日志中显示，建议起描述性名称便于排查）
`description`	`str`	`"no_description_provided"`	是	自由文本描述
`extra_parameters`	`dict`	`{}`	否	用户自定义扩展参数，built-in goal 不消费

Goal 在配置中的三个使用位置

位置	数量	检查时机	用途
`action.goal_config`	单个，可选	该 action 的 `execute()` 完成后立即检查	单步校验；失败可触发 retry
`task.goals`	列表，可选	所有 actions 完成后顺序检查	任务整体成功标准
`benchmark.goals`	列表，每 episode	benchmark 每个 episode 末尾检查	无人值守评测

检查顺序与失败处理：每个 action 执行后会先看 action.action_fail（执行异常）；若执行成功再检查 action.goal_config。两类失败都会进入 RetryConfig 系统，可通过 RetryRule.condition 区分（execution_failed vs goal_not_satisfied）。

通用单位约定

距离 / 位置容差：米
旋转角度 / 角度容差：弧度（不是度数 —— 常见 bug）
四元数：wxyz 顺序（不是 xyzw —— 常见 bug）
关节位置：关节本身的单位（旋转副 = 弧度，移动副 = 米）

PoseGoal — 位姿目标（stereotype: `pose`）

最通用的目标类型：检查任意两个位姿之间的距离是否在容差内。"位姿"可以是机器人末端、场景实体、或一个内联的字面位姿。

源码：fastsim/extensions/common/goal/goals/pose_goal.py

字段	类型	默认值	说明
`position_tolerance`	`float`	`null`	位置容差（米），`null` 时跳过位置检查（必须 ≥ 0）
`rotation_tolerance`	`float`	`null`	旋转容差（弧度），`null` 时跳过旋转检查（必须 ≥ 0）
`position_offset`	`list[3]`	`[0, 0, 0]`	在 pose B 的本体坐标系中应用到 B 的位置偏移
`rotation_offset`	`list[4]`	`[1, 0, 0, 0]`	应用到 B 的四元数偏移（wxyz）
`pose_A_source`	`enum`	`"ee"`	pose A 来源：`ee` / `spawnable` / `pose`
`pose_A_params`	`dict`	`{}`	pose A 查询参数（取决于 source）
`pose_B_source`	`enum`	`"spawnable"`	pose B 来源
`pose_B_params`	`dict`	`{}`	pose B 查询参数

pose_*_params 按 source 分发：

source	必填 params	可选 params	说明
`ee`	`robot_name: str`	`arm_name: str`	末端执行器位姿；多臂 ModularRobot 时指定 `arm_name`；强制使用世界坐标系
`spawnable`	`spawnable_name: str`	—	场景实体位姿；移动机器人会先尝试 `get_robot_navigation_pose`
`pose`	取决于 type	`type` 字段（`quaternion` / `rotation_matrix` / `homogeneous_matrix` / `euler_xyz` / `rot6d`）	字面量位姿，根据 type 解析对应数据

判定逻辑：

解析 pose A 和 pose B（均在世界坐标系）
用 position_offset + rotation_offset 构造偏移 pose（视为相对 B 的本体）
计算 offset_pose_B = pose_B * offset_pose
位置检查：欧氏距离 |pose_A.position - offset_pose_B.position| ≤ position_tolerance
旋转检查：测地线角误差 ≤ rotation_tolerance
两个 tolerance 都为 null 时 目标永远满足（仅打印 info 日志）—— 易踩坑

示例 1：检查末端到达门把手附近

yaml

goals:
  - stereotype: pose
    name: ee_at_handle
    description: 末端在门把手上方 5cm、5° 范围内
    position_tolerance: 0.01      # 1cm
    rotation_tolerance: 0.0872    # 约 5° 转弧度
    position_offset: [0.0, 0.0, 0.05]    # 把手本体坐标系上方 5cm
    rotation_offset: [1.0, 0.0, 0.0, 0.0]
    pose_A_source: ee
    pose_A_params:
      robot_name: panda
      # arm_name: left_arm     # ModularRobot 多臂时填
    pose_B_source: spawnable
    pose_B_params:
      spawnable_name: door_handle

示例 2：检查物体到达世界坐标系中的字面位姿

yaml

  - stereotype: pose
    name: cube_at_target
    position_tolerance: 0.02
    rotation_tolerance: null      # 不检查旋转
    pose_A_source: spawnable
    pose_A_params:
      spawnable_name: red_cube
    pose_B_source: pose
    pose_B_params:
      type: quaternion
      position: [0.5, 0.2, 0.1]
      rotation: [1.0, 0.0, 0.0, 0.0]

易错点：

position_offset 是在 B 的本体坐标系，B 旋转时偏移会跟着转。如需世界坐标偏移，让 B 是固定姿态的实体或用字面 pose
rotation_tolerance 是弧度，不是度
rotation_offset 默认 [1, 0, 0, 0] = 单位四元数（wxyz）
两个 tolerance 都为 null → 目标恒为真，写完 YAML 检查一下

OnTopGoal — 物体堆叠目标（stereotype: `on_top`）

检查 A 物体是否堆叠在 B 物体之上。基于 AABB 包围盒做垂直 + 水平判定。

源码：fastsim/extensions/common/goal/goals/on_top_goal.py

字段	类型	默认值	说明
`object_A_name`	`str`	—	必填。被检查物体名称（应在上方）
`object_B_name`	`str`	—	必填。参考物体名称（在下方）
`max_horizontal_offset`	`float`	`0.01`	水平中心距容差（米），仅在 `horizontal_check_mode: center_distance` 时使用
`max_vertical_offset`	`float`	`0.02`	垂直间隙容差（米）= A 底部 - B 顶部
`horizontal_check_mode`	`enum`	`"inside_xy"`	水平判定模式：`center_distance` / `inside_xy`

horizontal_check_mode 详解：

inside_xy：A 的中心 (XY) 必须落在 B 的 XY 包围盒内（默认，适合"杯子在盘子上"这类大托盘场景）；max_horizontal_offset 被忽略
center_distance：A 与 B 的 XY 中心距离 ≤ max_horizontal_offset（适合"小方块叠在大方块中心"这类对齐场景）

判定逻辑：

取两物体的世界 AABB
delta_z = min(corners_A.z) - max(corners_B.z)，若 > max_vertical_offset 则失败（A 浮太高）
注意：delta_z < 0（A 嵌入 B 内部）不会失败
水平检查按上述模式

示例：

yaml

  - stereotype: on_top
    name: cup_on_plate
    object_A_name: red_cup
    object_B_name: dinner_plate
    max_vertical_offset: 0.02         # 杯子允许悬浮 2cm 之内
    horizontal_check_mode: inside_xy  # 杯心必须在盘内

  - stereotype: on_top
    name: small_block_centered
    object_A_name: small_cube
    object_B_name: big_cube
    max_vertical_offset: 0.005
    max_horizontal_offset: 0.01       # 中心偏差不超过 1cm
    horizontal_check_mode: center_distance

易错点：

包围盒是 世界轴对齐（AABB），物体倾斜后 AABB 会变大
inside_xy 只看 A 的中心，A 一半悬空也算通过
delta_z 不做下界检查 —— 如果 A 穿入 B，物理上不合理但目标不报错

InsideGoal — 物体内部目标（stereotype: `inside`）

检查 A 物体是否完全在 B 物体的 AABB 内（严格容器约束）。

源码：fastsim/extensions/common/goal/goals/inside_goal.py

字段	类型	默认值	说明
`object_A_name`	`str`	—	必填。被检查物体（应在内部）
`object_B_name`	`str`	—	必填。容器物体
`max_offset`	`float`	`0.0`	单边突出容差（米），默认严格容纳

判定逻辑：A 的 8 个角点 全部落在 B 的 AABB（沿三个轴向各扩 max_offset）内才算通过。任何一个角点在任何一个轴上越界即失败。

示例：

yaml

  - stereotype: inside
    name: ball_in_box
    object_A_name: tennis_ball
    object_B_name: cardboard_box
    max_offset: 0.005   # 允许每个轴向超出 5mm

易错点：

严格 AABB 包含 —— 笔从杯口伸出来一截就会失败
旋转的容器（如倾斜的盒子）AABB 会扩大，可能导致非预期通过
max_offset = 0 时浮点抖动可能让结果反复横跳，建议留几毫米余量

GraspedGoal — 抓取成功目标（stereotype: `grasped`）

通过 EE 与物体中心的距离判定抓取是否成功（启发式，非接触级别）。

源码：fastsim/extensions/common/goal/goals/grasped_goal.py

字段	类型	默认值	说明
`robot_name`	`str`	—	必填。持有物体的机器人
`arm_name`	`str`	`null`	ModularRobot 多臂时指定，单臂可省略
`object_name`	`str`	—	必填。被抓物体名称
`max_distance`	`float`	`0.08`	EE 到物体中心的最大距离（米），必须 > 0

判定逻辑：|EE_world_pos - object_world_pos| ≤ max_distance 即视为抓取成功。

示例：

yaml

  - stereotype: grasped
    name: hold_apple
    robot_name: panda
    object_name: red_apple
    max_distance: 0.08

  # ModularRobot 多臂场景
  - stereotype: grasped
    name: left_hand_holds_cup
    robot_name: dual_arm_robot
    arm_name: left_arm
    object_name: white_cup
    max_distance: 0.06

易错点：

基于距离的启发式，不读夹爪状态也不查接触力 —— 物体掉到 EE 附近也会"通过"
max_distance 默认 8cm 偏宽松，小物体应缩紧、大件物体应放宽
距离测的是 EE 原点 ↔ 物体中心，长条物从一端被抓时中心可能离 EE 较远
max_distance 必须严格 > 0

ArticulatedJointStateGoal — 关节体状态目标（stereotype: `articulated_joint_state`，新增）

检查关节体的指定关节是否到达目标位置。适用于"门是否打开 90°"、"抽屉是否拉出 20cm"这类场景。

源码：fastsim/extensions/common/goal/goals/articulated_joint_state_goal.py

字段	类型	默认值	说明
`object_name`	`str`	—	必填。关节体名称
`joint_positions`	`dict[str, float]`	`{}`	必填且非空。关节名 → 目标位置的映射
`tolerance`	`float`	`0.001`	每关节绝对误差容差（同关节单位），必须 ≥ 0

判定逻辑：

查询 object_name 上 joint_positions 中所有关节的当前位置
任一关节 |actual - target| > tolerance 即失败
返回信息会列出所有违反者（actual / target / error / tolerance）

示例：

yaml

  # 单关节：柜门完全打开
  - stereotype: articulated_joint_state
    name: door_open
    object_name: cabinet_left
    joint_positions:
      left_door_hinge: 1.5708   # 90° = π/2 弧度
    tolerance: 0.05             # 约 3° 余量

  # 多关节：抽屉 + 门同时检查
  - stereotype: articulated_joint_state
    name: cabinet_fully_configured
    object_name: kitchen_cabinet
    joint_positions:
      drawer_slide: 0.20        # 移动副，单位为米
      door_hinge:   0.0         # 旋转副，单位为弧度（关闭）
    tolerance: 0.01

易错点：

joint_positions 不能为空，空 dict 校验会报错
关节名必须是 USD/URDF 中的精确名称，拼错会触发"长度不匹配"失败
单位是 关节本身的单位：旋转副 = 弧度，移动副 = 米
tolerance 是 单一标量，所有关节共用；不同关节需要不同容差时拆成多个 goal
仅适用于 articulated 类型物体，rigid 物体调用会在 controller 层报错

Goal 在 action 与 task 配置中的完整示例

yaml

task:
  name: pick_and_place
  retry_config:
    rules:
      - condition: all
        retry_times: 2

  actions:
    - stereotype: pick
      name: pick_cube
      robot_name: arm
      arm_name: main_arm
      # action 级别 goal_config：单 goal，pick 完成后立即检查
      # 失败会触发 retry（依规则可重试或回滚）
      goal_config:
        stereotype: grasped
        name: grasp_check
        robot_name: arm
        object_name: red_cube
        max_distance: 0.05

    - stereotype: place
      name: place_cube
      robot_name: arm
      arm_name: main_arm
      goal_config:
        stereotype: pose
        name: ee_at_drop
        position_tolerance: 0.02
        rotation_tolerance: null
        pose_A_source: ee
        pose_A_params: { robot_name: arm, arm_name: main_arm }
        pose_B_source: spawnable
        pose_B_params: { spawnable_name: tray }

  # task 级别 goals：列表，所有 actions 跑完后依次检查，全部通过才算任务成功
  goals:
    - stereotype: on_top
      name: cube_on_tray
      object_A_name: red_cube
      object_B_name: tray
      max_vertical_offset: 0.02
      horizontal_check_mode: inside_xy

    - stereotype: grasped
      name: not_holding_anymore
      robot_name: arm
      object_name: red_cube
      max_distance: 1.0           # 仅当物体远离 EE 才算"放下"
      # 注意：此处用 grasped + 大 max_distance 的写法不通用
      # 需要"未抓持"语义时建议自己实现自定义 goal

重试机制（RetryConfig）

FastSim 的任务系统内置了自动重试机制，当 action 执行失败或 goal 校验不通过时，可以自动回跳到指定的 action 重新执行，无需人工干预。

RetryConfig 字段

字段	类型	默认值	说明
`retry_times`	`int`	`0`	最大重试次数，0 表示不重试
`retry_from`	`str`	`null`	重试时跳回的 action 名称；`null` 则重试失败的当前 action
`retry_condition`	`enum`	`all`	触发重试的条件（见下表）

retry_condition 选项

值	含义
`all`	任何失败都触发重试（默认）
`goal_not_satisfied`	仅当 action 执行成功但 goal 校验不通过时触发
`execution_failed`	仅当 action 本身执行失败（异常或超时）时触发

配置层级

RetryConfig 支持两级配置，action 级别优先于 task 级别：

yaml

task:
  name: pick_and_place
  # 全局重试：所有 action 默认重试 2 次
  retry_config:
    retry_times: 2
    retry_condition: all

  actions:
    - stereotype: pick
      name: pick_cube
      robot_name: arm
      # 该 action 使用自己的重试配置（覆盖全局）
      retry_config:
        retry_times: 3
        retry_from: open_gripper   # 失败时跳回 open_gripper 重新执行
        retry_condition: goal_not_satisfied

    - stereotype: place
      name: place_cube
      robot_name: arm
      # 该 action 不设 retry_config → 使用 task 级别的全局配置

重试执行流程

text

action 执行
    │
    ├─ 成功 → 继续下一个 action
    │
    └─ 失败
        │
        ├─ 检查 retry_condition 是否匹配失败类型
        │   ├─ 不匹配 → 任务失败
        │   └─ 匹配 → 检查重试次数
        │
        ├─ retry_count < retry_times？
        │   ├─ 否 → 任务失败（"after N retries"）
        │   └─ 是 → 执行重试
        │
        └─ 重试执行
            ├─ 计算跳回位置（retry_from 指定的 action 或当前 action）
            ├─ 重置跳回范围内所有 action 的失败状态
            ├─ 回滚执行结果记录
            └─ 从跳回位置重新开始执行

retry_from 跳回规则

retry_from: null（默认）：重试失败的当前 action 本身
retry_from: "action_name"：跳回指定名称的 action，从该 action 开始重新执行后续所有 action
跳回目标必须在当前 action 之前（不能向后跳）
跳回范围内的所有 action 的失败状态会被重置

重试上下文（RetryContext）

重试执行时，每个 action 的执行结果中会附带 RetryContext 信息，可通过 task_observer 观测：

字段	类型	说明
`attempt`	`int`	当前重试次数（1-indexed）
`max_retries`	`int`	最大重试次数
`triggered_by`	`str`	触发重试的 action 名称
`triggered_by_reason`	`str`	失败原因
`retry_from`	`str`	跳回到的 action 名称

重试配置示例

示例 1：简单重试（重试当前 action）

yaml

task:
  name: grasp_retry
  actions:
    - stereotype: pick
      name: pick_object
      robot_name: arm
      arm_name: main_arm
      retry_config:
        retry_times: 3           # 最多重试 3 次
        retry_condition: all     # 任何失败都重试
      goal_config:
        stereotype: grasped
        name: check_grasp
        robot_name: arm
        object_name: target_obj
        max_distance: 0.08

示例 2：跳回重试（从准备动作开始重来）

yaml

task:
  name: pick_and_place_with_retry
  actions:
    - stereotype: release
      name: open_gripper
      robot_name: arm
      arm_name: main_arm

    - stereotype: pick
      name: pick_cube
      robot_name: arm
      arm_name: main_arm
      retry_config:
        retry_times: 3
        retry_from: open_gripper        # 失败时跳回 open_gripper
        retry_condition: goal_not_satisfied  # 仅目标未满足时重试
      goal_config:
        stereotype: grasped
        name: grasp_check
        robot_name: arm
        object_name: red_cube

    - stereotype: place
      name: place_cube
      robot_name: arm
      arm_name: main_arm

在此示例中，如果 pick_cube 的 grasped 目标检查未通过（物体未被抓住），系统会自动跳回 open_gripper，重新打开夹爪后再次尝试抓取，最多重试 3 次。

完整配置示例

yaml

task:
  name: pick_and_place
  description: Pick the red cube and place it on the tray
  retry_config:
    retry_times: 2
    retry_condition: all

  actions:
    - stereotype: pick
      name: pick_cube
      description: Pick up the red cube
      robot_name: arm
      arm_name: main_arm
      wait_robot_stopped: true
      goal_config:
        stereotype: grasped
        name: grasp_check
        robot_name: arm
        object_name: cube
      visualization_config:
        enable_target_bounding_box_visualization: true

    - stereotype: place
      name: place_cube_on_tray
      description: Place the cube onto the tray
      robot_name: arm
      arm_name: main_arm

  goals:
    - stereotype: on_top
      name: cube_on_tray
      description: The cube is on the tray
      object_A_name: cube
      object_B_name: tray
      max_horizontal_offset: 0.01
      max_vertical_offset: 0.02