Add LinkerHand Isaac Sim bridge and G20 assets.
Includes G20 PLAN/cmd_u8 bridge, joint mapping, dual-hand sim scripts, and G20/L20/O6 URDF with USD payloads for Isaac Sim integration. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
sunxianghui
Executable
+724
@@ -0,0 +1,724 @@
|
||||
#!/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())
|
||||
Reference in New Issue
Block a user