#!/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()