#!/usr/bin/env bash # 同时开两个 MuJoCo：G20 左手标定 vs L20 原版 URDF（未标定） # # 标定: /home/lxqs/图片/linkerhand_g20_left_tag.urdf # → 同步包内 linker_hand_g20_left.xml（限位/侧摆轴/body 零位）；mapping=g20_calibrated # （可用 G20_LEFT_URDF=... 覆盖） # 未标定: L20 V10.1 左手 URDF → 运行时按左手模板刷新 linker_hand_l20v101_left.xml # mapping=l20_urdf # # 主动关节统一按当前模型限位线性换算；g20_calibrated 仅保留可选被动 JSON 联动。 # # 默认运动学硬写。需要 PD+自碰时手动加 --pd-drive（两窗均加）。 # 两个窗口共用 /cb_left_hand_control_cmd（20 维 0~255）。 # # 用法: # ./hands/G20/scripts/run_g20_left_dual_compare.sh # ./hands/G20/scripts/run_g20_left_dual_compare.sh --cmd-topic /cb_left_hand_control_cmd --speed 1.0 # ./hands/G20/scripts/run_g20_left_dual_compare.sh --pd-drive set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)" cd "$ROOT" PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2" G20_DIR="$PKG_SRC/urdf/G20/linker_hand_g20_left" URDF_CALIBRATED="${G20_LEFT_URDF:-$ROOT/hands/G20/reference/left_calibrated.urdf}" MESHDIR_CALIB="$G20_DIR/meshes" URDF_UNCALIBRATED="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_left/linkerhand_L20v10.1_left.urdf" MESHDIR_UNCAL="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_left/meshes" CALIB_XML="$G20_DIR/linker_hand_g20_left.xml" CALIB_URDF_PKG="$G20_DIR/linkerhand_g20_left.urdf" L20_TEMPLATE="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_left/linker_hand_l20v101_left.xml" STOCK_XML="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_left/linker_hand_l20v101_left.xml" if [[ ! -f "$URDF_CALIBRATED" ]]; then # 回退到包内副本 URDF_CALIBRATED="$CALIB_URDF_PKG" fi if [[ ! -f "$URDF_CALIBRATED" ]]; then echo "[error] missing calibrated URDF: $URDF_CALIBRATED" >&2 exit 1 fi if [[ ! -f "$CALIB_XML" ]]; then echo "[error] missing calibrated MJCF template: $CALIB_XML" >&2 exit 1 fi if [[ ! -f "$URDF_UNCALIBRATED" ]]; then echo "[error] missing uncalibrated URDF: $URDF_UNCALIBRATED" >&2 exit 1 fi if [[ ! -d "$MESHDIR_UNCAL" ]]; then echo "[error] missing uncalibrated meshes: $MESHDIR_UNCAL" >&2 exit 1 fi if [[ ! -f "$L20_TEMPLATE" ]]; then echo "[error] missing L20 V10.1 left MJCF template: $L20_TEMPLATE" >&2 exit 1 fi set +u # shellcheck disable=SC1091 source /opt/ros/jazzy/setup.bash if [[ -f "$ROOT/install/setup.bash" ]]; then # shellcheck disable=SC1091 source "$ROOT/install/setup.bash" fi set -u VENV_SITE="$ROOT/venv/lib/python3.12/site-packages" if [[ -d "$VENV_SITE" ]]; then export PYTHONPATH="$VENV_SITE${PYTHONPATH:+:$PYTHONPATH}" fi export PYTHONPATH="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2${PYTHONPATH:+:$PYTHONPATH}" echo "[g20-left-dual] sync calibrated MJCF from G20 left URDF -> $CALIB_XML" python3 - "$URDF_CALIBRATED" "$MESHDIR_CALIB" "$CALIB_XML" "$CALIB_URDF_PKG" <<'PY' import re import shutil import sys from pathlib import Path import mujoco import numpy as np urdf_cal = Path(sys.argv[1]) meshdir_cal = Path(sys.argv[2]) calib_xml = Path(sys.argv[3]) calib_urdf_pkg = Path(sys.argv[4]) # 包内主副本 + tag 文件名副本 shutil.copy2(urdf_cal, calib_urdf_pkg) shutil.copy2(urdf_cal, calib_urdf_pkg.parent / "linkerhand_g20_left_tag.urdf") MCP_ROLL_JOINTS = ( "index_mcp_roll", "middle_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", ) BODY_FROM_JOINT = { "thumb_cmc_roll": "thumb_metacarpals_base1", "thumb_cmc_yaw": "thumb_metacarpals_base2", "thumb_cmc_pitch": "thumb_metacarpals", "thumb_mcp": "thumb_proximal", "thumb_ip": "thumb_distal", "index_mcp_roll": "index_metacarpals", "index_mcp_pitch": "index_proximal", "index_pip": "index_middle", "index_dip": "index_distal", "middle_mcp_roll": "middle_metacarpals", "middle_mcp_pitch": "middle_proximal", "middle_pip": "middle_middle", "middle_dip": "middle_distal", "ring_mcp_roll": "ring_metacarpals", "ring_mcp_pitch": "ring_proximal", "ring_pip": "ring_middle", "ring_dip": "ring_distal", "pinky_mcp_roll": "pinky_metacarpals", "pinky_mcp_pitch": "pinky_proximal", "pinky_pip": "pinky_middle", "pinky_dip": "pinky_distal", } def rpy_to_quat(roll, pitch, yaw): cr, sr = np.cos(roll / 2), np.sin(roll / 2) cp, sp = np.cos(pitch / 2), np.sin(pitch / 2) cy, sy = np.cos(yaw / 2), np.sin(yaw / 2) return np.array( [ cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy, ] ) def parse_urdf(path: Path) -> dict[str, dict]: text = path.read_text() joints: dict[str, dict] = {} for m in re.finditer(r"]*)>(.*?)", text, re.S): attrs, body = m.group(1), m.group(2) name_m = re.search(r'name="([^"]+)"', attrs) if not name_m: continue name = name_m.group(1) if "revolute" not in attrs: continue origin = re.search(r']*xyz="([^"]+)"[^>]*rpy="([^"]+)"', body) lim = re.search(r'lower="([^"]+)"[^>]*upper="([^"]+)"', body) # URDF 常写成 ，不能写死 "axis xyz=" axis = re.search(r']*xyz="([^"]+)"', body) xyz = [float(x) for x in origin.group(1).split()] if origin else [0.0, 0.0, 0.0] rpy = [float(x) for x in origin.group(2).split()] if origin else [0.0, 0.0, 0.0] joints[name] = { "lo": float(lim.group(1)), "hi": float(lim.group(2)), "xyz": xyz, "rpy": rpy, "axis": axis.group(1) if axis else "1 0 0", } return joints def apply_urdf_to_mjcf(template: str, joints: dict[str, dict]) -> str: xml = template for jname, info in joints.items(): lo, hi = info["lo"], info["hi"] pat = rf'(]*\srange=")([^"]+)(")' xml = re.sub( pat, lambda m, lo=lo, hi=hi: f"{m.group(1)}{lo} {hi}{m.group(3)}", xml, count=1, ) act_pat = rf'(]*\sctrlrange=")([^"]+)(")' xml = re.sub( act_pat, lambda m, lo=lo, hi=hi: f"{m.group(1)}{lo} {hi}{m.group(3)}", xml, count=1, ) if jname in MCP_ROLL_JOINTS: ax = info["axis"].strip() ax_pat = rf'(]*\saxis=")([^"]+)(")' xml = re.sub(ax_pat, lambda m, ax=ax: f"{m.group(1)}{ax}{m.group(3)}", xml, count=1) # 同步零位 body pos/quat（拇指 CMC 等）；保留原有 visual/collision geom 结构 for jname, bname in BODY_FROM_JOINT.items(): if jname not in joints: continue j = joints[jname] xyz, rpy = j["xyz"], j["rpy"] q = rpy_to_quat(*rpy) pos = f"{xyz[0]:g} {xyz[1]:g} {xyz[2]:g}" m = re.search(rf']*>', xml) if not m: continue if np.allclose(q, [1, 0, 0, 0], atol=1e-9): new = f'' else: quat = "{:.8g} {:.8g} {:.8g} {:.8g}".format(*q) new = f'' xml = xml[: m.start()] + new + xml[m.end() :] return xml joints = parse_urdf(urdf_cal) xml = apply_urdf_to_mjcf(calib_xml.read_text(), joints) calib_xml.write_text(xml) mujoco.MjModel.from_xml_path(str(calib_xml)) for j in MCP_ROLL_JOINTS: ax = re.search(rf']*\saxis="([^"]+)"', calib_xml.read_text()) print(f"[g20-left-dual] synced {j} axis={ax.group(1) if ax else '?'}") for j in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"): info = joints[j] print(f"[g20-left-dual] {j} rpy={info['rpy']} lim=[{info['lo']}, {info['hi']}]") print(f"[g20-left-dual] calibrated URDF: {urdf_cal}") print(f"[g20-left-dual] calibrated MJCF: {calib_xml} (package template)") if meshdir_cal.is_dir(): print(f"[g20-left-dual] calibrated meshes: {meshdir_cal}") PY echo "[g20-left-dual] refresh uncalibrated L20 V10.1 left MJCF from $URDF_UNCALIBRATED" python3 - "$URDF_UNCALIBRATED" "$MESHDIR_UNCAL" "$L20_TEMPLATE" "$STOCK_XML" <<'PY' import re import shutil import sys from pathlib import Path import mujoco import numpy as np urdf_path = Path(sys.argv[1]) meshdir = Path(sys.argv[2]) template_path = Path(sys.argv[3]) out_path = Path(sys.argv[4]) def rpy_to_quat(roll, pitch, yaw): cr, sr = np.cos(roll / 2), np.sin(roll / 2) cp, sp = np.cos(pitch / 2), np.sin(pitch / 2) cy, sy = np.cos(yaw / 2), np.sin(yaw / 2) return np.array([ cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy, ]) def parse_joints(path: Path) -> dict: text = path.read_text() joints = {} for m in re.finditer(r"]*)>(.*?)", text, re.S): attrs, body = m.group(1), m.group(2) name = re.search(r'name="([^"]+)"', attrs).group(1) jtype = re.search(r'type="([^"]+)"', attrs).group(1) if jtype != "revolute": continue origin = re.search(r']*xyz="([^"]+)"[^>]*rpy="([^"]+)"', body) lim = re.search(r'lower="([^"]+)"[^>]*upper="([^"]+)"', body) axis = re.search(r']*xyz="([^"]+)"', body) joints[name] = { "xyz": tuple(map(float, origin.group(1).split())), "rpy": tuple(map(float, origin.group(2).split())), "lo": float(lim.group(1)), "hi": float(lim.group(2)), "axis": tuple(map(float, axis.group(1).split())), } if "thumb_dip" in joints and "thumb_ip" not in joints: joints["thumb_ip"] = dict(joints["thumb_dip"]) return joints def fmt_axis(ax): return " ".join(f"{v:g}" for v in ax) def fmt_range(lo, hi): return f"{lo:g} {hi:g}" out_path.parent.mkdir(parents=True, exist_ok=True) mesh_out = out_path.parent / "meshes" mesh_out.mkdir(exist_ok=True) for f in meshdir.glob("*"): shutil.copy2(f, mesh_out / f.name) shutil.copy2(urdf_path, out_path.parent / urdf_path.name) # 先读模板全文（out 可能与 template 同路径） xml = template_path.read_text() l20 = parse_joints(urdf_path) joint_specs = {n: (j["lo"], j["hi"], j["axis"]) for n, j in l20.items() if n != "thumb_dip"} for name, (lo, hi, axis) in joint_specs.items(): pat = re.compile(rf']*>') m = pat.search(xml) if not m: raise SystemExit(f"missing joint {name}") new = ( f'' ) xml = xml[: m.start()] + new + xml[m.end() :] for name, (lo, hi, _) in joint_specs.items(): pat = re.compile( rf'(]*\bctrlrange=")[^"]+(")' ) if not pat.search(xml): raise SystemExit(f"missing actuator {name}_pos") xml = pat.sub(rf"\g<1>{lo:g} {hi:g}\2", xml, count=1) body_from_joint = { "thumb_cmc_roll": "thumb_metacarpals_base1", "thumb_cmc_yaw": "thumb_metacarpals_base2", "thumb_cmc_pitch": "thumb_metacarpals", "thumb_mcp": "thumb_proximal", "thumb_ip": "thumb_distal", "index_mcp_roll": "index_metacarpals", "index_mcp_pitch": "index_proximal", "index_pip": "index_middle", "index_dip": "index_distal", "middle_mcp_roll": "middle_metacarpals", "middle_mcp_pitch": "middle_proximal", "middle_pip": "middle_middle", "middle_dip": "middle_distal", "ring_mcp_roll": "ring_metacarpals", "ring_mcp_pitch": "ring_proximal", "ring_pip": "ring_middle", "ring_dip": "ring_distal", "pinky_mcp_roll": "pinky_metacarpals", "pinky_mcp_pitch": "pinky_proximal", "pinky_pip": "pinky_middle", "pinky_dip": "pinky_distal", } for jname, bname in body_from_joint.items(): j = l20[jname] xyz = j["xyz"] rpy = j["rpy"] q = rpy_to_quat(*rpy) pos = f"{xyz[0]:g} {xyz[1]:g} {xyz[2]:g}" pat = re.compile(rf']*>') m = pat.search(xml) if not m: raise SystemExit(f"missing body {bname}") if np.allclose(q, [1, 0, 0, 0], atol=1e-9): new = f'' else: quat = "{:.6g} {:.6g} {:.6g} {:.6g}".format(*q) new = f'' xml = xml[: m.start()] + new + xml[m.end() :] out_path.write_text(xml) mujoco.MjModel.from_xml_path(str(out_path)) print(f"[g20-left-dual] uncalibrated URDF: {urdf_path}") print(f"[g20-left-dual] uncalibrated meshes: {meshdir} -> {mesh_out}") print(f"[g20-left-dual] uncalibrated MJCF -> {out_path} (L20v101 left template + visual/collision split)") PY EXTRA=("$@") cleanup() { [[ -n "${PID_G20:-}" ]] && kill "$PID_G20" 2>/dev/null || true [[ -n "${PID_L20:-}" ]] && kill "$PID_L20" 2>/dev/null || true } trap cleanup EXIT INT TERM echo "[g20-left-dual] G20 calibrated xml=$CALIB_XML" "$ROOT/hands/G20/scripts/run_g20_mujoco_plan.sh" \ --xml "$CALIB_XML" \ --mapping-urdf "$URDF_CALIBRATED" \ --node-name g20_calibrated_left \ --window-title "G20 left 标定 (linkerhand_g20_left_tag)" \ --cmd-topic /cb_left_hand_control_cmd \ --sim-state-topic /sim/mujoco/g20/left/calibrated/joint_state \ --mapping g20_calibrated \ "${EXTRA[@]}" & PID_G20=$! sleep 1 echo "[g20-left-dual] L20 stock xml=$STOCK_XML (L20v101 left, URDF-native mapping)" "$ROOT/hands/G20/scripts/run_g20_mujoco_plan.sh" \ --xml "$STOCK_XML" \ --mapping-urdf "$URDF_UNCALIBRATED" \ --node-name l20_stock_left \ --window-title "L20 V10.1 left 未标定" \ --cmd-topic /cb_left_hand_control_cmd \ --sim-state-topic /sim/mujoco/l20/left/stock/joint_state \ --mapping l20_urdf \ "${EXTRA[@]}" & PID_L20=$! echo "[g20-left-dual] both started. cmd topic: /cb_left_hand_control_cmd" echo "[g20-left-dual] mode: kinematic（运动学硬写；稳定跟 0~255）" echo "[g20-left-dual] 标定 mapping=g20_calibrated（主动线性，仅被动 JSON）；未标定 mapping=l20_urdf + collision_project=on" echo "[g20-left-dual] 需要 PD+自碰: ./hands/G20/scripts/run_g20_left_dual_compare.sh --pd-drive" echo "[g20-left-dual] Ctrl+C stops both." wait "$PID_G20" "$PID_L20"