sunxianghui/mujoco_linkerbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Organize hand models and launchers; use linear active joint mapping

Browse Source
This commit is contained in:
sunxianghui
2026-09-28 11:05:30 +08:00
parent 325dff6870
commit 01c9aa5c7a
712 changed files with 205077 additions and 6209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hands/G20/scripts/run_g20_joint_gui.sh
Executable
+25
View File
@@ -0,0 +1,25 @@
#!/usr/bin/env bash
# G20 关节弧度监视 GUI（默认 6 位小数）
# 订阅 /sim/mujoco/g20/left/joint_state
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"
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
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"
exec python3 -m linker_hand_mujoco_ros2.g20_joint_monitor_gui "$@"
hands/G20/scripts/run_g20_left_desktop_urdf.sh
Executable
+57
View File
@@ -0,0 +1,57 @@
#!/usr/bin/env bash
# MuJoCo G20 左手 — 桌面 URDF 验证
# URDF: /home/lxqs/桌面/L20/LEFT/linkerhand_g20_left.urdf（已同步到包内）
# 默认 mapping=g20_urdf（四指侧摆无外部取负，跟 URDF 轴正向）
#
# 用法:
# ./hands/G20/scripts/run_g20_left_desktop_urdf.sh
# ./hands/G20/scripts/run_g20_left_desktop_urdf.sh --pd-drive
# 另开终端扫侧摆方向:
# ./hands/G20/scripts/run_g20_left_mcp_roll_check.sh
# 监视关节弧度:
# ./hands/G20/scripts/run_g20_joint_gui.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"
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
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"
XML="$PKG_SRC/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linker_hand_g20_left_desktop.xml"
URDF="$PKG_SRC/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linkerhand_g20_left_desktop.urdf"
if [[ ! -f "$XML" ]]; then
echo "[error] missing MJCF: $XML" >&2
exit 1
fi
echo "[g20-left-desktop] urdf=$URDF"
echo "[g20-left-desktop] xml=$XML"
echo "[g20-left-desktop] cmd-topic=/cb_left_hand_control_cmd"
echo "[g20-left-desktop] sim-state=/sim/mujoco/g20/left/joint_state"
echo "[g20-left-desktop] mapping=g20_urdf (侧摆 cmd0→负限位, cmd255→正限位, 无外部取负)"
echo "[g20-left-desktop] URDF 四指 mcp_roll 轴: +X (未翻转；右手已改为 -X)"
echo "[g20-left-desktop] 侧摆验证: 另开终端运行 ./hands/G20/scripts/run_g20_left_mcp_roll_check.sh"
exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge \
--xml "$XML" \
--node-name g20_mujoco_plan_bridge_left_desktop \
--window-title "G20 left (desktop URDF)" \
--cmd-topic /cb_left_hand_control_cmd \
--sim-state-topic /sim/mujoco/g20/left/joint_state \
--nominal-topic /retarget/g20/left/joint_target_nominal \
--mapping g20_urdf \
"$@"
hands/G20/scripts/run_g20_left_dual_compare.sh
Executable
+404
View File
@@ -0,0 +1,404 @@
#!/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"<joint\b([^>]*)>(.*?)</joint>", 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'<origin[^>]*xyz="([^"]+)"[^>]*rpy="([^"]+)"', body)
lim = re.search(r'lower="([^"]+)"[^>]*upper="([^"]+)"', body)
# URDF 常写成 <axis\n xyz="..."/>，不能写死 "axis xyz="
axis = re.search(r'<axis[^>]*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'(<joint name="{re.escape(jname)}"[^>]*\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'(<position name="{re.escape(jname)}_pos"[^>]*\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'(<joint name="{re.escape(jname)}"[^>]*\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'<body name="{re.escape(bname)}"[^>]*>', xml)
if not m:
continue
if np.allclose(q, [1, 0, 0, 0], atol=1e-9):
new = f'<body name="{bname}" pos="{pos}">'
else:
quat = "{:.8g} {:.8g} {:.8g} {:.8g}".format(*q)
new = f'<body name="{bname}" pos="{pos}" quat="{quat}">'
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'<joint name="{j}"[^>]*\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"<joint\b([^>]*)>(.*?)</joint>", 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'<origin[^>]*xyz="([^"]+)"[^>]*rpy="([^"]+)"', body)
lim = re.search(r'lower="([^"]+)"[^>]*upper="([^"]+)"', body)
axis = re.search(r'<axis[^>]*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'<joint name="{re.escape(name)}"[^>]*>')
m = pat.search(xml)
if not m:
raise SystemExit(f"missing joint {name}")
new = (
f'<joint name="{name}" pos="0 0 0" axis="{fmt_axis(axis)}" '
f'range="{fmt_range(lo, hi)}" actuatorfrcrange="-100 100"/>'
)
xml = xml[: m.start()] + new + xml[m.end() :]
for name, (lo, hi, _) in joint_specs.items():
pat = re.compile(
rf'(<position name="{re.escape(name)}_pos"[^>]*\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'<body name="{re.escape(bname)}"[^>]*>')
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'<body name="{bname}" pos="{pos}">'
else:
quat = "{:.6g} {:.6g} {:.6g} {:.6g}".format(*q)
new = f'<body name="{bname}" pos="{pos}" quat="{quat}">'
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"
hands/G20/scripts/run_g20_left_mcp_roll_check.sh
Executable
+35
View File
@@ -0,0 +1,35 @@
#!/usr/bin/env bash
# 左手四指侧摆方向检查：依次扫 SDK 通道 6~9（index/middle/ring/pinky mcp_roll）
# 需先运行: ./hands/G20/scripts/run_g20_left_desktop_urdf.sh
#
# 预期（g20_urdf, URDF 轴 +X）:
# cmd=0 → 关节角为负（约 -0.23 rad，URDF lower）
# cmd=255 → 关节角为正（约 +0.23 rad，URDF upper）
# 若视觉与真机相反，说明 URDF 轴方向或映射仍不一致。
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"
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
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"
CMD_TOPIC="${CMD_TOPIC:-/cb_left_hand_control_cmd}"
STATE_TOPIC="${STATE_TOPIC:-/sim/mujoco/g20/left/joint_state}"
exec python3 -m linker_hand_mujoco_ros2.g20_left_mcp_roll_check \
--cmd-topic "$CMD_TOPIC" \
--state-topic "$STATE_TOPIC" \
"$@"
hands/G20/scripts/run_g20_mujoco_plan.sh
Executable
+28
View File
@@ -0,0 +1,28 @@
#!/usr/bin/env bash
# MuJoCo G20 左手仿真桥
# 默认订阅 /cb_left_hand_control_cmd（0~255）
# 发布 /sim/mujoco/g20/left/joint_state
# 默认 1.35× 墙钟 + 关节限速；可用 --speed / --slew-rad-s 再调
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"
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
# 源码优先（含 urdf 资产）
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"
exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge "$@"
hands/G20/scripts/run_g20_right_dual_compare.sh
Executable
+337
View File
@@ -0,0 +1,337 @@
#!/usr/bin/env bash
# 同时开两个 MuJoCo：G20 右手标定 vs L20 原版 URDF（未标定）
#
# 标定: 包内 linker_hand_g20_right.xml（启动前从 linkerhand_G20_RIGHT_tag.urdf 仅 patch 限位/侧摆轴；保留 _col 碰撞体与 body quat）
# 未标定: /home/lxqs/linkerhand_L20_V10 (2).1--0728/linkerhand_L20_V10.1_right.urdf → 运行时生成 *_stock.xml
#
# 默认运动学硬写（与 run_dual_mujoco_compare.sh / O6·L6 双开一致）：稳定、跟 0~255 真机命令。
# 需要 PD+自碰时手动加 --pd-drive（两窗均加）。
#
# 两个窗口共用 /cb_right_hand_control_cmd（20 维 0~255）。
#
# 用法:
# ./hands/G20/scripts/run_g20_right_dual_compare.sh
# ./hands/G20/scripts/run_g20_right_dual_compare.sh --cmd-topic /cb_right_hand_control_cmd --speed 1.0
# ./hands/G20/scripts/run_g20_right_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_right"
URDF_CALIBRATED="${G20_RIGHT_URDF:-$ROOT/hands/G20/reference/right_calibrated.urdf}"
MESHDIR_CALIB="$G20_DIR/meshes"
URDF_UNCALIBRATED="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_right/linkerhand_L20v10.1_right.urdf"
MESHDIR_UNCAL="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_right/meshes"
CALIB_XML="$G20_DIR/linker_hand_g20_right.xml"
CALIB_URDF_PKG="$G20_DIR/linkerhand_g20_right.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_right/linker_hand_l20v101_right.xml"
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-right-dual] sync calibrated MJCF from G20_RIGHT_tag 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
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])
# 包内留一份 Linkerbot URDF 副本，便于对照
shutil.copy2(urdf_cal, calib_urdf_pkg)
# 只同步限位/轴；不要改 body quat（会破坏 MJCF 运动链，_col 与 visual 错位→对指穿模）
MCP_ROLL_JOINTS = (
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
)
def parse_urdf(path: Path) -> dict[str, dict]:
text = path.read_text()
joints: dict[str, dict] = {}
for m in re.finditer(r"<joint\b([^>]*)>(.*?)</joint>", 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'<origin[^>]*xyz="([^"]+)"[^>]*rpy="([^"]+)"', body)
lim = re.search(r'lower="([^"]+)"[^>]*upper="([^"]+)"', body)
# URDF 常写成 <axis\n xyz="..."/>，不能写死 "axis xyz="
axis = re.search(r'<axis[^>]*xyz="([^"]+)"', body)
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)),
"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'(<joint name="{re.escape(jname)}"[^>]*\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'(<position name="{re.escape(jname)}_pos"[^>]*\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'(<joint name="{re.escape(jname)}"[^>]*\saxis=")([^"]+)(")'
xml = re.sub(ax_pat, lambda m, ax=ax: f"{m.group(1)}{ax}{m.group(3)}", xml, count=1)
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'<joint name="{j}"[^>]*\saxis="([^"]+)"', calib_xml.read_text())
print(f"[g20-right-dual] synced {j} axis={ax.group(1) if ax else '?'}")
print(f"[g20-right-dual] calibrated URDF: {urdf_cal}")
print(f"[g20-right-dual] calibrated MJCF: {calib_xml} (package template + _col)")
if meshdir_cal.is_dir():
print(f"[g20-right-dual] calibrated meshes: {meshdir_cal} (package _col meshes)")
PY
echo "[g20-right-dual] build uncalibrated L20 V10.1 right MJCF from template + $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"<joint\b([^>]*)>(.*?)</joint>", 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'<origin[^>]*xyz="([^"]+)"[^>]*rpy="([^"]+)"', body)
lim = re.search(r'lower="([^"]+)"[^>]*upper="([^"]+)"', body)
axis = re.search(r'<axis[^>]*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)
l20 = parse_joints(urdf_path)
xml = template_path.read_text()
xml = xml.replace('model="linker_hand_l20v101_left"', 'model="linker_hand_l20v101_right"')
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'<joint name="{re.escape(name)}"[^>]*>')
m = pat.search(xml)
if not m:
raise SystemExit(f"missing joint {name}")
new = (
f'<joint name="{name}" pos="0 0 0" axis="{fmt_axis(axis)}" '
f'range="{fmt_range(lo, hi)}" actuatorfrcrange="-100 100"/>'
)
xml = xml[: m.start()] + new + xml[m.end() :]
for name, (lo, hi, _) in joint_specs.items():
pat = re.compile(
rf'(<position name="{re.escape(name)}_pos"[^>]*\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'<body name="{re.escape(bname)}"[^>]*>')
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'<body name="{bname}" pos="{pos}">'
else:
quat = "{:.6g} {:.6g} {:.6g} {:.6g}".format(*q)
new = f'<body name="{bname}" pos="{pos}" quat="{quat}">'
xml = xml[: m.start()] + new + xml[m.end() :]
out_path.write_text(xml)
mujoco.MjModel.from_xml_path(str(out_path))
print(f"[g20-right-dual] uncalibrated URDF: {urdf_path}")
print(f"[g20-right-dual] uncalibrated meshes: {meshdir} -> {mesh_out}")
print(f"[g20-right-dual] uncalibrated MJCF -> {out_path} (L20v101 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-right-dual] G20 calibrated xml=$CALIB_XML"
"$ROOT/hands/G20/scripts/run_g20_right_mujoco_plan.sh" \
--xml "$CALIB_XML" \
--mapping-urdf "$URDF_CALIBRATED" \
--node-name g20_calibrated_right \
--window-title "G20 right 标定 (G20_RIGHT_tag)" \
--cmd-topic /cb_right_hand_control_cmd \
--sim-state-topic /sim/mujoco/g20/right/calibrated/joint_state \
--mapping g20_urdf \
"${EXTRA[@]}" &
PID_G20=$!
sleep 1
echo "[g20-right-dual] L20 stock xml=$STOCK_XML (L20v101 template, URDF-native mapping)"
"$ROOT/hands/G20/scripts/run_g20_right_mujoco_plan.sh" \
--xml "$STOCK_XML" \
--mapping-urdf "$URDF_UNCALIBRATED" \
--node-name l20_stock_right \
--window-title "L20 V10.1 right 未标定" \
--cmd-topic /cb_right_hand_control_cmd \
--sim-state-topic /sim/mujoco/l20/right/stock/joint_state \
--mapping l20_urdf \
"${EXTRA[@]}" &
PID_L20=$!
echo "[g20-right-dual] both started. cmd topic: /cb_right_hand_control_cmd"
echo "[g20-right-dual] mode: kinematic（运动学硬写，与左手双开一致；稳定跟 0~255）"
echo "[g20-right-dual] 标定 mapping=g20_urdf（仅按URDF，不查JSON）；未标定 mapping=l20_urdf + collision_project=on"
echo "[g20-right-dual] 需要 PD+自碰: ./hands/G20/scripts/run_g20_right_dual_compare.sh --pd-drive"
echo "[g20-right-dual] Ctrl+C stops both."
wait "$PID_G20" "$PID_L20"
hands/G20/scripts/run_g20_right_joint_gui.sh
Executable
+27
View File
@@ -0,0 +1,27 @@
#!/usr/bin/env bash
# G20 右手关节弧度监视 GUI（默认 6 位小数）
# 订阅 /sim/mujoco/g20/right/joint_state
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"
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
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"
exec python3 -m linker_hand_mujoco_ros2.g20_joint_monitor_gui \
--topic /sim/mujoco/g20/right/joint_state \
"$@"
hands/G20/scripts/run_g20_right_mujoco_plan.sh
Executable
+54
View File
@@ -0,0 +1,54 @@
#!/usr/bin/env bash
# MuJoCo G20 右手仿真桥
# 默认订阅 /cb_right_hand_control_cmd（0~255，与真机 SDK 一致）
# 发布 /sim/mujoco/g20/right/joint_state
# 模型：urdf/G20/linker_hand_g20_right/linker_hand_g20_right.xml
# 默认 1.35× 墙钟 + 关节限速；可用 --speed / --slew-rad-s 再调
#
# 用法:
# ./hands/G20/scripts/run_g20_right_mujoco_plan.sh
# ./hands/G20/scripts/run_g20_right_mujoco_plan.sh --cmd-topic /g20/cb_right_hand_control_cmd
# 监视关节（另开终端）:
# ./hands/G20/scripts/run_g20_right_joint_gui.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"
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
# 源码优先（含 urdf 资产）
PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"
XML="$PKG_SRC/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_right/linker_hand_g20_right.xml"
if [[ ! -f "$XML" ]]; then
echo "[error] missing G20 right MJCF: $XML" >&2
exit 1
fi
echo "[g20-right] xml=$XML"
echo "[g20-right] model=G20_RIGHT_tag"
echo "[g20-right] cmd-topic=/cb_right_hand_control_cmd (override with --cmd-topic)"
echo "[g20-right] sim-state=/sim/mujoco/g20/right/joint_state"
exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge \
--xml "$XML" \
--node-name g20_mujoco_plan_bridge_right \
--window-title "G20 right" \
--cmd-topic /cb_right_hand_control_cmd \
--sim-state-topic /sim/mujoco/g20/right/joint_state \
--nominal-topic /retarget/g20/right/joint_target_nominal \
--mapping g20_urdf \
"$@"