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