isaac_linkerbot/isaac_g20_plan_bridge.py

#!/usr/bin/env python3
# SPDX-License-Identifier: Apache-2.0
"""Isaac Sim G20 实机命令桥。

默认：
  订阅  /g20/cb_left_hand_control_cmd   JointState（与真机相同的 0~255 cmd）
  发布  /sim/isaac/g20/left/joint_state JointState（仿真 DOF 弧度）

按关节名匹配；0~255 经 joint_mapping 转为 URDF 弧度。
默认位置硬写以跟真机；``--pd-drive`` 可选。

PLAN 名义弧度：``--plan-nominal`` 订阅 /retarget/g20/left/joint_target_nominal。
"""

from __future__ import annotations

import argparse
import sys
import time
from pathlib import Path

LINKERHAND_ROOT = Path(__file__).resolve().parent
URDF_ROOT = LINKERHAND_ROOT / "urdf"
DEFAULT_G20_USD = URDF_ROOT / "g20/left/linkerhand_g20_left/linkerhand_g20_left.usda"

TOPIC_NOMINAL = "/retarget/g20/left/joint_target_nominal"
TOPIC_SIM_STATE = "/sim/isaac/g20/left/joint_state"
TOPIC_G20_CMD = "/g20/cb_left_hand_control_cmd"
TOPIC_LEGACY_CMD = "/cb_left_hand_control_cmd"  # 无命名空间旧话题
TOPIC_LEGACY_STATE = "/cb_left_hand_state"

HAND_MOUNT_Y = 0.45
HAND_MOUNT_Z = 0.70
TRACK_TOL_RAD = 0.02
WARN_INTERVAL_S = 2.0
# 跟真机：默认不限速（0=每步直达 goal）；需要柔顺时用 --slew-rad
DEFAULT_SLEW_RAD = 0.0
DEFAULT_PHYS_PER_RENDER = 16
PD_STIFFNESS = 15.0
PD_DAMPING = 2.5

# 与 MuJoCo G20 共用的对比视角：正对掌心（法向约 -X）、手指朝上(+Z)
# lookat 相对手本地原点；相机从 +X 看向 -X（与 MuJoCo azimuth=180 一致）
COMPARE_LOOKAT_LOCAL = (0.04, -0.04, 0.12)
COMPARE_DISTANCE = 0.50
COMPARE_ELEVATION_DEG = -12.0
PD_MAX_EFFORT = 8.0
LIMIT_MARGIN_RAD = 0.05
THUMB_PD_STIFFNESS = 10.0
THUMB_PD_DAMPING = 3.0
THUMB_PD_MAX_EFFORT = 6.0
THUMB_ACTUATED = frozenset(
    {"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp"}
)


def _normalize_argv(argv: list[str]) -> list[str]:
    """把常见单横杠长选项改成双横杠，避免 ``-hand-gravity`` 被拆成 ``-h`` 等短选项。"""
    aliases = {
        "-hand-gravity": "--hand-gravity",
        "-pd-drive": "--pd-drive",
        "-legacy-sdk": "--legacy-sdk",
        "-plan-nominal": "--plan-nominal",
        "-track-test": "--track-test",
        "-headless": "--headless",
        "-no-self-collision": "--no-self-collision",
    }
    out: list[str] = []
    for a in argv:
        out.append(aliases.get(a, a))
    return out


def parse_args() -> argparse.Namespace:
    p = argparse.ArgumentParser(description="Isaac G20 bridge (default: /g20/cb_left_hand_control_cmd)")
    p.add_argument("--headless", action="store_true")
    p.add_argument("--g20-usd", type=Path, default=DEFAULT_G20_USD)
    p.add_argument("--max-steps", type=int, default=0, help=">0 时跑满步数退出")
    p.add_argument(
        "--plan-nominal",
        action="store_true",
        help="订阅 PLAN 名义弧度 /retarget/g20/left/joint_target_nominal",
    )
    p.add_argument(
        "--legacy-sdk",
        action="store_true",
        help="订阅旧话题 /cb_left_hand_control_cmd（无 /g20 前缀）",
    )
    p.add_argument(
        "--pd-drive",
        action="store_true",
        help="PD 动力学（默认位置硬写；大跳指令仍靠 --slew-rad 限速）",
    )
    p.add_argument(
        "--hand-gravity",
        action="store_true",
        help="手部重力（仅 --pd-drive 下生效）",
    )
    p.add_argument(
        "--no-self-collision",
        action="store_true",
        help="关闭自碰（会穿模；默认跟随 USD 开启自碰）",
    )
    p.add_argument(
        "--slew-rad",
        type=float,
        default=DEFAULT_SLEW_RAD,
        help="每物理步最大目标变化(rad)；0=瞬跳跟真机。默认 0",
    )
    p.add_argument(
        "--phys-per-render",
        type=int,
        default=DEFAULT_PHYS_PER_RENDER,
        help="每次渲染前进的物理步数（默认 16）",
    )
    p.add_argument(
        "--track-test",
        action="store_true",
        help="内置 open→半握，验收主动关节跟踪 ≤0.02rad",
    )
    p.add_argument("--cmd-topic", default=TOPIC_G20_CMD, help="0~255 命令话题")
    p.add_argument("--nominal-topic", default=TOPIC_NOMINAL)
    p.add_argument("--sim-state-topic", default=TOPIC_SIM_STATE)
    args, unknown = p.parse_known_args(_normalize_argv(sys.argv[1:]))
    if unknown:
        print(f"[args] ignoring unknown argv: {unknown}")
    sys.argv = [sys.argv[0]]
    return args


args = parse_args()

from isaacsim import SimulationApp

simulation_app = SimulationApp({"headless": args.headless})

import numpy as np

import isaacsim.core.experimental.utils.app as app_utils
import isaacsim.core.experimental.utils.prim as prim_utils
import isaacsim.core.experimental.utils.stage as stage_utils
from isaacsim.core.experimental.objects import DistantLight, GroundPlane
from isaacsim.core.experimental.prims import Articulation, XformPrim
from isaacsim.core.rendering_manager import RenderingManager
from isaacsim.core.simulation_manager import SimulationManager
from isaacsim.core.utils.viewports import set_camera_view

sys.path.insert(0, str(LINKERHAND_ROOT))
from joint_mapping import (
    L20_SDK_TO_URDF,
    complete_g20_mimic_positions,
    g20_actuated_joint_names,
    g20_required_joint_names,
    open_hand_command,
    sdk_range_to_full_urdf_positions,
    urdf_positions_to_sdk_range,
)

app_utils.enable_extension("isaacsim.ros2.bridge")
simulation_app.update()

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState


def _fix_base_to_world(hand_path: str) -> None:
    from pxr import Gf, Usd, UsdGeom, UsdPhysics

    stage = stage_utils.get_current_stage()
    hand_prim = stage.GetPrimAtPath(hand_path)
    if not hand_prim.IsValid():
        return

    base = None
    first = None
    for prim in Usd.PrimRange(hand_prim):
        if not prim.HasAPI(UsdPhysics.RigidBodyAPI):
            continue
        if first is None:
            first = prim
        if prim.GetName() in ("hand_base_link", "base_link", "palm"):
            base = prim
            break
    base = base or first
    if base is None:
        print(f"[anchor] no RigidBody under {hand_path}")
        return

    xf = UsdGeom.Xformable(base).ComputeLocalToWorldTransform(Usd.TimeCode.Default())
    pos = xf.ExtractTranslation()
    quat = xf.ExtractRotationQuat()
    pos_f = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2]))
    rot_f = Gf.Quatf(float(quat.GetReal()), *[float(v) for v in quat.GetImaginary()])

    for prim in Usd.PrimRange(hand_prim):
        if prim.GetName() != "root_joint" or not prim.IsA(UsdPhysics.Joint):
            continue
        joint = UsdPhysics.Joint(prim)
        b1 = [str(t) for t in (joint.GetBody1Rel().GetTargets() or [])]
        b0 = [str(t) for t in (joint.GetBody0Rel().GetTargets() or [])]
        if str(base.GetPath()) not in b1 or len(b0) != 0:
            prim.SetActive(False)
            print(f"[anchor] disabled invalid {prim.GetPath()}")
            continue
        if not prim.IsActive():
            prim.SetActive(True)
        joint.CreateLocalPos0Attr().Set(pos_f)
        joint.CreateLocalRot0Attr().Set(rot_f)
        joint.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))
        joint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0))
        print(
            f"[anchor] USD root_joint -> {base.GetPath()} @ "
            f"({float(pos[0]):.3f},{float(pos[1]):.3f},{float(pos[2]):.3f})"
        )
        return

    joint_path = f"{hand_path}/WorldFixedJoint"
    if stage.GetPrimAtPath(joint_path).IsValid():
        stage.RemovePrim(joint_path)
    joint = UsdPhysics.FixedJoint.Define(stage, joint_path)
    joint.CreateBody0Rel().SetTargets([])
    joint.CreateBody1Rel().SetTargets([base.GetPath()])
    joint.CreateLocalPos0Attr().Set(pos_f)
    joint.CreateLocalRot0Attr().Set(rot_f)
    joint.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))
    joint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0))
    joint.CreateExcludeFromArticulationAttr().Set(False)
    print(f"[anchor] WorldFixedJoint {joint_path}")


def _patch_joint_max_forces(hand_path: str, max_force: float = 8.0) -> None:
    from pxr import Usd, UsdPhysics

    stage = stage_utils.get_current_stage()
    hand_prim = stage.GetPrimAtPath(hand_path)
    if not hand_prim.IsValid():
        return
    for prim in Usd.PrimRange(hand_prim):
        if not prim.IsA(UsdPhysics.RevoluteJoint):
            continue
        drive = UsdPhysics.DriveAPI.Get(prim, "angular")
        if not drive:
            continue
        attr = drive.GetMaxForceAttr()
        if attr and float(attr.Get() or 0.0) < max_force:
            attr.Set(float(max_force))


def _load_g20(usd_path: Path) -> str:
    if not usd_path.is_file():
        raise FileNotFoundError(f"USD 不存在: {usd_path}")
    mount = "/World/G20Hand"
    stage_utils.define_prim(mount, "Xform")
    XformPrim(mount, positions=[[0.0, HAND_MOUNT_Y, HAND_MOUNT_Z]], reset_xform_op_properties=True)
    hand_path = f"{mount}/hand"
    stage_utils.add_reference_to_stage(usd_path=str(usd_path), path=hand_path)
    prim_utils.set_prim_variants(hand_path, variants=[("Physics", "physx")])
    _patch_joint_max_forces(hand_path)
    return hand_path


def validate_g20_dof_names(dof_names: list[str]) -> None:
    have = set(dof_names)
    required = g20_required_joint_names()
    missing = sorted(required - have)
    if missing:
        raise RuntimeError(
            "G20 Articulation 关节名与 PLAN/URDF 规范不匹配，缺失: "
            + ", ".join(missing)
            + f" | have={sorted(have)}"
        )


class G20PlanBridge:
    """订阅 cmd_u8 / PLAN 名义弧度，驱动 Articulation 并发布仿真状态。"""

    def __init__(
        self,
        node: Node,
        art: Articulation,
        *,
        use_cmd_u8: bool,
        cmd_topic: str,
        use_physics_drive: bool,
        nominal_topic: str,
        sim_state_topic: str,
        slew_rad: float = DEFAULT_SLEW_RAD,
    ) -> None:
        self.node = node
        self.art = art
        self.use_cmd_u8 = use_cmd_u8
        self.use_physics_drive = use_physics_drive
        self.slew_rad = max(0.0, float(slew_rad))
        self.dof_names = list(art.dof_names)
        validate_g20_dof_names(self.dof_names)
        self.dof_index = {n: i for i, n in enumerate(self.dof_names)}
        self.actuated = g20_actuated_joint_names()
        self.actuated_indices = [self.dof_index[n] for n in sorted(self.actuated) if n in self.dof_index]

        lower, upper = art.get_dof_limits()
        self._lo = lower.numpy().flatten()
        self._hi = upper.numpy().flatten()

        open_full = sdk_range_to_full_urdf_positions(open_hand_command("G20"), "G20", "left")
        open_full = complete_g20_mimic_positions(open_full)
        self.goal: dict[str, float] = dict(open_full)
        self.applied: dict[str, float] = dict(open_full)
        self.targets = self.applied
        self._last_warn_missing = 0.0
        self._last_warn_unknown = 0.0
        self.track_nominal: dict[str, float] | None = None

        if use_cmd_u8:
            self.sub = node.create_subscription(JointState, cmd_topic, self._on_cmd_u8, 10)
            self.legacy_pub = None
            node.get_logger().info(f"G20 cmd_u8: sub {cmd_topic} (0~255 → URDF rad)")
        else:
            self.sub = node.create_subscription(JointState, nominal_topic, self._on_nominal, 10)
            self.legacy_pub = None
            node.get_logger().info(f"G20 PLAN: sub {nominal_topic} (by name, rad)")

        self.pub = node.create_publisher(JointState, sim_state_topic, 10)
        node.get_logger().info(
            f"G20: pub {sim_state_topic}, actuated={len(self.actuated_indices)}, "
            f"drive={'PD' if use_physics_drive else 'position'}, "
            f"slew_rad={self.slew_rad}"
        )

    def set_nominal_targets(self, positions: dict[str, float]) -> None:
        self.goal = complete_g20_mimic_positions(positions)
        self.track_nominal = {n: float(self.goal[n]) for n in self.actuated if n in self.goal}

    def _on_nominal(self, msg: JointState) -> None:
        if not msg.name:
            self._warn_missing("empty name[]")
            return
        n = min(len(msg.name), len(msg.position))
        incoming = {str(msg.name[i]): float(msg.position[i]) for i in range(n)}
        unknown = [k for k in incoming if k not in self.dof_index]
        if unknown:
            self._warn_unknown(unknown)
        missing_act = sorted(self.actuated - set(incoming.keys()))
        if missing_act:
            self._warn_missing(f"missing actuated: {missing_act}")
            merged = dict(self.goal)
            for k, v in incoming.items():
                if k in self.dof_index:
                    merged[k] = v
            self.goal = complete_g20_mimic_positions(merged)
        else:
            merged = {k: v for k, v in incoming.items() if k in self.dof_index}
            self.goal = complete_g20_mimic_positions(merged)
        self.track_nominal = {n: float(self.goal[n]) for n in self.actuated if n in self.goal}

    def _on_cmd_u8(self, msg: JointState) -> None:
        """接收与真机相同的 0~255 JointState（优先按 name，否则按下标）。"""
        vals = [255.0] * 20
        name_to_sdk = {name: idx for idx, name in L20_SDK_TO_URDF.items()}
        if msg.name:
            n = min(len(msg.name), len(msg.position))
            for i in range(n):
                key = str(msg.name[i])
                pos = float(msg.position[i])
                if key in name_to_sdk:
                    vals[name_to_sdk[key]] = pos
                elif key.startswith("reserved_"):
                    try:
                        vals[int(key.split("_", 1)[1])] = pos
                    except ValueError:
                        pass
                elif i < 20:
                    vals[i] = pos
        else:
            for i, x in enumerate(msg.position[:20]):
                vals[i] = float(x)
        full = sdk_range_to_full_urdf_positions(vals, "G20", "left")
        self.set_nominal_targets(full)

    def _warn_missing(self, detail: str) -> None:
        now = time.monotonic()
        if now - self._last_warn_missing < WARN_INTERVAL_S:
            return
        self._last_warn_missing = now
        self.node.get_logger().warn(f"nominal cmd incomplete, holding last targets ({detail})")

    def _warn_unknown(self, names: list[str]) -> None:
        now = time.monotonic()
        if now - self._last_warn_unknown < WARN_INTERVAL_S:
            return
        self._last_warn_unknown = now
        self.node.get_logger().warn(f"unknown joint names ignored: {names[:8]}")

    def _slew_applied_toward_goal(self) -> None:
        """每步把 applied 向 goal 限速逼近，产生中间运动过程。"""
        max_step = self.slew_rad
        margin = LIMIT_MARGIN_RAD
        for name, g in self.goal.items():
            idx = self.dof_index.get(name)
            if idx is None:
                continue
            lo = float(self._lo[idx]) + margin
            hi = float(self._hi[idx]) - margin
            if lo > hi:
                lo = float(self._lo[idx])
                hi = float(self._hi[idx])
            g = float(np.clip(g, lo, hi))
            a = float(self.applied.get(name, g))
            if max_step <= 0.0:
                a = g
            else:
                delta = g - a
                if abs(delta) > max_step:
                    a = a + float(np.sign(delta) * max_step)
                else:
                    a = g
            self.applied[name] = float(np.clip(a, lo, hi))
        self.applied = complete_g20_mimic_positions(self.applied)
        self.targets = self.applied

    def apply_targets(self) -> None:
        self._slew_applied_toward_goal()
        # PD：绝不能驱动 mimic（与 PhysX mimic 冲突 → Invalid PhysX transform）
        # 位置硬写：写全关节（含已解析 mimic）
        if self.use_physics_drive:
            name_iter = (n for n in self.applied if n in self.actuated)
        else:
            name_iter = self.applied.keys()

        names = []
        vals = []
        for name in name_iter:
            rad = self.applied[name]
            idx = self.dof_index.get(name)
            if idx is None:
                continue
            names.append(name)
            vals.append(float(rad))
        if not names:
            return
        indices = [self.dof_index[n] for n in names]
        positions = np.asarray(vals, dtype=np.float64)
        if self.use_physics_drive:
            self.art.set_dof_position_targets(positions, dof_indices=indices)
        else:
            self.art.set_dof_positions(positions, dof_indices=indices)
            try:
                self.art.set_dof_velocities(
                    np.zeros(len(indices), dtype=np.float64), dof_indices=indices
                )
            except Exception:
                pass

    def recover_if_invalid(self) -> bool:
        """若出现 NaN/Inf，复位到张开名义位姿。"""
        pos = self.art.get_dof_positions().numpy().flatten()
        if np.isfinite(pos).all():
            return False
        self.node.get_logger().warn("invalid dof state (NaN/Inf) → reset to open pose")
        open_full = complete_g20_mimic_positions(
            sdk_range_to_full_urdf_positions(open_hand_command("G20"), "G20", "left")
        )
        self.goal = dict(open_full)
        self.applied = dict(open_full)
        self.targets = self.applied
        self.track_nominal = {n: float(open_full[n]) for n in self.actuated if n in open_full}
        names = []
        vals = []
        for name, rad in open_full.items():
            idx = self.dof_index.get(name)
            if idx is None:
                continue
            names.append(name)
            vals.append(float(np.clip(rad, float(self._lo[idx]), float(self._hi[idx]))))
        if names:
            indices = [self.dof_index[n] for n in names]
            self.art.set_dof_positions(np.asarray(vals, dtype=np.float64), dof_indices=indices)
            try:
                self.art.set_dof_velocities(
                    np.zeros(len(indices), dtype=np.float64), dof_indices=indices
                )
            except Exception:
                pass
        return True

    def publish_state(self) -> None:
        pos = self.art.get_dof_positions().numpy().flatten()
        try:
            vel = self.art.get_dof_velocities().numpy().flatten()
        except Exception:
            vel = np.zeros_like(pos)
        msg = JointState()
        msg.header.stamp = self.node.get_clock().now().to_msg()
        msg.name = list(self.dof_names)
        msg.position = [float(x) for x in pos]
        msg.velocity = [float(x) for x in vel]
        self.pub.publish(msg)
        if self.legacy_pub is not None:
            by_name = {n: float(pos[i]) for i, n in enumerate(self.dof_names)}
            sdk = urdf_positions_to_sdk_range(by_name, "G20", "left")
            leg = JointState()
            leg.header = msg.header
            leg.name = [f"joint_{i}" for i in range(20)]
            leg.position = [float(v) for v in sdk]
            self.legacy_pub.publish(leg)

    def tracking_error(self) -> tuple[float, float]:
        """Return (max_abs, mean_abs) over actuated joints vs track_nominal."""
        if not self.track_nominal:
            return 0.0, 0.0
        pos = self.art.get_dof_positions().numpy().flatten()
        errs = []
        for name, tgt in self.track_nominal.items():
            idx = self.dof_index.get(name)
            if idx is None:
                continue
            errs.append(abs(float(pos[idx]) - float(tgt)))
        if not errs:
            return 0.0, 0.0
        return float(max(errs)), float(sum(errs) / len(errs))


def _compare_camera_eye_target() -> tuple[list[float], list[float]]:
    """正对掌心的 eye/target（世界系），与 MuJoCo COMPARE_* 一致（从 +X 看）。"""
    import math

    ox, oy, oz = 0.0, HAND_MOUNT_Y, HAND_MOUNT_Z
    lx, ly, lz = COMPARE_LOOKAT_LOCAL
    target = [ox + lx, oy + ly, oz + lz]
    elev = math.radians(COMPARE_ELEVATION_DEG)
    # 从 +X 看向 lookat，略俯视（与 MuJoCo azimuth=180 同向）
    eye = [
        target[0] + COMPARE_DISTANCE * math.cos(elev),
        target[1],
        target[2] - COMPARE_DISTANCE * math.sin(elev),
    ]
    return eye, target


def main() -> int:
    stage_utils.create_new_stage()
    stage_utils.set_stage_units(meters_per_unit=1.0)
    GroundPlane("/World/GroundPlane", positions=[0.0, 0.0, 0.0])
    DistantLight("/World/DistantLight").set_intensities(300.0)
    eye, target = _compare_camera_eye_target()
    set_camera_view(eye=eye, target=target, camera_prim_path="/OmniverseKit_Persp")

    hand_path = _load_g20(args.g20_usd)
    SimulationManager.set_physics_dt(1.0 / 120.0)

    app_utils.play()
    simulation_app.update()
    _fix_base_to_world(hand_path)
    app_utils.stop()
    simulation_app.update()
    app_utils.play()
    simulation_app.update()
    # play/stop 后重设，避免 viewport 被重置
    eye, target = _compare_camera_eye_target()
    set_camera_view(eye=eye, target=target, camera_prim_path="/OmniverseKit_Persp")

    art = Articulation(hand_path)
    use_pd = bool(args.pd_drive)
    if args.hand_gravity and not use_pd:
        print("[warn] --hand-gravity ignored without --pd-drive (PLAN default is position drive)")
    # 默认跟随 USD：自碰开启（防穿模）；仅 --no-self-collision 关闭。
    enable_self_col = not bool(args.no_self_collision)
    try:
        art.set_enabled_self_collisions([enable_self_col])
    except Exception as exc:
        print(f"[stabilize] self_collision set failed: {exc}")
    print(f"[stabilize] self_collision={'ON' if enable_self_col else 'OFF'} (USD selective colliders)")
    art.set_link_enabled_gravities(bool(args.hand_gravity) and use_pd)
    if use_pd:
        act = [
            i
            for i, n in enumerate(art.dof_names)
            if n in g20_actuated_joint_names()
        ]
        stiff = np.array(
            [
                THUMB_PD_STIFFNESS if art.dof_names[i] in THUMB_ACTUATED else PD_STIFFNESS
                for i in act
            ],
            dtype=float,
        )
        damp = np.array(
            [
                THUMB_PD_DAMPING if art.dof_names[i] in THUMB_ACTUATED else PD_DAMPING
                for i in act
            ],
            dtype=float,
        )
        effort = np.array(
            [
                THUMB_PD_MAX_EFFORT if art.dof_names[i] in THUMB_ACTUATED else PD_MAX_EFFORT
                for i in act
            ],
            dtype=float,
        )
        art.set_dof_gains(stiffnesses=stiff, dampings=damp, dof_indices=act)
        art.set_dof_max_efforts(effort, dof_indices=act)
        # mimic 零增益，避免与 PhysX mimic 冲突
        mimic_idx = [
            i
            for i, n in enumerate(art.dof_names)
            if n not in g20_actuated_joint_names()
        ]
        if mimic_idx:
            art.set_dof_gains(
                stiffnesses=np.zeros(len(mimic_idx)),
                dampings=np.zeros(len(mimic_idx)),
                dof_indices=mimic_idx,
            )
    else:
        # 位置硬写：零增益避免与 set_dof_positions 冲突
        n = len(art.dof_names)
        art.set_dof_gains(stiffnesses=np.zeros(n), dampings=np.zeros(n))

    if not rclpy.ok():
        rclpy.init()
    node = Node("isaac_g20_plan_bridge")
    try:
        use_cmd_u8 = not bool(args.plan_nominal)
        cmd_topic = TOPIC_LEGACY_CMD if args.legacy_sdk else str(args.cmd_topic)
        bridge = G20PlanBridge(
            node,
            art,
            use_cmd_u8=use_cmd_u8,
            cmd_topic=cmd_topic,
            use_physics_drive=use_pd,
            nominal_topic=args.nominal_topic,
            sim_state_topic=args.sim_state_topic,
            slew_rad=float(args.slew_rad),
        )
    except RuntimeError as exc:
        node.get_logger().error(str(exc))
        node.destroy_node()
        rclpy.shutdown()
        simulation_app.close()
        return 1

    # 初始 snap 到 open（跳过 slew）
    old_slew = bridge.slew_rad
    bridge.slew_rad = 0.0
    bridge.apply_targets()
    bridge.slew_rad = old_slew
    for _ in range(10):
        SimulationManager.step()
        simulation_app.update()

    if args.plan_nominal:
        mode = "PLAN nominal"
    elif args.legacy_sdk:
        mode = f"cmd_u8:{TOPIC_LEGACY_CMD}"
    else:
        mode = f"cmd_u8:{args.cmd_topic}"
    drive = "PD" if use_pd else "position"
    node.get_logger().info(
        f"仿真就绪 [{mode}, drive={drive}, slew={args.slew_rad} rad/step, "
        f"phys/render={max(1, int(args.phys_per_render))}] pub={args.sim_state_topic}"
    )

    track_phase = 0
    track_ok = True
    max_steps = int(args.max_steps)
    if args.track_test and max_steps <= 0:
        max_steps = 480
    phys_per_render = max(1, int(args.phys_per_render))

    step = 0
    try:
        while simulation_app.is_running():
            for _ in range(phys_per_render):
                rclpy.spin_once(node, timeout_sec=0.0)
                if args.track_test and step % 160 == 0:
                    if track_phase % 2 == 0:
                        cmd = open_hand_command("G20")
                    else:
                        cmd = [80.0] * 20
                    bridge.set_nominal_targets(
                        sdk_range_to_full_urdf_positions(cmd, "G20", "left")
                    )
                    track_phase += 1
                bridge.apply_targets()
                SimulationManager.step()
                if bridge.recover_if_invalid():
                    bridge.apply_targets()
                    SimulationManager.step()
                bridge.publish_state()
                # 位置驱动：物理步进后再钉一次
                if not use_pd:
                    bridge.apply_targets()
                step += 1
                if max_steps > 0 and step >= max_steps:
                    break
            RenderingManager.render()
            simulation_app.update()
            if max_steps > 0 and step >= max_steps:
                break
    finally:
        if args.track_test:
            # settle：给够 slew 走完
            for _ in range(240):
                bridge.apply_targets()
                SimulationManager.step()
                simulation_app.update()
            mx, mean = bridge.tracking_error()
            print(
                f"[track-test] actuated max_abs={mx:.4f} rad mean_abs={mean:.4f} rad "
                f"tol={TRACK_TOL_RAD}"
            )
            track_ok = mx <= TRACK_TOL_RAD
            print("[track-test] RESULT:", "PASS" if track_ok else "FAIL")
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
        simulation_app.close()

    return 0 if track_ok else 2


if __name__ == "__main__":
    sys.exit(main())