From 8dcec797cad9a9bdda662311e178f0a43344afd2 Mon Sep 17 00:00:00 2001 From: Richie6667 <1052996586@qq.com> Date: Wed, 22 Jul 2026 15:26:40 +0800 Subject: [PATCH 1/4] Implement code changes to enhance functionality and improve performance --- examples/act_policy_eval.py | 1124 +++++++++++++++++++++++++++++++++++ 1 file changed, 1124 insertions(+) create mode 100644 examples/act_policy_eval.py diff --git a/examples/act_policy_eval.py b/examples/act_policy_eval.py new file mode 100644 index 0000000..43ceeef --- /dev/null +++ b/examples/act_policy_eval.py @@ -0,0 +1,1124 @@ +"""把训练好的 lerobot ACT 模型接入 aao 仿真器做闭环评估。 + +改编自 ``policy_eval_example.py``：循环骨架完全一致，只是把 +``RecordedDemoPolicy``（回放录制动作）换成 ``ACTPolicyAdapter`` +（每步调用训练好的 ACT 模型出动作）。 + +数据流（单步）:: + + env.capture_observation() # aao 仿真观测 + -> 拼 observation.state(10) + 两路 RGB # 适配成 lerobot 输入 + -> policy.select_action(obs) -> action(8) # ACT 推理(内部带动作分块) + -> 拆成 position(3)+quat(4)+gripper(1) # 适配回 aao 动作 + -> env.apply_pose_action("arm", ...) # 驱动仿真 + +用法（在 auto-atomic-operation 根目录下运行）:: + + python examples/act_policy_eval.py \ + --checkpoint /home/richie/airdc/outputs/train/2026-06-03/12-14-11_act/checkpoints/last/pretrained_model \ + --config-name <你采集 aao_pick_data 时用的 task 配置名> \ + --batch-size 1 --num-rollouts 10 + +注意： +- ``--config-name`` 必须和你采集训练数据时用的 aao task 配置一致，否则 + 场景/相机/物体对不上。 +- state / action / image 的键与顺序必须和训练配置 (configs/config.yaml) 完全一致， + 见下方 STATE_KEYS / IMAGE_KEYS / 动作切分。 +""" + +from __future__ import annotations + +import argparse +import json +import time +from pathlib import Path +from typing import Any, Optional + +import numpy as np +import torch + +from auto_atom import ( + ExecutionContext, + PolicyEvaluator, + TaskUpdate, + load_task_file_hydra, +) +from lerobot.configs.policies import PreTrainedConfig +from lerobot.policies.act.modeling_act import ACTDecoderLayer, ACTEncoderLayer, ACTPolicy +from lerobot.policies.factory import make_pre_post_processors + + +# --- 必须与训练配置 configs/config.yaml 的 mcap.states / mcap.images 一致 --- +# observation.state = concat 这些键 -> 维度 3+6+1 = 10 +STATE_KEYS = [ + "arm/pose/position", # 3 + "arm/pose/rotation_6d", # 6 + "gripper/joint_state/position", # 1 +] +# observation.images. <- 仿真观测里的 color 键 +# 左边是仿真观测 key，右边是模型里 image feature 名（训练时由目录名推断） +IMAGE_KEYS = { + "wrist_cam/color/image_raw": "observation.images.wrist_cam", + "env0_cam/color/image_raw": "observation.images.env0_cam", +} +# action(8) 的切分，必须与 mcap.actions 顺序一致：position(3)+orientation/quat(4)+gripper(1) +ACT_POS = slice(0, 3) +ACT_QUAT = slice(3, 7) +ACT_GRIP = slice(7, 8) +VERTICAL_QUAT_XYZW = np.asarray([0.0, np.sqrt(0.5), 0.0, np.sqrt(0.5)], dtype=np.float32) + + +def _patch_lerobot_act_attention_no_weights() -> None: + """ACT does not consume attention weights; skip that fragile CUDA path.""" + + if getattr(ACTEncoderLayer, "_aao_no_weights_patch", False): + return + + def encoder_forward(self, x, pos_embed=None, key_padding_mask=None): + skip = x + if self.pre_norm: + x = self.norm1(x) + q = k = x if pos_embed is None else x + pos_embed + x = self.self_attn( + q, + k, + value=x, + key_padding_mask=key_padding_mask, + need_weights=False, + )[0] + x = skip + self.dropout1(x) + if self.pre_norm: + skip = x + x = self.norm2(x) + else: + x = self.norm1(x) + skip = x + x = self.linear2(self.dropout(self.activation(self.linear1(x)))) + x = skip + self.dropout2(x) + if not self.pre_norm: + x = self.norm2(x) + return x + + def decoder_forward(self, x, encoder_out, decoder_pos_embed=None, encoder_pos_embed=None): + skip = x + if self.pre_norm: + x = self.norm1(x) + q = k = self.maybe_add_pos_embed(x, decoder_pos_embed) + x = self.self_attn(q, k, value=x, need_weights=False)[0] + x = skip + self.dropout1(x) + if self.pre_norm: + skip = x + x = self.norm2(x) + else: + x = self.norm1(x) + skip = x + x = self.multihead_attn( + query=self.maybe_add_pos_embed(x, decoder_pos_embed), + key=self.maybe_add_pos_embed(encoder_out, encoder_pos_embed), + value=encoder_out, + need_weights=False, + )[0] + x = skip + self.dropout2(x) + if self.pre_norm: + skip = x + x = self.norm3(x) + else: + x = self.norm2(x) + skip = x + x = self.linear2(self.dropout(self.activation(self.linear1(x)))) + x = skip + self.dropout3(x) + if not self.pre_norm: + x = self.norm3(x) + return x + + ACTEncoderLayer.forward = encoder_forward + ACTDecoderLayer.forward = decoder_forward + ACTEncoderLayer._aao_no_weights_patch = True + ACTDecoderLayer._aao_no_weights_patch = True + + +_patch_lerobot_act_attention_no_weights() + + +def _normalize_quat_xyzw(quat: Any) -> np.ndarray: + q = np.asarray(quat, dtype=np.float64).reshape(4) + norm = np.linalg.norm(q) + if norm < 1e-12: + return np.asarray([0.0, 0.0, 0.0, 1.0], dtype=np.float64) + return q / norm + + +def _quat_xyzw_to_matrix(quat: Any) -> np.ndarray: + x, y, z, w = _normalize_quat_xyzw(quat) + xx, yy, zz = x * x, y * y, z * z + xy, xz, yz = x * y, x * z, y * z + wx, wy, wz = w * x, w * y, w * z + return np.asarray( + [ + [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)], + [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)], + [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)], + ], + dtype=np.float64, + ) + + +def _normalize_quat_batch_xyzw(quat: Any) -> np.ndarray: + q = np.asarray(quat, dtype=np.float64).reshape(-1, 4) + norm = np.linalg.norm(q, axis=-1, keepdims=True) + return q / np.clip(norm, 1e-12, None) + + +def _quat_mul_batch_xyzw(a: Any, b: Any) -> np.ndarray: + """Hamilton product for xyzw quaternions, batched as (B, 4).""" + a = np.asarray(a, dtype=np.float64).reshape(-1, 4) + b = np.asarray(b, dtype=np.float64).reshape(-1, 4) + ax, ay, az, aw = a[:, 0], a[:, 1], a[:, 2], a[:, 3] + bx, by, bz, bw = b[:, 0], b[:, 1], b[:, 2], b[:, 3] + return np.stack( + [ + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + aw * bw - ax * bx - ay * by - az * bz, + ], + axis=-1, + ) + + +def _rot6d_to_matrix_batch(rot6d: Any) -> np.ndarray: + x = np.asarray(rot6d, dtype=np.float64).reshape(-1, 6) + a1 = x[:, 0:3] + a2 = x[:, 3:6] + b1 = a1 / np.clip(np.linalg.norm(a1, axis=1, keepdims=True), 1e-8, None) + a2 = a2 - np.sum(b1 * a2, axis=1, keepdims=True) * b1 + b2 = a2 / np.clip(np.linalg.norm(a2, axis=1, keepdims=True), 1e-8, None) + b3 = np.cross(b1, b2, axis=1) + return np.stack((b1, b2, b3), axis=2) + + +def _matrix_to_rot6d_batch(matrix: Any) -> np.ndarray: + R = np.asarray(matrix, dtype=np.float64).reshape(-1, 3, 3) + return np.concatenate((R[:, :, 0], R[:, :, 1]), axis=1) + + +def _matrix_to_quat_xyzw_batch(matrix: Any) -> np.ndarray: + """和训练侧 act_relative_processor.matrix_to_quat_xyzw 完全一致的 numpy 版。""" + m = np.asarray(matrix, dtype=np.float64).reshape(-1, 3, 3) + m00, m11, m22 = m[:, 0, 0], m[:, 1, 1], m[:, 2, 2] + qw = 0.5 * np.sqrt(np.clip(1.0 + m00 + m11 + m22, 0.0, None)) + qx = 0.5 * np.sqrt(np.clip(1.0 + m00 - m11 - m22, 0.0, None)) + qy = 0.5 * np.sqrt(np.clip(1.0 - m00 + m11 - m22, 0.0, None)) + qz = 0.5 * np.sqrt(np.clip(1.0 - m00 - m11 + m22, 0.0, None)) + qx = np.copysign(qx, m[:, 2, 1] - m[:, 1, 2]) + qy = np.copysign(qy, m[:, 0, 2] - m[:, 2, 0]) + qz = np.copysign(qz, m[:, 1, 0] - m[:, 0, 1]) + quat = np.stack((qx, qy, qz, qw), axis=-1) + return _normalize_quat_batch_xyzw(quat) + + +def _rot6d_to_quat_xyzw_batch(rot6d: Any) -> np.ndarray: + """rot6d -> quat(xyzw),列约定与训练一致(b1,b2,b3 为列)。""" + return _matrix_to_quat_xyzw_batch(_rot6d_to_matrix_batch(rot6d)) + + +def _global_rela_rot6d(rot6d: Any, ref_rot6d: Any) -> np.ndarray: + R_ref = _rot6d_to_matrix_batch(ref_rot6d) + R = _rot6d_to_matrix_batch(rot6d) + return _matrix_to_rot6d_batch(np.einsum("bij,bjk->bik", R_ref.transpose(0, 2, 1), R)) + + +def _quat_angle_deg(q1: Any, q2: Any) -> float: + a = _normalize_quat_xyzw(q1) + b = _normalize_quat_xyzw(q2) + dot = abs(float(np.dot(a, b))) + dot = min(1.0, max(-1.0, dot)) + return float(np.degrees(2.0 * np.arccos(dot))) + + +def _upright_tilt_deg(quat: Any) -> float: + rot = _quat_xyzw_to_matrix(quat) + local_z_in_world = rot[:, 2] + cos_angle = float(np.dot(local_z_in_world, np.asarray([0.0, 0.0, 1.0]))) + cos_angle = min(1.0, max(-1.0, cos_angle)) + return float(np.degrees(np.arccos(cos_angle))) + + +def _tensor_summary(value: Any) -> str: + if isinstance(value, torch.Tensor): + data = value.detach() + finite = torch.isfinite(data) + if finite.any(): + finite_data = data[finite] + min_val = float(finite_data.min().cpu()) + max_val = float(finite_data.max().cpu()) + mean_val = float(finite_data.float().mean().cpu()) + return ( + f"shape={tuple(data.shape)} dtype={data.dtype} device={data.device} " + f"min={min_val:.5f} max={max_val:.5f} mean={mean_val:.5f}" + ) + return f"shape={tuple(data.shape)} dtype={data.dtype} device={data.device} no_finite_values" + arr = np.asarray(value) + if arr.size: + return ( + f"shape={arr.shape} dtype={arr.dtype} " + f"min={float(np.nanmin(arr)):.5f} max={float(np.nanmax(arr)):.5f} " + f"mean={float(np.nanmean(arr)):.5f}" + ) + return f"shape={arr.shape} dtype={arr.dtype} empty" + + +def _print_section(title: str) -> None: + print(f"\n== {title} ==") + + +def _image_keys_from_checkpoint(checkpoint: str) -> dict[str, str]: + """Infer simulator camera keys from the checkpoint image feature names.""" + config_path = Path(checkpoint) / "config.json" + with config_path.open(encoding="utf-8") as f: + checkpoint_config = json.load(f) + + image_keys: dict[str, str] = {} + for feat_key in checkpoint_config.get("input_features", {}): + prefix = "observation.images." + if feat_key.startswith(prefix): + camera_name = feat_key.removeprefix(prefix) + image_keys[f"{camera_name}/color/image_raw"] = feat_key + return image_keys or dict(IMAGE_KEYS) + + +# --------------------------------------------------------------------------- +# ACT 适配器 policy +# --------------------------------------------------------------------------- + + +class ACTPolicyAdapter: + """把 lerobot ACTPolicy 包成 aao PolicyEvaluator 需要的 policy 接口。""" + + def __init__( + self, + checkpoint: str, + device: str = "cuda", + debug: bool = False, + action_debug_every: int = 0, + replan_every_step: bool = False, + delta_position_action: bool = False, + delta_orientation_action: bool = False, + episode0_rela_pose: bool = False, + lock_vertical_orientation: bool = False, + place_xy_from_target: bool = False, + place_xy_offset: tuple[float, float] = (0.0, 0.0), + ) -> None: + self.device = device if torch.cuda.is_available() else "cpu" + self.checkpoint = str(Path(checkpoint).expanduser()) + self.debug = debug + self.action_debug_every = max(int(action_debug_every), 0) + self.replan_every_step = replan_every_step + self.delta_position_action = delta_position_action + self.delta_orientation_action = delta_orientation_action + self.episode0_rela_pose = episode0_rela_pose + self.lock_vertical_orientation = lock_vertical_orientation + self.place_xy_from_target = place_xy_from_target + self.place_xy_offset = np.asarray(place_xy_offset, dtype=np.float32).reshape(2) + self._printed_debug = False + self._step = 0 + self._delta_anchor_pos: Optional[np.ndarray] = None + self._delta_anchor_quat: Optional[np.ndarray] = None + self._episode0_anchor_pos: Optional[np.ndarray] = None + self._episode0_anchor_rot6d: Optional[np.ndarray] = None + self._episode0_anchor_quat: Optional[np.ndarray] = None + self.image_keys = _image_keys_from_checkpoint(self.checkpoint) + policy_cfg = PreTrainedConfig.from_pretrained(self.checkpoint) + policy_cfg.device = self.device + self.policy = ACTPolicy.from_pretrained(self.checkpoint, config=policy_cfg) + self.preprocessor, self.postprocessor = make_pre_post_processors( + policy_cfg=policy_cfg, + pretrained_path=self.checkpoint, + preprocessor_overrides={ + "device_processor": {"device": self.device}, + }, + ) + self.policy.eval() + self.policy.to(self.device) + self._validate_flags_against_checkpoint() + + def _checkpoint_uses_chunk_relative(self) -> bool: + """从 policy_preprocessor.json 检测 checkpoint 是否内嵌 chunk-relative processor。 + + 训练时 ACTChunkRelativePoseProcessorStep 会作为 preprocessor step 存进 checkpoint, + 其 registry 名是 'airdc_act_chunk_relative_pose',class 路径含 + 'ACTChunkRelativePoseProcessorStep'。据此判断该模型是否 chunk-delta 训练。 + """ + cfg_path = Path(self.checkpoint) / "policy_preprocessor.json" + if not cfg_path.exists(): + return False + try: + with cfg_path.open(encoding="utf-8") as f: + text = f.read() + except OSError: + return False + return ( + "airdc_act_chunk_relative_pose" in text + or "ACTChunkRelativePoseProcessorStep" in text + ) + + def _validate_flags_against_checkpoint(self) -> None: + """核对 CLI flag 与 checkpoint 真实训练表示,不符直接报错,杜绝'能跑但解释错'。 + + - chunk-delta checkpoint 必须开 --delta-position-action + --delta-orientation-action, + 且不能开 --episode0-rela-pose; + - 非 chunk-delta checkpoint 不能开 --delta-* (那样会把绝对/episode0 动作误当增量)。 + """ + is_chunk = self._checkpoint_uses_chunk_relative() + delta_on = self.delta_position_action or self.delta_orientation_action + if is_chunk: + if self.episode0_rela_pose: + raise ValueError( + "Checkpoint 是 chunk-delta 训练(内嵌 ACTChunkRelativePoseProcessorStep)," + "不能用 --episode0-rela-pose。请改用 " + "--delta-position-action --delta-orientation-action。" + ) + if not (self.delta_position_action and self.delta_orientation_action): + raise ValueError( + "Checkpoint 是 chunk-delta 训练,但未同时开启 " + "--delta-position-action 和 --delta-orientation-action。" + "缺失会把相对增量误当绝对位姿执行(能跑但动作全错)。" + ) + else: + if delta_on: + raise ValueError( + "Checkpoint 不是 chunk-delta 训练(preprocessor 中无 " + "ACTChunkRelativePoseProcessorStep),但传了 --delta-position-action/" + "--delta-orientation-action。对绝对或 episode0 模型这会把绝对位姿误当增量。" + "请去掉 --delta-* (episode0 模型用 --episode0-rela-pose,绝对模型不带相对 flag)。" + ) + + def reset(self) -> None: + # 清空 ACT 内部的动作分块队列，开始新一轮 rollout + self.policy.reset() + self.preprocessor.reset() + self.postprocessor.reset() + self._delta_anchor_pos = None + self._delta_anchor_quat = None + self._episode0_anchor_pos = None + self._episode0_anchor_rot6d = None + self._episode0_anchor_quat = None + self._step = 0 + + def _observed_quat(self, observation: dict) -> np.ndarray: + quat = np.asarray(observation["arm/pose/orientation"]["data"], dtype=np.float64) + return _normalize_quat_batch_xyzw(quat).astype(np.float32) + + def _rot6d_anchor_quat(self, observation: dict) -> np.ndarray: + """chunk-delta 训练锚点:q0 = rot6d_to_quat(state 的 rot6d)。 + 必须和 act_relative_processor 训练侧一致——数据里 arm/pose/orientation + 与 arm/pose/rotation_6d 约定不同(y轴翻转 180°),直接用 orientation 当锚会让 + 姿态还原偏 180°。这里内联同一套 rot6d->quat 数学,避免 import 那个模块触发 + ProcessorStepRegistry 重复注册。""" + rot6d = np.asarray(observation["arm/pose/rotation_6d"]["data"], dtype=np.float32) + q = _rot6d_to_quat_xyzw_batch(rot6d) + return _normalize_quat_batch_xyzw(q).astype(np.float32) + + def _ensure_episode0_anchor(self, observation: dict) -> None: + if self._episode0_anchor_pos is not None: + return + self._episode0_anchor_pos = np.asarray( + observation["arm/pose/position"]["data"], dtype=np.float32 + ).copy() + self._episode0_anchor_rot6d = np.asarray( + observation["arm/pose/rotation_6d"]["data"], dtype=np.float32 + ).copy() + self._episode0_anchor_quat = self._observed_quat(observation).copy() + + def _state_parts(self, observation: dict) -> list[np.ndarray]: + if not self.episode0_rela_pose: + return [ + np.asarray(observation[k]["data"], dtype=np.float32) + for k in STATE_KEYS + ] + + self._ensure_episode0_anchor(observation) + assert self._episode0_anchor_pos is not None + assert self._episode0_anchor_rot6d is not None + # episode0 相对语义固定作用于 (position, rotation_6d, gripper) 这三段; + # 用 STATE_KEYS 取值以便与顶部配置保持单一来源,避免硬编码 key 漂移。 + pos = np.asarray(observation[STATE_KEYS[0]]["data"], dtype=np.float32) + rot6d = np.asarray(observation[STATE_KEYS[1]]["data"], dtype=np.float32) + grip = np.asarray(observation[STATE_KEYS[2]]["data"], dtype=np.float32) + pos_rela = pos - self._episode0_anchor_pos + rot6d_rela = _global_rela_rot6d(rot6d, self._episode0_anchor_rot6d).astype( + np.float32 + ) + return [pos_rela, rot6d_rela, grip] + + def _build_obs(self, observation: dict) -> dict: + """aao 批量观测 dict -> lerobot 输入 dict。 + + observation[key]["data"] 形状：低维 (B, dim)，图像 (B, H, W, 3) uint8。 + """ + # 1) 低维状态拼成 observation.state -> (B, 10) + state_parts = self._state_parts(observation) + state = np.concatenate(state_parts, axis=-1) + obs: dict[str, torch.Tensor] = { + "observation.state": torch.from_numpy(state).to(self.device), + } + # 2) 彩色图 (B,H,W,3) uint8 -> (B,3,H,W) float[0,1] + for sim_key, feat_key in self.image_keys.items(): + img = np.asarray(observation[sim_key]["data"]) # (B,H,W,3) uint8 + t = torch.from_numpy(img).to(self.device).float() / 255.0 + obs[feat_key] = t.permute(0, 3, 1, 2).contiguous() + return obs + + def check_io(self, observation: dict) -> None: + """Print a one-frame train/inference IO consistency report.""" + config_path = Path(self.checkpoint) / "config.json" + with config_path.open(encoding="utf-8") as f: + checkpoint_config = json.load(f) + + _print_section("Checkpoint Features") + print("input_features:") + for key, feature in checkpoint_config["input_features"].items(): + print(f" {key}: type={feature['type']} shape={tuple(feature['shape'])}") + print("output_features:") + for key, feature in checkpoint_config["output_features"].items(): + print(f" {key}: type={feature['type']} shape={tuple(feature['shape'])}") + print(f"normalization_mapping: {checkpoint_config.get('normalization_mapping')}") + print( + f"chunk_size={checkpoint_config.get('chunk_size')} " + f"n_action_steps={checkpoint_config.get('n_action_steps')}" + ) + + _print_section("Expected Script Mapping") + print(f"STATE_KEYS: {STATE_KEYS}") + print(f"IMAGE_KEYS: {self.image_keys}") + print("ACTION layout: position[0:3] quat_xyzw[3:7] gripper_abs[7:8]") + print(f"delta_position_action: {self.delta_position_action}") + print(f"delta_orientation_action: {self.delta_orientation_action}") + print(f"episode0_rela_pose: {self.episode0_rela_pose}") + print(f"lock_vertical_orientation: {self.lock_vertical_orientation}") + if self.episode0_rela_pose: + print( + "episode0 mapping: state=(pos-pos0, rot6d_rela_to_rot6d0, gripper_abs); " + "action=(pos_rela+pos0, quat_rela*quat0, gripper_abs)" + ) + + _print_section("Raw AAO Observation") + for key in STATE_KEYS: + payload = observation[key]["data"] + print(f" {key}: {_tensor_summary(payload)}") + for sim_key in self.image_keys: + payload = observation[sim_key]["data"] + print(f" {sim_key}: {_tensor_summary(payload)}") + + built_obs = self._build_obs(observation) + _print_section("Model Input Before Preprocessor") + for key, value in built_obs.items(): + print(f" {key}: {_tensor_summary(value)}") + + processed_obs = self.preprocessor(dict(built_obs)) + _print_section("Model Input After Preprocessor") + for key, value in processed_obs.items(): + if isinstance(value, torch.Tensor): + print(f" {key}: {_tensor_summary(value)}") + + missing = [ + key + for key in checkpoint_config["input_features"] + if key not in processed_obs + ] + extra = [ + key + for key in processed_obs + if key.startswith("observation.") and key not in checkpoint_config["input_features"] + ] + shape_mismatches = [] + for key, feature in checkpoint_config["input_features"].items(): + value = processed_obs.get(key) + if not isinstance(value, torch.Tensor): + continue + expected = tuple(feature["shape"]) + actual = tuple(value.shape[1:]) + if actual != expected: + shape_mismatches.append((key, actual, expected)) + + _print_section("Feature Check") + if missing: + print(f"missing inputs: {missing}") + else: + print("missing inputs: none") + if extra: + print(f"extra observation inputs: {extra}") + else: + print("extra observation inputs: none") + if shape_mismatches: + for key, actual, expected in shape_mismatches: + print(f"shape mismatch: {key}: actual={actual} expected={expected}") + else: + print("shape mismatches: none") + + self.policy.reset() + raw_action = self.policy.select_action(processed_obs) + _print_section("Raw Policy Action Before Postprocessor") + print(f" action: {_tensor_summary(raw_action)}") + action = self.postprocessor(raw_action) + _print_section("Action After Postprocessor") + print(f" action: {_tensor_summary(action)}") + action_np = action.detach().cpu().numpy().astype(np.float32) + pos = action_np[:, ACT_POS] + quat = action_np[:, ACT_QUAT] + grip = action_np[:, ACT_GRIP] + abs_pos = pos + abs_quat = quat + if self.episode0_rela_pose: + self._ensure_episode0_anchor(observation) + assert self._episode0_anchor_pos is not None + assert self._episode0_anchor_quat is not None + abs_pos = pos + self._episode0_anchor_pos + rel_quat = _normalize_quat_batch_xyzw(quat) + abs_quat = _quat_mul_batch_xyzw(rel_quat, self._episode0_anchor_quat) + elif self.delta_orientation_action: + rel_quat = _normalize_quat_batch_xyzw(quat) + # 和主 rollout 路径一致:chunk-delta 姿态锚点用 rot6d 转,不是 arm/pose/orientation + anchor_quat = self._rot6d_anchor_quat(observation) + abs_quat = _quat_mul_batch_xyzw(anchor_quat, rel_quat) + if self.lock_vertical_orientation: + abs_quat = np.broadcast_to(VERTICAL_QUAT_XYZW, abs_quat.shape).copy() + quat_norm = np.linalg.norm(abs_quat, axis=-1) + print(f" position_or_delta[0]: {pos[0].round(5).tolist()}") + if self.episode0_rela_pose: + assert self._episode0_anchor_pos is not None + assert self._episode0_anchor_quat is not None + print(f" episode0_position[0]: {self._episode0_anchor_pos[0].round(5).tolist()}") + print(f" absolute_position[0]: {abs_pos[0].round(5).tolist()}") + print(f" relative_quat_xyzw[0]: {quat[0].round(5).tolist()}") + print( + f" absolute_quat_xyzw[0]: {abs_quat[0].round(5).tolist()} " + f"norm={quat_norm[0]:.5f}" + ) + elif self.delta_position_action: + anchor = np.asarray( + observation[STATE_KEYS[0]]["data"], dtype=np.float32 + )[:, : pos.shape[-1]] + chunk_abs_pos = anchor + pos + print(f" chunk_anchor_position[0]: {anchor[0].round(5).tolist()}") + print(f" absolute_position[0]: {chunk_abs_pos[0].round(5).tolist()}") + print(f" quat_xyzw[0]: {abs_quat[0].round(5).tolist()} norm={quat_norm[0]:.5f}") + else: + print(f" quat_xyzw[0]: {abs_quat[0].round(5).tolist()} norm={quat_norm[0]:.5f}") + print(f" gripper_abs[0]: {grip[0].round(5).tolist()}") + + @torch.inference_mode() + def act( + self, observation: Any, update: TaskUpdate, evaluator: PolicyEvaluator + ) -> Optional[dict]: + obs = self._build_obs(observation) + obs = self.preprocessor(obs) + current_pos = np.asarray( + observation[STATE_KEYS[0]]["data"], dtype=np.float32 + ) + queue_empty = True + if hasattr(self.policy, "_action_queue"): + queue_empty = len(self.policy._action_queue) == 0 + if self.replan_every_step: + # ACT 默认会把 n_action_steps 个动作放进队列开环执行。对 waypoint + # 型任务，逐步重规划更容易从“到上方”切到“下压/闭合”。 + self.policy.reset() + queue_empty = True + need_chunk_anchor = self.delta_position_action or self.delta_orientation_action + if need_chunk_anchor and (queue_empty or self._delta_anchor_pos is None): + self._delta_anchor_pos = current_pos[:, :3].copy() + # chunk-delta 姿态锚点必须和训练一致:用 rot6d 转出的 q0,不是 arm/pose/orientation + self._delta_anchor_quat = self._rot6d_anchor_quat(observation) + action = self.policy.select_action(obs) # normalized or raw depending on processor chain + action = self.postprocessor(action) # (B, 8), unnormalized policy action + action = action.detach().cpu().numpy().astype(np.float32) + + pos = action[:, ACT_POS] # (B, 3) + quat = action[:, ACT_QUAT] # (B, 4) xyzw + grip = action[:, ACT_GRIP] # (B, 1) + delta_pos = None + anchor = None + delta_quat = None + anchor_quat = None + if self.episode0_rela_pose: + self._ensure_episode0_anchor(observation) + assert self._episode0_anchor_pos is not None + assert self._episode0_anchor_quat is not None + delta_pos = pos + anchor = self._episode0_anchor_pos + pos = anchor + delta_pos + delta_quat = _normalize_quat_batch_xyzw(quat) + anchor_quat = self._episode0_anchor_quat + # Match PoseGlobalRelaAbsTool.to_abs_orientation: abs = rela * ref. + quat = _quat_mul_batch_xyzw(delta_quat, anchor_quat) + if self.delta_position_action: + anchor = ( + current_pos[:, :3] + if self._delta_anchor_pos is None + else self._delta_anchor_pos + ) + delta_pos = pos + pos = anchor + delta_pos + if self.delta_orientation_action: + delta_quat = _normalize_quat_batch_xyzw(quat) + anchor_quat = ( + self._rot6d_anchor_quat(observation) + if self._delta_anchor_quat is None + else self._delta_anchor_quat + ) + quat = _quat_mul_batch_xyzw(anchor_quat, delta_quat) + if self.lock_vertical_orientation: + quat = np.broadcast_to(VERTICAL_QUAT_XYZW, quat.shape).copy() + if self.place_xy_from_target: + stage_names = list(getattr(update, "stage_name", [])) + try: + context = evaluator._require_context() + target = context.backend.get_object_handler("target_pedestal") + target_pose = target.get_pose() if target is not None else None + except Exception: # noqa: BLE001 - optional diagnostic/control path + target_pose = None + if target_pose is not None: + target_xy = np.asarray(target_pose.position, dtype=np.float32)[:, :2] + for env_index in range(min(pos.shape[0], len(stage_names))): + if "place" in stage_names[env_index]: + pos[env_index, :2] = target_xy[env_index] + self.place_xy_offset + # 模型输出的四元数不是单位长度，apply_pose_action 需要单位四元数 + norm = np.linalg.norm(quat, axis=-1, keepdims=True) + quat = quat / np.clip(norm, 1e-8, None) + should_print = self.debug and ( + not self._printed_debug + or (self.action_debug_every > 0 and self._step % self.action_debug_every == 0) + ) + if should_print: + self._printed_debug = True + msg = ( + f"[debug] step={self._step} " + f"pos={pos[0].round(4).tolist()} " + f"quat={quat[0].round(4).tolist()} " + f"grip_abs={grip[0].round(4).tolist()}" + ) + if self.episode0_rela_pose: + msg += ( + f" episode0_dpos={delta_pos[0].round(4).tolist()}" + f" episode0_pos={anchor[0].round(4).tolist()}" + f" episode0_dquat={delta_quat[0].round(4).tolist()}" + f" episode0_quat={anchor_quat[0].round(4).tolist()}" + ) + elif self.delta_position_action: + msg += ( + f" delta={delta_pos[0].round(4).tolist()}" + f" anchor={anchor[0].round(4).tolist()}" + ) + if self.delta_orientation_action: + msg += ( + f" dquat={delta_quat[0].round(4).tolist()}" + f" anchor_q={anchor_quat[0].round(4).tolist()}" + ) + print(msg) + self._step += 1 + return {"position": pos, "orientation": quat, "gripper": grip} + + +# --------------------------------------------------------------------------- +# action_applier / observation_getter（与 policy_eval_example.py 相同） +# --------------------------------------------------------------------------- + + +def action_applier( + context: ExecutionContext, action: Any, env_mask: Optional[np.ndarray] = None +) -> None: + if action is not None: + context.backend.env.apply_pose_action( + "arm", + action["position"], + action["orientation"], + action["gripper"], + kinematic=False, + ) + + +def observation_getter(context: ExecutionContext) -> dict: + return context.backend.env.capture_observation() + + +def _object_pose(context: ExecutionContext, name: str, env_index: int = 0) -> tuple[np.ndarray, np.ndarray] | None: + handler = context.backend.get_object_handler(name) + if handler is None: + return None + pose = handler.get_pose().select(env_index) + return ( + np.asarray(pose.position[0], dtype=np.float64), + np.asarray(pose.orientation[0], dtype=np.float64), + ) + + +def _trace_rollout_state( + evaluator: PolicyEvaluator, + update: TaskUpdate, + *, + rollout: int, + step: int, + observation: Optional[dict] = None, + last_action: Optional[dict], +) -> None: + context = evaluator._require_context() + env_index = 0 + operator = context.backend.get_operator_handler("arm") + eef_pose = operator.get_end_effector_pose().select(env_index) + eef_pos = np.asarray(eef_pose.position[0], dtype=np.float64) + eef_quat = np.asarray(eef_pose.orientation[0], dtype=np.float64) + + source = _object_pose(context, "source_block", env_index) + target = _object_pose(context, "target_pedestal", env_index) + grip_text = "" + if observation is not None and "gripper/joint_state/position" in observation: + grip_obs = np.asarray( + observation["gripper/joint_state/position"]["data"], dtype=np.float64 + ) + if grip_obs.ndim >= 2 and grip_obs.shape[0] > env_index: + grip_text = f" grip_obs={grip_obs[env_index].round(4).tolist()}" + source_text = "source=missing" + if source is not None: + source_pos, source_quat = source + world_delta = source_pos - eef_pos + eef_delta = _quat_xyzw_to_matrix(eef_quat).T @ world_delta + try: + target_handler = context.backend.get_object_handler("source_block") + grasp = operator._check_grasp_conditions(env_index, target_handler) + except Exception as exc: # noqa: BLE001 - debug path should not stop rollout + grasp = {"error": type(exc).__name__} + source_text = ( + f"source_pos={source_pos.round(4).tolist()} " + f"source_tilt={_upright_tilt_deg(source_quat):.2f}deg " + f"src-eef_world={world_delta.round(4).tolist()} " + f"src_in_eef={eef_delta.round(4).tolist()} " + f"grasp={grasp}" + ) + + target_text = "target=missing" + if target is not None: + target_pos, _ = target + target_text = f"target_pos={target_pos.round(4).tolist()}" + + action_text = "" + if last_action is not None: + cmd_pos = np.asarray(last_action["position"][env_index], dtype=np.float64) + cmd_quat = np.asarray(last_action["orientation"][env_index], dtype=np.float64) + cmd_grip = np.asarray(last_action["gripper"][env_index], dtype=np.float64) + action_text = ( + f" cmd_pos={cmd_pos.round(4).tolist()} " + f"cmd_quat_err={_quat_angle_deg(cmd_quat, VERTICAL_QUAT_XYZW):.3f}deg" + f" cmd_grip={cmd_grip.round(4).tolist()}" + ) + + print( + f"[trace] rollout={rollout} step={step} " + f"stage={int(update.stage_index[env_index])}:{update.stage_name[env_index]} " + f"status={update.status[env_index]} " + f"eef_pos={eef_pos.round(4).tolist()} " + f"eef_quat_err={_quat_angle_deg(eef_quat, VERTICAL_QUAT_XYZW):.3f}deg " + f"{source_text} {target_text}{grip_text}{action_text}" + ) + + +def _maybe_trace_rollout_state( + args: argparse.Namespace, + evaluator: PolicyEvaluator, + update: TaskUpdate, + *, + rollout: int, + step: int, + observation: Optional[dict], + last_action: Optional[dict], + prev_stage: Optional[tuple[int, str, str]], +) -> Optional[tuple[int, str, str]]: + if not args.trace_grasp: + return prev_stage + stage_key = ( + int(update.stage_index[0]), + update.stage_name[0], + str(update.status[0]), + ) + trace_every = max(int(args.trace_every), 1) + if prev_stage != stage_key or step % trace_every == 0: + _trace_rollout_state( + evaluator, + update, + rollout=rollout, + step=step, + observation=observation, + last_action=last_action, + ) + return stage_key + + +def _run_policy_tail( + *, + policy: ACTPolicyAdapter, + evaluator: PolicyEvaluator, + update: TaskUpdate, + args: argparse.Namespace, + rollout: int, + step: int, + prev_stage: Optional[tuple[int, str, str]], + last_action: Optional[dict], +) -> tuple[TaskUpdate, int, Optional[tuple[int, str, str]], Optional[dict], int]: + seconds = max(float(args.post_success_policy_seconds), 0.0) + if seconds <= 0.0: + return update, step, prev_stage, last_action, 0 + + freq = max(float(args.sim_loop_frequency), 1.0) + interval = 1.0 / freq + deadline = time.monotonic() + seconds + tail_steps = 0 + print(f"[rollout {rollout}] success met; continuing policy for {seconds:.2f}s") + while time.monotonic() < deadline: + step += 1 + obs = evaluator.get_observation() + action = policy.act(obs, update=update, evaluator=evaluator) + last_action = action + prev_stage = _maybe_trace_rollout_state( + args, + evaluator, + update, + rollout=rollout, + step=step, + observation=obs, + last_action=last_action, + prev_stage=prev_stage, + ) + update = evaluator.update(action) + tail_steps += 1 + remaining = deadline - time.monotonic() + if remaining > 0.0: + time.sleep(min(interval, remaining)) + return update, step, prev_stage, last_action, tail_steps + + +def _hold_after_done(evaluator: PolicyEvaluator, seconds: float, frequency: float) -> None: + seconds = max(float(seconds), 0.0) + if seconds <= 0.0: + return + print(f"[viewer] holding final state for {seconds:.2f}s") + if evaluator.sim_loop_running: + time.sleep(seconds) + return + + env = evaluator.get_env() + if not hasattr(env, "update"): + time.sleep(seconds) + return + + interval = 1.0 / max(float(frequency), 1.0) + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + with evaluator.sim_lock: + env.update() + remaining = deadline - time.monotonic() + if remaining > 0.0: + time.sleep(min(interval, remaining)) + + +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- + + +def main() -> None: + parser = argparse.ArgumentParser(description="ACT policy 在 aao 仿真里闭环评估") + parser.add_argument("--checkpoint", required=True, help="pretrained_model 目录路径") + parser.add_argument("--config-name", required=True, help="aao task 配置名(与采集时一致)") + parser.add_argument("--batch-size", type=int, default=1, help="并行环境数") + parser.add_argument("--num-rollouts", type=int, default=10, help="跑多少轮评估") + parser.add_argument("--max-steps", type=int, default=400, help="每轮最大步数") + parser.add_argument( + "--override", + action="append", + default=[], + help="额外 Hydra override，可重复传，例如 --override +env=gl", + ) + parser.add_argument("--debug-action", action="store_true", help="打印模型动作") + parser.add_argument("--action-debug-every", type=int, default=0, help="每 N 步打印一次动作") + parser.add_argument( + "--check-io", + action="store_true", + help="检查训练/推理输入输出 key、shape、dtype、范围和归一化链", + ) + parser.add_argument( + "--check-io-only", + action="store_true", + help="只做 IO 检查，不执行 rollout", + ) + parser.add_argument( + "--replan-every-step", + action="store_true", + help="每步清空 ACT 动作队列并基于当前观测重新预测", + ) + parser.add_argument( + "--delta-position-action", + action="store_true", + help="把模型输出的 position[0:3] 当作相对查询时刻末端位置的 delta，再还原成绝对位置执行", + ) + parser.add_argument( + "--delta-orientation-action", + action="store_true", + help="chunk-relative 对比用：把 quat[3:7] 当作相对 chunk 起点姿态的 delta，再还原成绝对姿态执行", + ) + parser.add_argument( + "--episode0-rela-pose", + action="store_true", + help="按 mcap_data_loader poses.py 的 _rela 语义处理：pose 相对每个 rollout 第 0 帧，gripper 保持绝对值", + ) + parser.add_argument( + "--post-success-policy-seconds", + type=float, + default=0.0, + help="所有环境判定成功后继续执行 policy 多少秒，方便在 viewer 里观察门是否真的继续打开", + ) + parser.add_argument( + "--post-done-hold-seconds", + type=float, + default=0.0, + help="rollout done 后保持 viewer/物理仿真多少秒；不再更新判定，只保留最后控制", + ) + parser.add_argument( + "--lock-vertical-orientation", + action="store_true", + help="忽略模型输出的 quat，强制使用训练数据里的竖直末端姿态 [0,sqrt(0.5),0,sqrt(0.5)]", + ) + parser.add_argument( + "--place-xy-from-target", + action="store_true", + help="诊断/仿真用：放置阶段把执行 XY 对准 target_pedestal，Z 和夹爪仍用模型输出", + ) + parser.add_argument( + "--place-xy-offset", + type=float, + nargs=2, + default=(0.0, 0.0), + metavar=("DX", "DY"), + help="配合 --place-xy-from-target 使用的目标台 XY 偏移，单位米", + ) + parser.add_argument( + "--trace-grasp", + action="store_true", + help="打印抓取/放置诊断：末端、方块、目标台位姿和抓取侧向误差", + ) + parser.add_argument( + "--trace-every", + type=int, + default=20, + help="开启 --trace-grasp 时每 N 步打印一次诊断，阶段切换时总会打印", + ) + parser.add_argument( + "--sim-loop-frequency", type=float, default=10.0, help="from_config 的仿真频率" + ) + args = parser.parse_args() + if args.episode0_rela_pose and ( + args.delta_position_action or args.delta_orientation_action + ): + parser.error( + "--episode0-rela-pose already handles relative pose restoration; " + "do not combine it with chunk-relative --delta-position-action/" + "--delta-orientation-action." + ) + + overrides = [f"env.batch_size={args.batch_size}", *args.override] + task_file = load_task_file_hydra(args.config_name, overrides=overrides) + policy = ACTPolicyAdapter( + args.checkpoint, + debug=args.debug_action, + action_debug_every=args.action_debug_every, + replan_every_step=args.replan_every_step, + delta_position_action=args.delta_position_action, + delta_orientation_action=args.delta_orientation_action, + episode0_rela_pose=args.episode0_rela_pose, + lock_vertical_orientation=args.lock_vertical_orientation, + place_xy_from_target=args.place_xy_from_target, + place_xy_offset=tuple(args.place_xy_offset), + ) + evaluator = PolicyEvaluator( + action_applier=action_applier, + observation_getter=observation_getter, + ).from_config(task_file, args.sim_loop_frequency) + + successes = [] + try: + if args.check_io or args.check_io_only: + policy.reset() + evaluator.reset() + obs = evaluator.get_observation() + policy.check_io(obs) + if args.check_io_only: + return + + for rollout in range(args.num_rollouts): + policy.reset() + update = evaluator.reset() + step = -1 + prev_stage = None + last_action = None + for step in range(args.max_steps): + obs = evaluator.get_observation() + action = policy.act(obs, update=update, evaluator=evaluator) + last_action = action + prev_stage = _maybe_trace_rollout_state( + args, + evaluator, + update, + rollout=rollout, + step=step, + observation=obs, + last_action=last_action, + prev_stage=prev_stage, + ) + update = evaluator.update(action) + if update.done.all(): + if args.trace_grasp: + _trace_rollout_state( + evaluator, + update, + rollout=rollout, + step=step, + observation=obs, + last_action=last_action, + ) + if bool(np.all(update.success)): + update, step, prev_stage, last_action, tail_steps = _run_policy_tail( + policy=policy, + evaluator=evaluator, + update=update, + args=args, + rollout=rollout, + step=step, + prev_stage=prev_stage, + last_action=last_action, + ) + if tail_steps > 0: + print( + f"[rollout {rollout}] post-success policy steps={tail_steps}" + ) + _hold_after_done( + evaluator, + args.post_done_hold_seconds, + args.sim_loop_frequency, + ) + break + summary = evaluator.summarize( + update, max_updates=args.max_steps, updates_used=step + 1 + ) + ok = list(summary.final_success) + successes.extend(ok) + print( + f"[rollout {rollout}] steps={step + 1} " + f"completed_stages={summary.completed_stage_count} success={ok}" + ) + if successes: + rate = sum(bool(s) for s in successes) / len(successes) + print(f"\n成功率: {sum(bool(s) for s in successes)}/{len(successes)} = {rate:.1%}") + finally: + evaluator.close() + + +if __name__ == "__main__": + main() From 675ef534055b8c8afd5329ea9c4fe95bde7f9929 Mon Sep 17 00:00:00 2001 From: Richie6667 <1052996586@qq.com> Date: Wed, 22 Jul 2026 15:26:54 +0800 Subject: [PATCH 2/4] feat: update open_door.yaml for improved arm positioning and door operation --- aao_configs/open_door.yaml | 114 ++++++++++++++++++++++++------------- 1 file changed, 74 insertions(+), 40 deletions(-) diff --git a/aao_configs/open_door.yaml b/aao_configs/open_door.yaml index 19d442b..171f55c 100644 --- a/aao_configs/open_door.yaml +++ b/aao_configs/open_door.yaml @@ -6,14 +6,31 @@ scene_name: open_door task_name: open_door env: - # Task-specific arm home — gripper facing the door handle. Shifted for the - # consolidated demo.xml: door frame is at world (1.54, 0.1, -1.0) vs the - # legacy robotiq scene's (1.49, 0.1, 0.0), so the arm home moves +0.05 x - # and -1.0 z to keep the same relative pose to the handle. + # Task-specific arm home — gripper horizontal, pointing +X (fingers toward the + # door, body/wrist toward the robot). The robotiq palm/wrist sits ~0.14m BEHIND + # the fingertips, so pointing +X keeps the bulky body clear of both the lever + # and the door panel (only ~8cm apart) while the fingers reach in to clamp the + # lever bar vertically (jaw along world Z, straddling the bar top/bottom). + # NOTE: for the mocap gripper this value is the *mocap target*; the TCP/pads + # land ~0.34m ahead of it, so this freejoint pose was calibrated so the open + # finger pads sit ~5.5cm in front of the bar (a clean pre-grasp) with the palm + # back at x~1.27 (clear of the 1.46 bar and 1.52 panel). initial_joint_positions: - robotiq_freejoint: [1.285, 0.3155, -0.195, 0.5, 0.5, -0.5, -0.5] + robotiq_freejoint: [1.057, 0.436, -0.367, -0.5, -0.5, 0.5, 0.5] mask_objects: ["handle_lever_body", "handle_body_phys"] - operations: ["pick", "push"] + operations: ["push"] + # Door latch: keeps door_hinge spring-locked near zero until the handle is + # pressed past unlock_threshold. Without this the door swings freely the + # instant the gripper touches it, so the handle never gets pressed and the + # approach just bulldozes the (unlatched) door. + pre_step_callbacks: + - _target_: auto_atom.callbacks.door_latch.DoorLatchCallback + door_joint: door_hinge + handle_joint: handle_hinge + kp: 80.0 + kd: 8.0 + unlock_threshold: 0.12 + lock_zone: 0.05 viewer: lookat: [1.45, 0.12, 0.8] distance: 1.2 @@ -23,56 +40,73 @@ env: hold_seconds: 1.0 task: + randomization: + arm: + eef: + x: [-0.03, 0.03] + y: [-0.03, 0.03] + z: [-0.03, 0.03] + roll: [-0.0873, 0.0873] + pitch: [-0.0873, 0.0873] + yaw: [-0.0873, 0.0873] stages: - - name: rotate_handle - object: handle_lever_body - operation: pick + # One continuous push: press the lever down to unlock, then swing the door + # open. The success object is handle_body_phys (rides on the door panel), so + # the door's translation satisfies the "pushed" displacement check — the + # lever only rotates in place, which a displacement check would reject. + - name: open_door + object: handle_body_phys + operation: push operator: arm param: pre_move: - - position: [0.095, 0.106, 0.0] - reference: object - max_linear_step: 0.02 + # 0. Re-home in front of the lever bar with the open finger pads + # straddling it in Z (~5.5cm in front). The mocap reset settles to a + # slightly different pose per batch env, so drive to an explicit + # object-referenced point (world axes) to converge every env. + - position: [-0.13, 0.079, 0.0] + reference: object_world + max_linear_step: 0.1 + max_angular_step: 0.3 + # 1. Advance +X so the bar enters the jaw (between the top/bottom pads). + - position: [0.06, 0.0, 0.0] + reference: eef_world + max_linear_step: 0.1 max_angular_step: 0.25 - - position: [0.095, 0.055, 0.0] - reference: object - max_linear_step: 0.01 - max_angular_step: 0.2 eef: close: true post_move: + # 2. Press the lever down to unlock. handle_hinge's local Y axis maps + # to world -X (door frame is rotated +90deg about Z), so the arc + # axis is world [-1,0,0]; a positive target angle presses the lever + # down past the latch's unlock_threshold. max_step 0.15 keeps each + # arc sub-step (>~0.014m) above the 0.01m tolerance so it moves. - arc: pivot: handle_hinge - axis: [0, 1, 0] + axis: [-1, 0, 0] angle: 0.45 absolute: true - max_step: 0.03 - reference: world - - name: push_open - object: handle_body_phys - operation: push - operator: arm - param: - pre_move: - - position: [0.0, 0.0, 0.0] - reference: eef_world - max_linear_step: 0.01 - max_angular_step: 0.15 - post_move: - - arc: - pivot: handle_hinge - axis: [0, 1, 0] - angle: 0.55 - absolute: true - max_step: 0.03 + max_step: 0.15 reference: world + # 3. Swing the door open by sweeping the gripper +0.9rad around the + # door_hinge (world +Z). RELATIVE (not absolute) on purpose: a + # snapshot-based arc rotates the gripper from its pose at action + # start by the cumulative angle, so the orientation target stays a + # clean horizontal Z-sweep. An absolute arc instead re-references + # the live (contact-deflected) pose every tick, so when the door + # momentarily stalls on its high frictionloss (5.0) the grip twists + # the gripper and it accumulates into the wrist flipping to point + # straight up. The per-waypoint tolerance lets the sweep finish when + # the door stalls a hair short of the full angle. - arc: pivot: door_hinge - axis: [0, 0, -1] - angle: -1.2 - absolute: true - max_step: 0.04 + axis: [0, 0, 1] + angle: 0.9 + max_step: 0.2 reference: world + tolerance: + position: 0.06 + orientation: 0.12 operators: - name: arm From 839f879de3bcfc1176c8f2d9acdd9f93358ae133 Mon Sep 17 00:00:00 2001 From: Richie6667 <1052996586@qq.com> Date: Wed, 22 Jul 2026 15:27:56 +0800 Subject: [PATCH 3/4] feat: Added documentation for the complete ACT strategy training and evaluation workflow. --- examples/act_policy_eval.md | 381 ++++++++++++++++++++++++++++++++++++ 1 file changed, 381 insertions(+) create mode 100644 examples/act_policy_eval.md diff --git a/examples/act_policy_eval.md b/examples/act_policy_eval.md new file mode 100644 index 0000000..9fececc --- /dev/null +++ b/examples/act_policy_eval.md @@ -0,0 +1,381 @@ +# ACT 策略训练与评估完整流程 + +本文档介绍如何在 aao 仿真环境中采集数据、训练 ACT 模型、并进行闭环评估的完整流程。 + +## 一、数据采集 + +### 1.1 环境准备 + +确保已安装 [AIRBOT-Data-Collection](https://github.com/DISCOVER-Robotics/AIRBOT-Data-Collection)： + +```bash +# 使用 Pixi（推荐） +pixi install +pixi shell -e collect + +# 或使用传统方式 +pip install -e .[all,airbot] +``` + +### 1.2 启动数据采集 + +在 `AIRBOT-Data-Collection` 目录下运行： + +```bash +airdc --path airbot_ie/configs/aao_config.yaml dataset.directory=my_task +``` + +- `--path`：指定配置文件路径 +- `dataset.directory`：数据保存目录名（保存在 `data/` 下） + +采集过程中使用键盘控制流程，按 `i` 键查看按键说明。 + +### 1.3 数据说明 + +采集完成后，数据保存为 `.mcap` 格式文件，位于 `data/my_task/` 目录。每个 episode 对应一个 `.mcap` 文件，包含： + +- **状态数据**：机械臂位姿、夹爪状态等 +- **动作数据**：示教端的位姿指令 +- **图像数据**：相机采集的 RGB 图像 + +## 二、模型训练 + +### 2.1 安装训练环境 + +确保已安装 [MCAP-DataLoader](https://github.com/OpenGHz/MCAP-DataLoader) 和 lerobot： + +```bash +pip install mcap-data-loader lerobot +``` + +### 2.2 准备训练配置 + +创建训练配置文件 `configs/config.yaml`： + +```yaml +batch_size: 8 +num_workers: 4 +policy: + type: act + chunk_size: 100 + n_action_steps: 100 + +dataset: + root: data # MCAP 数据根目录 + repo_id: my_task # 与采集时 dataset.directory 一致 + +mcap: + states: + - /follow/arm/pose/position + - /follow/arm/pose/rotation_6d + - /follow/gripper/joint_state/position + actions: + - /lead/arm/pose/position + - /lead/arm/pose/orientation + - /lead/gripper/joint_state/position + images: + - /wrist_cam/color/image_raw + - /env0_cam/color/image_raw +``` + +**配置说明：** + +- `states`：观测状态 topic 列表，会拼接成 `observation.state` +- `actions`：动作 topic 列表，会拼接成 `action` +- `images`：图像 topic 列表，会添加到 `observation.images` + +**关键要求：** + +- `states` / `actions` / `images` 的顺序必须固定，训练和评估时必须完全一致 +- topic 名称必须与 MCAP 数据中的 topic 名称匹配 + +### 2.3 开始训练 + +```bash +mcap-lerobot-train -c configs/config.yaml +``` + +训练过程中会自动： +- 加载 MCAP 数据并转换为 lerobot 格式 +- 保存 checkpoint 到 `outputs/train/_act/checkpoints/` +- 记录训练日志 + +训练完成后，模型保存在 `outputs/train//