isaac_linkerbot/linker_hand_dual_sim.py

#!/usr/bin/env python3
# SPDX-License-Identifier: Apache-2.0
"""在 Isaac Sim 中加载 LinkerHand G20 灵巧手，并通过 ROS2 话题控制。

默认仅加载 G20（``linkerhand_g20_left`` USD）。
默认 PD 动力学，以体现交付 USD 中的自碰/掌心接触垫；
``--position-drive`` 硬写关节（UI 跟手，但会穿模）；
``--hand-gravity`` 开手部重力。
"""

from __future__ import annotations

import argparse
import sys
from pathlib import Path

LINKERHAND_ROOT = Path(__file__).resolve().parent
URDF_ROOT = LINKERHAND_ROOT / "urdf"

DEFAULT_ASSETS = {
    ("O6", "left"): URDF_ROOT / "o6/left/linkerhand_o6_left/linkerhand_o6_left.usda",
    ("G20", "left"): URDF_ROOT / "g20/left/linkerhand_g20_left/linkerhand_g20_left.usda",
}

TOPICS = {
    "O6": {
        "cmd": "/o6/cb_left_hand_control_cmd",
        "state": "/o6/cb_left_hand_state",
    },
    "G20": {
        "cmd": "/cb_left_hand_control_cmd",
        "state": "/cb_left_hand_state",
    },
}

# 默认 PD：交付 USD 的自碰/掌心垫才会生效。硬写位置用 --position-drive（可穿模）。
DEFAULT_DRIVE_STIFFNESS = 80.0
DEFAULT_DRIVE_DAMPING = 25.0
G20_DRIVE_STIFFNESS = 25.0
G20_DRIVE_DAMPING = 40.0
GRAVITY_STIFFNESS_SCALE = 1.0
GRAVITY_DAMPING_SCALE = 1.2
DEFAULT_ACTUATED_MAX_EFFORT = 8.0
# 每物理步最大目标变化（rad）
MAX_TARGET_STEP_RAD = 0.006
CMD_SMOOTH_ALPHA = 0.08
CMD_SETTLE_EPS = 0.5
# 手掌安装高度（米）——抬高避免手指触地冲飞
HAND_MOUNT_Z = 0.70
HAND_MOUNT_Y = 0.45
# 根节点偏离安装位超过该距离视为爆炸并复位
ROOT_EXPLODE_DIST_M = 0.50
# 接触尖峰可达上千 rad/s；硬复位只会更抖。真正爆炸只看 NaN/根漂移。
DOF_VEL_SOFT_CLAMP = 30.0
DOF_VEL_HARD_LIMIT = 1.0e9


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="LinkerHand O6 + G20 dual-hand Isaac Sim ROS2 bridge")
    parser.add_argument("--headless", action="store_true", help="无 GUI 运行")
    parser.add_argument("--kinematic", action="store_true", help="运动学驱动（无物理，不适合测摩擦）")
    parser.add_argument(
        "--hand-gravity",
        action="store_true",
        help="启用手部连杆重力（PD 下生效；硬写位置驱动会忽略）",
    )
    parser.add_argument(
        "--pd-drive",
        action="store_true",
        help="PD 动力学驱动（默认已开启；与 --position-drive 互斥时以位置驱动为准）",
    )
    parser.add_argument(
        "--self-collision",
        action="store_true",
        help="强制走 PD 以便体现 USD 自碰（默认 PD 下已启用 USD 自碰）",
    )
    parser.add_argument(
        "--anti-penetration",
        action="store_true",
        help="兼容旧参数：碰撞/自碰已烘焙进 USD，无需再开",
    )
    parser.add_argument(
        "--no-collision",
        action="store_true",
        help="关闭全部碰撞几何（调试用）",
    )
    parser.add_argument(
        "--position-drive",
        action="store_true",
        help="硬写关节位置（UI 跟手，但会穿模；忽略 USD 自碰）",
    )
    parser.add_argument("--drive-stiffness", type=float, default=DEFAULT_DRIVE_STIFFNESS)
    parser.add_argument("--drive-damping", type=float, default=DEFAULT_DRIVE_DAMPING)
    parser.add_argument("--g20-drive-stiffness", type=float, default=G20_DRIVE_STIFFNESS)
    parser.add_argument("--g20-drive-damping", type=float, default=G20_DRIVE_DAMPING)
    parser.add_argument("--cmd-smooth", type=float, default=CMD_SMOOTH_ALPHA, help="指令低通 0~1，越大越跟手")
    parser.add_argument("--o6-usd", type=Path, default=DEFAULT_ASSETS[("O6", "left")])
    parser.add_argument("--g20-usd", type=Path, default=DEFAULT_ASSETS[("G20", "left")])
    parser.add_argument("--l20-usd", type=Path, dest="g20_usd", help=argparse.SUPPRESS)
    parser.add_argument("--o6-side", choices=["left", "right"], default="left")
    parser.add_argument("--g20-side", choices=["left", "right"], default="left")
    parser.add_argument("--o6-only", action="store_true", help="仅加载 O6")
    parser.add_argument("--g20-only", action="store_true", help="仅加载 G20")
    parser.add_argument("--both", action="store_true", help="同时加载 O6 + G20")
    parser.add_argument("--l20-side", choices=["left", "right"], dest="g20_side", help=argparse.SUPPRESS)
    parser.add_argument("--max-steps", type=int, default=0, help="运行指定步数后退出，0 表示持续运行")
    parser.add_argument(
        "--hold-open",
        action="store_true",
        help="忽略外部 control_cmd，始终保持 SDK 张开姿态（排查漂移/外部节点干扰）",
    )
    parser.add_argument(
        "--stability-test",
        action="store_true",
        help="跑固定步数并打印 G20 状态漂移统计后退出（自动 hold-open）",
    )
    parser.add_argument(
        "--motion-test",
        action="store_true",
        help="开合循环压力测试（内部切指令，不依赖外部 ROS pub）",
    )
    return parser.parse_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 (
    G20_OPEN_CMD,
    O6_SDK_JOINTS,
    mimic_joint_names,
    open_hand_command,
    sdk_range_to_full_urdf_positions,
    sdk_range_to_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


class HandBridge:
    """单只手的 ROS2 ↔ Articulation 桥。"""

    def __init__(
        self,
        node: Node,
        articulation: Articulation,
        hand: str,
        side: str,
        cmd_topic: str,
        state_topic: str,
        *,
        kinematic: bool,
        cmd_smooth: float,
        hold_open: bool = False,
        use_physics_drive: bool = False,
    ) -> None:
        self.node = node
        self.art = articulation
        self.hand = hand
        self.mapping_hand = hand if hand in ("O6", "G20", "L20") else "L20"
        self.side = side
        self.kinematic = kinematic
        self.hold_open = hold_open
        self.use_physics_drive = use_physics_drive
        self.cmd_smooth = float(np.clip(cmd_smooth, 0.0, 1.0))
        self.dof_count = 6 if hand == "O6" else 20
        open_cmd = open_hand_command(hand)
        self.cmd_values = list(open_cmd)
        self.smoothed_cmd = list(open_cmd)
        self._last_applied_key: tuple[float, ...] | None = None
        self._slew_targets: dict[int, float] = {}
        self._mount_position = None  # set by main after load
        self.dof_names = list(articulation.dof_names)
        self.dof_index = {name: i for i, name in enumerate(self.dof_names)}
        self.mimic_dofs = mimic_joint_names(self.mapping_hand, side)
        self.actuated_dof_indices = [
            i for i, name in enumerate(self.dof_names) if name not in self.mimic_dofs
        ]
        self._actuated_names = {self.dof_names[i] for i in self.actuated_dof_indices}

        lower, upper = articulation.get_dof_limits()
        self._limit_lo = lower.numpy().flatten()
        self._limit_hi = upper.numpy().flatten()

        if not hold_open:
            self.sub = node.create_subscription(JointState, cmd_topic, self._on_cmd, 10)
        self.pub = node.create_publisher(JointState, state_topic, 10)
        self._recover_cooldown = 0
        self.explode_count = 0
        self.max_root_drift = 0.0
        mode = "kinematic" if kinematic else "dynamic(PD)"
        hold_note = ", hold-open" if hold_open else ""
        drive_note = "PD" if use_physics_drive else "position"
        node.get_logger().info(
            f"{hand} ({side}) [{mode}{hold_note}, drive={drive_note}]: sub {cmd_topic}, "
            f"actuated={len(self.actuated_dof_indices)} mimic={len(self.mimic_dofs)}"
        )

    def set_sdk_command(self, values: list[float]) -> None:
        """内部压力测试：直接写 SDK 指令（绕过 ROS）。"""
        n = min(len(values), self.dof_count)
        self.cmd_values[:n] = [float(v) for v in values[:n]]

    def _on_cmd(self, msg: JointState) -> None:
        if self.hold_open or not msg.position:
            return
        n = min(len(msg.position), self.dof_count)
        incoming = [float(v) for v in msg.position[:n]]
        if all(abs(incoming[i] - self.cmd_values[i]) < CMD_SETTLE_EPS for i in range(n)):
            return
        self.cmd_values[:n] = incoming

    def _smooth_commands(self) -> None:
        if self.hold_open:
            self.cmd_values = list(open_hand_command(self.hand))
            self.smoothed_cmd = list(self.cmd_values)
            return
        a = self.cmd_smooth
        if a <= 0.0:
            self.smoothed_cmd = list(self.cmd_values)
            return
        for i in range(self.dof_count):
            self.smoothed_cmd[i] = (1.0 - a) * self.smoothed_cmd[i] + a * self.cmd_values[i]
        if max(abs(self.smoothed_cmd[i] - self.cmd_values[i]) for i in range(self.dof_count)) < CMD_SETTLE_EPS:
            self.smoothed_cmd = list(self.cmd_values)

    def _resolve_targets(self) -> dict[str, float]:
        # 位置驱动/运动学：写全关节（含 mimic，比率需与 URDF 一致）
        if self.kinematic or not self.use_physics_drive:
            return sdk_range_to_full_urdf_positions(self.smoothed_cmd, self.mapping_hand, self.side)
        return sdk_range_to_urdf_positions(self.smoothed_cmd, self.mapping_hand, self.side)

    def _targets_to_dof_lists(self, targets: dict[str, float]) -> tuple[list[int], list[float]]:
        dof_indices: list[int] = []
        positions: list[float] = []
        for joint_name, rad in targets.items():
            idx = self._resolve_dof_index(joint_name)
            if idx is None:
                continue
            lo = float(self._limit_lo[idx])
            hi = float(self._limit_hi[idx])
            if hi > lo:
                rad = float(np.clip(rad, lo, hi))
            # 斜坡限制：相对上一帧已下发目标，避免大跳
            prev = self._slew_targets.get(idx, rad)
            delta = rad - prev
            if abs(delta) > MAX_TARGET_STEP_RAD:
                rad = prev + float(np.sign(delta) * MAX_TARGET_STEP_RAD)
            if hi > lo:
                rad = float(np.clip(rad, lo, hi))
            self._slew_targets[idx] = rad
            dof_indices.append(idx)
            positions.append(rad)
        return dof_indices, positions

    def _is_exploded(self) -> tuple[bool, str]:
        """返回 (是否需硬复位, 原因)。速度尖峰单独处理，不触发张开复位。"""
        try:
            root = self.art.get_world_poses()[0]
            root = root.numpy().flatten() if hasattr(root, "numpy") else np.asarray(root).flatten()
            if np.isnan(root).any() or np.isinf(root).any():
                return True, "root_nan"
            if self._mount_position is not None:
                mount = np.asarray(self._mount_position, dtype=float)
                drift = float(np.linalg.norm(root[:3] - mount))
                self.max_root_drift = max(self.max_root_drift, drift)
                limit = ROOT_EXPLODE_DIST_M if self.use_physics_drive else 5.0
                if drift > limit:
                    return True, f"root_drift={drift:.3f}m"
            elif float(np.max(np.abs(root))) > 5.0:
                return True, "root_far"

            pos = self.art.get_dof_positions()
            pos = pos.numpy().flatten() if hasattr(pos, "numpy") else np.asarray(pos).flatten()
            if np.isnan(pos).any() or np.isinf(pos).any():
                return True, "dof_pos_nan"

            if self.use_physics_drive:
                try:
                    vel = self.art.get_dof_velocities()
                    vel = vel.numpy().flatten() if hasattr(vel, "numpy") else np.asarray(vel).flatten()
                    if np.isnan(vel).any() or np.isinf(vel).any():
                        return True, "dof_vel_nan"
                    max_abs = float(np.max(np.abs(vel))) if len(vel) else 0.0
                    if max_abs > DOF_VEL_HARD_LIMIT:
                        return True, f"dof_vel={max_abs:.1f}"
                    if max_abs > DOF_VEL_SOFT_CLAMP:
                        # 自碰接触尖峰：只清速度，不当成爆炸
                        try:
                            self.art.set_dof_velocities([0.0] * len(vel))
                        except Exception:
                            pass
                        return False, f"vel_spike={max_abs:.1f}_clamped"
                except Exception:
                    pass
        except Exception as exc:
            return True, f"exception:{exc}"
        return False, ""

    def _pin_root(self) -> None:
        """把掌根钉回安装位（仅硬写位置驱动备份；PD+WorldFixedJoint 时不要调）。"""
        if self._mount_position is None or self.use_physics_drive:
            return
        try:
            self.art.set_world_poses(
                positions=[self._mount_position],
                orientations=[[1.0, 0.0, 0.0, 0.0]],
            )
            if hasattr(self.art, "set_velocities"):
                self.art.set_velocities(
                    linear_velocities=[[0.0, 0.0, 0.0]],
                    angular_velocities=[[0.0, 0.0, 0.0]],
                )
        except Exception as exc:
            self.node.get_logger().warn(f"{self.hand}: _pin_root failed: {exc}")

    def recover_if_exploded(self) -> bool:
        """物理爆炸后复位到张开姿态。返回是否发生了硬复位。"""
        if getattr(self, "_recover_cooldown", 0) > 0:
            self._recover_cooldown -= 1
            if not self.use_physics_drive:
                self._pin_root()
            # PD 下若仍 NaN，必须再 snap 一次，否则冷却期结束后会立刻再爆
            try:
                pos = self.art.get_dof_positions()
                pos = pos.numpy().flatten() if hasattr(pos, "numpy") else np.asarray(pos).flatten()
                if np.isnan(pos).any() or np.isinf(pos).any():
                    self.initialize_pose()
                    self.art.set_dof_velocities([0.0] * int(self.art.num_dofs))
            except Exception:
                try:
                    self.initialize_pose()
                except Exception:
                    pass
            return True
        exploded, reason = self._is_exploded()
        if not exploded:
            return False
        try:
            root = self.art.get_world_poses()[0]
            root = root.numpy().flatten() if hasattr(root, "numpy") else np.asarray(root).flatten()
            root_txt = [round(float(x), 3) for x in root[:3]]
        except Exception:
            root_txt = ["?"]
        self.node.get_logger().warn(
            f"{self.hand}: 物理爆炸复位 reason={reason} root={root_txt}"
        )
        self.explode_count += 1
        self.cmd_values = list(open_hand_command(self.hand))
        self.smoothed_cmd = list(self.cmd_values)
        self._slew_targets.clear()
        self._last_applied_key = None
        self._pin_root()
        try:
            n = self.art.num_dofs
            if n > 0:
                self.art.set_dof_velocities([0.0] * n)
        except Exception:
            pass
        self.initialize_pose()
        self._pin_root()
        self._recover_cooldown = 30
        return True

    def apply_targets(self) -> None:
        if self.recover_if_exploded():
            return
        # 位置驱动：每帧钉掌根（O6 无 WorldFixedJoint；G20 作备份）
        if not self.use_physics_drive:
            self._pin_root()
        self._smooth_commands()
        targets = self._resolve_targets()
        dof_indices, positions = self._targets_to_dof_lists(targets)
        if not dof_indices:
            return
        apply_key = tuple(round(p, 5) for p in positions)
        self._last_applied_key = apply_key
        if self.kinematic or not self.use_physics_drive:
            self.art.set_dof_positions(positions, dof_indices=dof_indices)
        else:
            self.art.set_dof_position_targets(positions, dof_indices=dof_indices)

    def _read_sdk_state(self) -> list[float]:
        dof_pos = self.art.get_dof_positions()
        if hasattr(dof_pos, "numpy"):
            pos_array = dof_pos.numpy().flatten()
        else:
            pos_array = np.asarray(dof_pos).flatten()

        joint_pos_map: dict[str, float] = {}
        for name in self._actuated_names:
            idx = self.dof_index.get(name)
            if idx is not None and idx < len(pos_array):
                joint_pos_map[name] = float(pos_array[idx])
        return [float(v) for v in urdf_positions_to_sdk_range(joint_pos_map, self.hand, self.side)]

    def publish_state(self) -> None:
        sdk_range = self._read_sdk_state()
        msg = JointState()
        msg.header.stamp = self.node.get_clock().now().to_msg()
        if self.hand == "O6":
            msg.name = list(O6_SDK_JOINTS)
        else:
            msg.name = [f"motor_{i}" for i in range(20)]
        msg.position = sdk_range
        self.pub.publish(msg)

    def _resolve_dof_index(self, joint_name: str) -> int | None:
        if joint_name in self.dof_index:
            return self.dof_index[joint_name]
        for name, idx in self.dof_index.items():
            if name.endswith(joint_name) or name.split("/")[-1] == joint_name:
                return idx
        return None

    def snap_pose(self) -> None:
        """强制写入关节角与 PD 目标（初始化/复位）。"""
        if self.hold_open:
            self.smoothed_cmd = list(open_hand_command(self.hand))
            self.cmd_values = list(self.smoothed_cmd)
        # snap 时跳过斜坡，直接到位
        self._slew_targets.clear()
        targets = self._resolve_targets()
        # 直接限幅，不走斜坡字典预填
        dof_indices: list[int] = []
        positions: list[float] = []
        for joint_name, rad in targets.items():
            idx = self._resolve_dof_index(joint_name)
            if idx is None:
                continue
            lo = float(self._limit_lo[idx])
            hi = float(self._limit_hi[idx])
            if hi > lo:
                rad = float(np.clip(rad, lo, hi))
            self._slew_targets[idx] = rad
            dof_indices.append(idx)
            positions.append(rad)
        if not dof_indices:
            return
        self.art.set_dof_positions(positions, dof_indices=dof_indices)
        if not self.kinematic and self.use_physics_drive:
            self.art.set_dof_position_targets(positions, dof_indices=dof_indices)
        self._last_applied_key = tuple(round(p, 5) for p in positions)

    def initialize_pose(self) -> None:
        self.snap_pose()
        if not self.kinematic and self.use_physics_drive and self.actuated_dof_indices:
            self.art.set_dof_velocities(
                [0.0] * len(self.actuated_dof_indices),
                dof_indices=self.actuated_dof_indices,
            )


def _configure_drives(
    articulation: Articulation,
    hand: str,
    side: str,
    *,
    kinematic: bool,
    stiffness: float,
    damping: float,
    max_effort: float = DEFAULT_ACTUATED_MAX_EFFORT,
) -> None:
    mapping_hand = "O6" if hand == "O6" else "L20"
    mimics = mimic_joint_names(mapping_hand, side)
    actuated: list[int] = []
    mimic_indices: list[int] = []
    for i, name in enumerate(articulation.dof_names):
        (mimic_indices if name in mimics else actuated).append(i)

    if kinematic:
        n = articulation.num_dofs
        if n > 0:
            articulation.set_dof_gains(stiffnesses=[0.0] * n, dampings=[0.0] * n)
        return

    # 位置驱动遥操作：增益必须为 0，否则 PD 与 set_dof_positions 硬刚导致爆炸
    if stiffness <= 0.0 and damping <= 0.0:
        n = articulation.num_dofs
        if n > 0:
            articulation.set_dof_gains(stiffnesses=[0.0] * n, dampings=[0.0] * n)
        return

    if actuated:
        articulation.set_dof_gains(
            stiffnesses=[stiffness] * len(actuated),
            dampings=[damping] * len(actuated),
            dof_indices=actuated,
        )
        articulation.set_dof_max_efforts(
            [max_effort] * len(actuated),
            dof_indices=actuated,
        )
    if mimic_indices:
        # 不要把 mimic 的 stiffness 设为 0：部分后端会破坏 mimic 约束，连带整手塌陷
        articulation.set_dof_max_efforts(
            [max_effort] * len(mimic_indices),
            dof_indices=mimic_indices,
        )


def _author_self_collisions(hand_path: str, enabled: bool) -> None:
    """在 ArticulationRoot 上写入 PhysX/Newton 自碰，并配置碰撞体。

    enabled=True 时：关掉掌心/CMC 肥凸包，只保留手指段碰撞，降低自碰爆炸概率。
    """
    from pxr import Sdf, Usd, UsdPhysics

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

    try:
        from pxr import PhysxSchema

        has_physx_schema = True
    except ImportError:
        has_physx_schema = False

    # 这些碰撞体与拇指运动包络严重重叠，开自碰必顶死/NaN
    disable_col_names = {
        "hand_base_link",
        "thumb_metacarpals_base1",
        "thumb_metacarpals_base2",
        "thumb_metacarpals",
        "index_metacarpals",
        "middle_metacarpals",
        "ring_metacarpals",
        "pinky_metacarpals",
    }

    predicate = Usd.TraverseInstanceProxies(Usd.PrimAllPrimsPredicate)
    n_roots = 0
    n_col_on = 0
    n_col_off = 0
    for prim in Usd.PrimRange(hand_prim, predicate):
        if prim.HasAPI(UsdPhysics.ArticulationRootAPI) or prim.HasAPI("PhysicsArticulationRootAPI"):
            if has_physx_schema:
                PhysxSchema.PhysxArticulationAPI.Apply(prim)
            attr = prim.GetAttribute("physxArticulation:enabledSelfCollisions")
            if not attr:
                attr = prim.CreateAttribute(
                    "physxArticulation:enabledSelfCollisions", Sdf.ValueTypeNames.Bool
                )
            attr.Set(bool(enabled))
            nattr = prim.GetAttribute("newton:selfCollisionEnabled")
            if not nattr:
                nattr = prim.CreateAttribute("newton:selfCollisionEnabled", Sdf.ValueTypeNames.Bool)
            nattr.Set(bool(enabled))
            n_roots += 1
        if (
            prim.HasAPI(UsdPhysics.CollisionAPI)
            or prim.HasAPI("PhysicsCollisionAPI")
            or prim.HasAPI("PhysicsMeshCollisionAPI")
        ):
            name = prim.GetName()
            col_on = True
            if enabled and name in disable_col_names:
                col_on = False
            try:
                if prim.HasAPI(UsdPhysics.CollisionAPI):
                    UsdPhysics.CollisionAPI(prim).CreateCollisionEnabledAttr().Set(col_on)
                else:
                    attr = prim.GetAttribute("physics:collisionEnabled")
                    if not attr:
                        attr = prim.CreateAttribute(
                            "physics:collisionEnabled", Sdf.ValueTypeNames.Bool
                        )
                    attr.Set(col_on)
                if col_on:
                    n_col_on += 1
                else:
                    n_col_off += 1
            except Exception:
                pass
    print(
        f"[stabilize] authored self_collision={enabled} roots={n_roots} "
        f"col_on={n_col_on} col_off={n_col_off} under {hand_path}"
    )
    if enabled:
        _filter_thumb_palm_collisions(hand_path)


def _filter_thumb_palm_collisions(hand_path: str) -> None:
    """拇指链与掌根凸包在 CMC 处天然重叠；PhysX 只自动忽略父子连杆。

    不过滤时自碰会顶住拇指。处理：
    1) 关掉 CMC 中间连杆 + 掌根碰撞体（显示 mesh 仍在；物理不互顶）
    2) palm↔拇指 FilteredPairs
    四指之间自碰仍保留，用来减轻拇指穿其它指。
    """
    from pxr import Usd, UsdPhysics

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

    predicate = Usd.TraverseInstanceProxies(Usd.PrimAllPrimsPredicate)
    palm = None
    thumb_links: list = []
    cmc_disabled = 0
    palm_col_off = 0
    for prim in Usd.PrimRange(hand_prim, predicate):
        name = prim.GetName()
        if prim.HasAPI(UsdPhysics.CollisionAPI) and name in (
            "thumb_metacarpals_base1",
            "thumb_metacarpals_base2",
            "hand_base_link",
        ):
            try:
                UsdPhysics.CollisionAPI(prim).CreateCollisionEnabledAttr().Set(False)
                if name == "hand_base_link":
                    palm_col_off += 1
                else:
                    cmc_disabled += 1
            except Exception as exc:
                print(f"[stabilize] collision disable failed {prim.GetPath()}: {exc}")
        if not prim.HasAPI(UsdPhysics.RigidBodyAPI):
            continue
        if name in ("hand_base_link", "base_link", "palm"):
            palm = prim
        elif name.startswith("thumb_"):
            thumb_links.append(prim)

    if palm is None or not thumb_links:
        print(
            f"[stabilize] thumb-palm filter skipped "
            f"(palm={palm is not None}, thumbs={len(thumb_links)}, "
            f"cmc_off={cmc_disabled}, palm_col_off={palm_col_off})"
        )
        return

    api = UsdPhysics.FilteredPairsAPI.Apply(palm)
    rel = api.CreateFilteredPairsRel()
    existing = {str(p) for p in (rel.GetTargets() or [])}
    added = 0
    for link in thumb_links:
        path = link.GetPath()
        if str(path) in existing:
            continue
        rel.AddTarget(path)
        other = UsdPhysics.FilteredPairsAPI.Apply(link)
        other_rel = other.CreateFilteredPairsRel()
        if palm.GetPath() not in (other_rel.GetTargets() or []):
            other_rel.AddTarget(palm.GetPath())
        added += 1
    print(
        f"[stabilize] palm↔thumb filter pairs={added} cmc_off={cmc_disabled} "
        f"palm_col_off={palm_col_off} thumbs={[t.GetName() for t in thumb_links]}"
    )


def _stabilize_articulation(
    articulation: Articulation,
    *,
    enable_gravity: bool,
    disable_collisions: bool = False,
    enable_self_collisions: bool = False,
) -> None:
    """动力学仿真稳定性：默认保留碰撞；自碰需配合 PD 位置目标才有效。"""
    try:
        articulation.set_enabled_self_collisions([bool(enable_self_collisions)])
    except Exception as exc:
        print(f"[stabilize] set_enabled_self_collisions failed: {exc}")
    articulation.set_link_enabled_gravities(enable_gravity)
    if disable_collisions:
        from pxr import Usd, UsdPhysics

        try:
            link_groups = articulation.link_paths
        except Exception:
            link_groups = []
        stage = stage_utils.get_current_stage()
        predicate = Usd.TraverseInstanceProxies(Usd.PrimAllPrimsPredicate)
        n_disabled = 0
        for group in link_groups:
            for path in group:
                prim = stage.GetPrimAtPath(path)
                if not prim.IsValid():
                    continue
                for child in Usd.PrimRange(prim, predicate):
                    if child.HasAPI(UsdPhysics.CollisionAPI):
                        try:
                            UsdPhysics.CollisionAPI(child).CreateCollisionEnabledAttr().Set(False)
                            n_disabled += 1
                        except Exception:
                            pass
        print(f"[stabilize] disabled {n_disabled} collision APIs under articulation")
    else:
        print(
            f"[stabilize] collisions ON, self_collision={'ON' if enable_self_collisions else 'OFF'}"
        )


def _patch_joint_max_forces(hand_path: str, max_force: float = DEFAULT_ACTUATED_MAX_EFFORT) -> None:
    """URDF 转换默认 maxForce=1，直接改 USD 驱动上限。"""
    from pxr import Usd, UsdPhysics

    stage = stage_utils.get_current_stage()
    root = stage.GetPrimAtPath(hand_path)
    if not root.IsValid():
        return
    for prim in Usd.PrimRange(root):
        for drive_name in ("angular", "linear"):
            if not prim.HasAPI(UsdPhysics.DriveAPI, drive_name):
                continue
            drive = UsdPhysics.DriveAPI(prim, drive_name)
            attr = drive.GetMaxForceAttr()
            if attr:
                attr.Set(float(max_force))
            stiff = drive.GetStiffnessAttr()
            damp = drive.GetDampingAttr()
            # 仅保证属性存在；实际增益仍由 Articulation.set_dof_gains 写入
            if stiff and stiff.Get() is None:
                stiff.Set(0.0)
            if damp and damp.Get() is None:
                damp.Set(0.0)


def _disable_floating_root_joint(hand_path: str) -> None:
    """禁用资产内无效 root_joint（body0 非刚体）。"""
    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 prim.GetName() == "root_joint" and prim.IsA(UsdPhysics.Joint):
            prim.SetActive(False)
            print(f"[anchor] disabled floating {prim.GetPath()}")
            return


def _find_root_rigid_body(hand_prim) -> object | None:
    """找掌根刚体：优先 hand_base_link / base_link，否则第一个 RigidBody。"""
    from pxr import Usd, UsdPhysics

    preferred = None
    first = None
    for prim in Usd.PrimRange(hand_prim):
        if not prim.HasAPI(UsdPhysics.RigidBodyAPI):
            continue
        if first is None:
            first = prim
        name = prim.GetName()
        if name in ("hand_base_link", "base_link", "palm"):
            preferred = prim
            break
    return preferred or first


def _fix_base_to_world(hand_path: str) -> None:
    """把掌根钉到当前世界位姿：优先改写 USD 内 root_joint 的 localPos0/Rot0。

    交付 USD 的 root_joint 已是 body0=world、body1=palm；安装位姿由场景 Xform
    决定，因此这里只写入世界坐标，不另造 WorldFixedJoint（避免双固定）。
    """
    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 = _find_root_rigid_body(hand_prim)
    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()} body0={b0} body1={b1}")
            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 {prim.GetPath()} -> {base.GetPath()} @ "
            f"({float(pos[0]):.3f},{float(pos[1]):.3f},{float(pos[2]):.3f})"
        )
        return

    # Fallback if asset has no usable root_joint
    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} -> {base.GetPath()} @ "
        f"({float(pos[0]):.3f},{float(pos[1]):.3f},{float(pos[2]):.3f})"
    )


def _load_hand_reference(usd_path: Path, mount_path: str, position: list[float]) -> str:
    """加载手资产引用，返回 hand_path（此时尚未创建 Articulation）。"""
    if not usd_path.is_file():
        raise FileNotFoundError(f"USD 不存在: {usd_path}")
    stage_utils.define_prim(mount_path, "Xform")
    XformPrim(mount_path, positions=[position], reset_xform_op_properties=True)
    hand_path = f"{mount_path}/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 _print_g20_stability_report(samples: list[list[float]]) -> bool:
    """Return True if G20 state is stable enough."""
    import statistics

    open_pose = [float(v) for v in G20_OPEN_CMD]
    if len(samples) < 5:
        print(f"G20 stability: insufficient samples ({len(samples)})")
        return False

    max_step = 0.0
    for i in range(1, len(samples)):
        for m in range(20):
            max_step = max(max_step, abs(samples[i][m] - samples[i - 1][m]))

    motor_std = [statistics.pstdev(s[m] for s in samples) for m in range(20)]
    max_dev = max(max(abs(s[m] - open_pose[m]) for s in samples) for m in range(20))
    unstable = [m for m, std in enumerate(motor_std) if std > 1.0]

    print("=== G20 stability report ===")
    print(f"samples={len(samples)} max_step_delta={max_step:.2f} max_dev_open={max_dev:.2f}")
    print(f"unstable_motors(std>1): {unstable}")
    print(f"first: {[round(v, 1) for v in samples[0]]}")
    print(f"last:  {[round(v, 1) for v in samples[-1]]}")
    ok = max_step <= 2.0 and len(unstable) == 0 and max_dev <= 80.0
    print("RESULT:", "PASS" if ok else "FAIL")
    return ok


def main() -> None:
    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])
    light = DistantLight("/World/DistantLight")
    light.set_intensities(300.0)
    set_camera_view(eye=[0.55, 0.0, 0.55], target=[0.0, 0.0, 0.35], camera_prim_path="/OmniverseKit_Persp")

    # 默认只加载 G20；--o6-only / --both 可改
    if args.both:
        load_o6 = load_g20 = True
    elif args.o6_only:
        load_o6, load_g20 = True, False
    else:
        # --g20-only 或未指定：仅 G20
        load_o6, load_g20 = False, True

    o6_path = (
        _load_hand_reference(args.o6_usd, "/World/O6Hand", [0.0, -HAND_MOUNT_Y, HAND_MOUNT_Z])
        if load_o6
        else None
    )
    g20_path = (
        _load_hand_reference(args.g20_usd, "/World/G20Hand", [0.0, HAND_MOUNT_Y, HAND_MOUNT_Z])
        if load_g20
        else None
    )

    SimulationManager.set_physics_dt(1.0 / 120.0)

    app_utils.play()
    simulation_app.update()

    # play 之后再钉世界固定关节（此时世界位姿已正确）
    if o6_path:
        _disable_floating_root_joint(o6_path)
    if g20_path:
        _fix_base_to_world(g20_path)

    # 碰撞/自碰已写进交付 USD；此处不再运行时改 mesh
    # 仅在显式 --no-collision 时关掉
    want_self_collision_early = False
    if args.anti_penetration:
        print(
            "[stabilize] 碰撞/自碰已烘焙进 USD："
            f"{args.g20_usd if hasattr(args,'g20_usd') else 'g20'}；"
            "请直接加载该资产。运行时勿再改 collision。"
        )
        args.anti_penetration = False

    app_utils.stop()
    simulation_app.update()
    app_utils.play()
    simulation_app.update()

    # 不再调用 _author_self_collisions（交付以 USD 为准）

    app_utils.stop()
    simulation_app.update()
    app_utils.play()
    simulation_app.update()

    # 必须在最终 play 之后创建 Articulation，否则 tensor 句柄会过期
    o6 = Articulation(o6_path) if o6_path else None
    g20 = Articulation(g20_path) if g20_path else None

    hand_configs = []
    if load_o6:
        hand_configs.append((o6, "O6", args.o6_side))
    if load_g20:
        hand_configs.append((g20, "G20", args.g20_side))

    # 硬写 set_dof_positions 会穿透接触；自碰必须用 PD 目标
    # 默认 PD（交付 USD 自碰生效）；仅 --position-drive / --kinematic 关闭
    want_self_collision = (not args.no_collision) and (not args.kinematic)
    use_physics_drive_early = (not args.kinematic) and (not args.position_drive)
    if args.position_drive:
        print(
            "[stabilize] --position-drive：硬写关节，USD 自碰无法阻止穿模。"
            "防拇指穿掌请去掉该参数（默认 PD）。"
        )
    if args.self_collision and args.position_drive:
        print("[stabilize] --self-collision 与 --position-drive 冲突，改用 PD")
        use_physics_drive_early = True
    if args.hand_gravity and not use_physics_drive_early:
        print(
            "[warn] --hand-gravity 在硬写位置驱动下已忽略。"
            "测重力/摩擦: 默认 PD + --hand-gravity（不要加 --position-drive）"
        )
    enable_link_gravity = bool(args.hand_gravity) and use_physics_drive_early

    for art, hand, side in hand_configs:
        if use_physics_drive_early:
            stiff = args.drive_stiffness
            damp = args.drive_damping
            if hand == "G20":
                stiff = args.g20_drive_stiffness
                damp = args.g20_drive_damping
            if enable_link_gravity:
                stiff *= GRAVITY_STIFFNESS_SCALE
                damp *= GRAVITY_DAMPING_SCALE
            effort = DEFAULT_ACTUATED_MAX_EFFORT
        else:
            # 位置驱动：零增益，只靠 set_dof_positions
            stiff = 0.0
            damp = 0.0
            effort = DEFAULT_ACTUATED_MAX_EFFORT
        _configure_drives(
            art,
            hand,
            side,
            kinematic=args.kinematic,
            stiffness=stiff,
            damping=damp,
            max_effort=effort,
        )
        if hand == "G20" and g20_path:
            # 已在 stop/play 前写入；此处再刷一遍 API 状态
            try:
                art.set_enabled_self_collisions([want_self_collision])
            except Exception:
                pass
        elif hand == "O6" and o6_path:
            try:
                art.set_enabled_self_collisions([want_self_collision])
            except Exception:
                pass
        _stabilize_articulation(
            art,
            enable_gravity=enable_link_gravity,
            # 尊重 USD 碰撞；仅 --no-collision 时关掉
            disable_collisions=bool(args.no_collision),
            enable_self_collisions=want_self_collision and use_physics_drive_early,
        )

    simulation_app.update()

    rclpy.init()
    ros_node = Node("linker_hand_isaac_sim")

    # 与上面一致：自碰开启时必须用 PD 目标
    use_physics_drive = use_physics_drive_early
    bridge_kwargs = {
        "kinematic": args.kinematic,
        "cmd_smooth": args.cmd_smooth,
        "hold_open": args.hold_open or args.stability_test,
        "use_physics_drive": use_physics_drive,
    }
    o6_bridge = None
    g20_bridge = None
    if load_o6:
        o6_bridge = HandBridge(
            ros_node, o6, hand="O6", side=args.o6_side,
            cmd_topic=TOPICS["O6"]["cmd"], state_topic=TOPICS["O6"]["state"],
            **bridge_kwargs,
        )
        o6_bridge._mount_position = [0.0, -HAND_MOUNT_Y, HAND_MOUNT_Z]
    if load_g20:
        g20_bridge = HandBridge(
            ros_node, g20, hand="G20", side=args.g20_side,
            cmd_topic=TOPICS["G20"]["cmd"], state_topic=TOPICS["G20"]["state"],
            **bridge_kwargs,
        )
        g20_bridge._mount_position = [0.0, HAND_MOUNT_Y, HAND_MOUNT_Z]

    if o6_bridge:
        o6_bridge.initialize_pose()
    if g20_bridge:
        g20_bridge.initialize_pose()

    if use_physics_drive:
        for _ in range(120):
            if o6_bridge:
                o6_bridge.apply_targets()
            if g20_bridge:
                g20_bridge.apply_targets()
            SimulationManager.step()
            simulation_app.update()
        if g20_bridge and args.stability_test:
            ros_node.get_logger().info(f"G20 after settle: {g20_bridge._read_sdk_state()[:8]}")
    else:
        for _ in range(5):
            simulation_app.update()

    mode_str = (
        "运动学"
        if args.kinematic
        else (
            "PD+自碰"
            if use_physics_drive and want_self_collision
            else ("纯 PD 动力学" if use_physics_drive else "位置驱动(可穿模)")
        )
    )
    grav_str = "开" if enable_link_gravity else ("关(硬写忽略)" if args.hand_gravity else "关")
    ros_node.get_logger().info(
        f"仿真就绪 [{mode_str}, 手部重力={grav_str}]。"
        f"UI: run_sim.sh ；测摩擦: run_sim.sh --pd-drive --hand-gravity"
    )

    stability_samples: list[list[float]] = []
    max_steps = args.max_steps
    if args.stability_test and max_steps <= 0:
        max_steps = 960
    if args.motion_test and max_steps <= 0:
        max_steps = 720

    def _motion_pose(phase: int) -> None:
        """phase 偶数=张开，奇数=半握。"""
        if phase % 2 == 0:
            if o6_bridge:
                o6_bridge.set_sdk_command(open_hand_command("O6"))
            if g20_bridge:
                g20_bridge.set_sdk_command(open_hand_command("G20"))
        else:
            if o6_bridge:
                o6_bridge.set_sdk_command([80.0] * 6)
            if g20_bridge:
                g20_bridge.set_sdk_command([80.0] * 20)

    try:
        step = 0
        while simulation_app.is_running():
            rclpy.spin_once(ros_node, timeout_sec=0.0)
            if args.motion_test and step % 90 == 0:
                _motion_pose(step // 90)
            if o6_bridge:
                o6_bridge.apply_targets()
            if g20_bridge:
                g20_bridge.apply_targets()
            if step % 6 == 0:
                if o6_bridge:
                    o6_bridge.publish_state()
                if g20_bridge:
                    g20_bridge.publish_state()
                if args.stability_test and g20_bridge and step >= 360:
                    stability_samples.append(g20_bridge._read_sdk_state())
            SimulationManager.step()
            if not use_physics_drive:
                if o6_bridge:
                    o6_bridge._pin_root()
                if g20_bridge:
                    g20_bridge._pin_root()
            RenderingManager.render()
            simulation_app.update()
            # update() 可能再推物理，渲染前最后再钉一次
            if not use_physics_drive:
                if o6_bridge:
                    o6_bridge._pin_root()
                if g20_bridge:
                    g20_bridge._pin_root()
            step += 1
            if max_steps > 0 and step >= max_steps:
                if args.stability_test:
                    _print_g20_stability_report(stability_samples)
                if args.motion_test:
                    for br in (o6_bridge, g20_bridge):
                        if br is None:
                            continue
                        print(
                            f"[motion-test] {br.hand}: explode_count={br.explode_count} "
                            f"max_root_drift={br.max_root_drift:.4f}m"
                        )
                    ok = all(
                        br is None or br.explode_count == 0
                        for br in (o6_bridge, g20_bridge)
                    )
                    print("[motion-test] RESULT:", "PASS" if ok else "FAIL")
                else:
                    ros_node.get_logger().info(f"已完成 {max_steps} 步，退出。")
                break
    except KeyboardInterrupt:
        pass
    finally:
        ros_node.destroy_node()
        rclpy.shutdown()
        app_utils.stop()
        simulation_app.close()


if __name__ == "__main__":
    main()