Add project sources for Mujoco LinkerHand ROS2

Co-authored-by: Cursor <cursoragent@cursor.com>

tools/friction_id/sweep_index_plot.py

@@ -0,0 +1,239 @@
+#!/usr/bin/env python3
+"""O6 单指位置控制：从最小扫到最大，记录并绘制 角度/速度/力矩。"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib
+
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import mujoco
+import mujoco.viewer
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[2]
+DEFAULT_XML = (
+    ROOT
+    / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+    / "urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml"
+)
+
+# 四指：根节 mcp_pitch + 远端 dip（mimic 0.89）
+# ROS 6 维索引：0拇指弯 1拇指横摆 2食指 3中指 4无名指 5小指
+FINGER_CFG = {
+    "index": {
+        "mcp": "index_mcp_pitch",
+        "dip": "index_dip",
+        "mcp_act": "index_mcp_pitch_pos",
+        "dip_act": "index_dip_pos",
+        "mimic": 0.89,
+        "ros_idx": 2,
+    },
+    "middle": {
+        "mcp": "middle_mcp_pitch",
+        "dip": "middle_dip",
+        "mcp_act": "middle_mcp_pitch_pos",
+        "dip_act": "middle_dip_pos",
+        "mimic": 0.89,
+        "ros_idx": 3,
+    },
+    "ring": {
+        "mcp": "ring_mcp_pitch",
+        "dip": "ring_dip",
+        "mcp_act": "ring_mcp_pitch_pos",
+        "dip_act": "ring_dip_pos",
+        "mimic": 0.89,
+        "ros_idx": 4,
+    },
+    "pinky": {
+        "mcp": "pinky_mcp_pitch",
+        "dip": "pinky_dip",
+        "mcp_act": "pinky_mcp_pitch_pos",
+        "dip_act": "pinky_dip_pos",
+        "mimic": 0.89,
+        "ros_idx": 5,
+    },
+}
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--xml", type=Path, default=DEFAULT_XML)
+    parser.add_argument("--hand", choices=["left", "right"], default="left")
+    parser.add_argument(
+        "--finger",
+        choices=list(FINGER_CFG.keys()),
+        default="middle",
+        help="要扫的手指（默认 middle）",
+    )
+    parser.add_argument("--duration", type=float, default=8.0, help="单向扫过时长 (s)")
+    parser.add_argument("--out", type=Path, default=None)
+    parser.add_argument("--viewer", action="store_true", help="打开 MuJoCo viewer")
+    args = parser.parse_args()
+
+    cfg = FINGER_CFG[args.finger]
+    if args.out is None:
+        args.out = ROOT / f"reports/O6_{args.finger}_sweep"
+
+    if args.hand == "right":
+        args.xml = (
+            ROOT
+            / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+            / "urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml"
+        )
+
+    args.out.mkdir(parents=True, exist_ok=True)
+    model = mujoco.MjModel.from_xml_path(str(args.xml))
+    data = mujoco.MjData(model)
+
+    mcp_jnt = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, cfg["mcp"])
+    dip_jnt = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, cfg["dip"])
+    mcp_act = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, cfg["mcp_act"])
+    dip_act = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, cfg["dip_act"])
+    if min(mcp_jnt, dip_jnt, mcp_act, dip_act) < 0:
+        raise RuntimeError(f"找不到 {args.finger} joint/actuator，请检查 MJCF")
+
+    mcp_qadr = int(model.jnt_qposadr[mcp_jnt])
+    mcp_dadr = int(model.jnt_dofadr[mcp_jnt])
+    mcp_lo, mcp_hi = map(float, model.jnt_range[mcp_jnt])
+    dip_lo, dip_hi = map(float, model.jnt_range[dip_jnt])
+    mimic = float(cfg["mimic"])
+    dt = float(model.opt.timestep)
+
+    half = args.duration
+    total_t = 2.0 * half
+    n_steps = int(total_t / dt)
+    t_arr = np.arange(n_steps) * dt
+
+    def cmd_at(t: float) -> float:
+        if t <= half:
+            return mcp_lo + (mcp_hi - mcp_lo) * (t / half)
+        u = (t - half) / half
+        return mcp_hi + (mcp_lo - mcp_hi) * u
+
+    data.ctrl[:] = 0.0
+    mujoco.mj_forward(model, data)
+
+    logs = {
+        "t": [],
+        "cmd_mcp": [],
+        "q_mcp": [],
+        "v_mcp": [],
+        "tau_mcp": [],
+        "q_dip": [],
+        "tau_dip": [],
+    }
+
+    viewer = None
+    if args.viewer:
+        viewer = mujoco.viewer.launch_passive(model, data)
+
+    for i in range(n_steps):
+        t = t_arr[i]
+        q_cmd = cmd_at(t)
+        dip_cmd = float(np.clip(q_cmd * mimic, dip_lo, dip_hi))
+        data.ctrl[mcp_act] = q_cmd
+        data.ctrl[dip_act] = dip_cmd
+        mujoco.mj_step(model, data)
+
+        logs["t"].append(t)
+        logs["cmd_mcp"].append(q_cmd)
+        logs["q_mcp"].append(float(data.qpos[mcp_qadr]))
+        logs["v_mcp"].append(float(data.qvel[mcp_dadr]))
+        logs["tau_mcp"].append(float(data.actuator_force[mcp_act]))
+        logs["q_dip"].append(float(data.qpos[int(model.jnt_qposadr[dip_jnt])]))
+        logs["tau_dip"].append(float(data.actuator_force[dip_act]))
+
+        if viewer is not None:
+            viewer.sync()
+
+    if viewer is not None:
+        viewer.close()
+
+    for k in logs:
+        logs[k] = np.asarray(logs[k], dtype=float)
+
+    csv_path = args.out / f"{args.finger}_sweep_{args.hand}.csv"
+    header = "t_s,cmd_mcp_rad,q_mcp_rad,v_mcp_rad_s,tau_mcp_Nm,q_dip_rad,tau_dip_Nm"
+    np.savetxt(
+        csv_path,
+        np.column_stack(
+            [
+                logs["t"],
+                logs["cmd_mcp"],
+                logs["q_mcp"],
+                logs["v_mcp"],
+                logs["tau_mcp"],
+                logs["q_dip"],
+                logs["tau_dip"],
+            ]
+        ),
+        delimiter=",",
+        header=header,
+        comments="",
+    )
+
+    fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+    fig.suptitle(
+        f"O6 {args.hand} {cfg['mcp']} position sweep  "
+        f"range=[{mcp_lo:.2f}, {mcp_hi:.2f}] rad",
+        fontsize=13,
+    )
+
+    axes[0].plot(logs["t"], np.rad2deg(logs["cmd_mcp"]), "k--", lw=1.2, label="cmd")
+    axes[0].plot(logs["t"], np.rad2deg(logs["q_mcp"]), "C0", lw=1.8, label="q")
+    axes[0].set_ylabel("angle (deg)")
+    axes[0].legend(loc="best")
+    axes[0].grid(True, alpha=0.3)
+
+    axes[1].plot(logs["t"], np.rad2deg(logs["v_mcp"]), "C1", lw=1.8, label="v")
+    axes[1].set_ylabel("velocity (deg/s)")
+    axes[1].legend(loc="best")
+    axes[1].grid(True, alpha=0.3)
+
+    axes[2].plot(logs["t"], logs["tau_mcp"], "C3", lw=1.8, label="tau (actuator_force)")
+    axes[2].set_ylabel("torque (N·m)")
+    axes[2].set_xlabel("time (s)")
+    axes[2].legend(loc="best")
+    axes[2].grid(True, alpha=0.3)
+
+    fig.tight_layout()
+    png_path = args.out / f"{args.finger}_sweep_{args.hand}_qvt.png"
+    fig.savefig(png_path, dpi=140)
+    plt.close(fig)
+
+    # ROS 示例：只动该指
+    pos = [255.0] * 6
+    pos_open = pos.copy()
+    pos_close = pos.copy()
+    pos_close[cfg["ros_idx"]] = 0.0
+
+    print("=" * 60)
+    print(f"finger: {args.finger}  joint: {cfg['mcp']}")
+    print(f"model:  {args.xml}")
+    print(
+        f"range:  [{mcp_lo:.3f}, {mcp_hi:.3f}] rad "
+        f"= [{np.rad2deg(mcp_lo):.1f}, {np.rad2deg(mcp_hi):.1f}] deg"
+    )
+    print(f"mimic:  dip = mcp * {mimic}")
+    print(f"sweep:  {mcp_lo:.3f} -> {mcp_hi:.3f} -> {mcp_lo:.3f}  total {total_t:.1f}s")
+    print(f"CSV:    {csv_path}")
+    print(f"plot:   {png_path}")
+    print(
+        "peak |q|={:.3f} rad  |v|={:.3f} rad/s  |tau|={:.3f} N·m".format(
+            np.max(np.abs(logs["q_mcp"])),
+            np.max(np.abs(logs["v_mcp"])),
+            np.max(np.abs(logs["tau_mcp"])),
+        )
+    )
+    print("=" * 60)
+    print(f"ROS 只弯{args.finger}（索引 {cfg['ros_idx']}）握紧示例 position:")
+    print(f"  {pos_close}")
+    print(f"张开示例: {pos_open}")
+
+
+if __name__ == "__main__":
+    main()

tools/hand_curve_recorder_standalone.py

@@ -0,0 +1,241 @@
+#!/usr/bin/env python3
+"""手部曲线录制 + 画图（单文件，仿真/真机通用）。
+
+依赖（两边都要）：
+  - 已 source ROS2（jazzy）
+  - pip: numpy matplotlib
+  - ros: rclpy sensor_msgs std_msgs
+
+用法示例：
+  python3 hand_curve_recorder_standalone.py --hand left --channel 3 --label sim
+  # 对端发 /cb_left_hand_control_cmd
+  # Ctrl+C → 当前目录下 reports/hand_curves/ 生成 csv + png
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import os
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+
+CHANNEL_NAMES = [
+    "thumb_bend",
+    "thumb_yaw",
+    "index",
+    "middle",
+    "ring",
+    "pinky",
+]
+
+
+def _pad(seq, n, fill=float("nan")):
+    out = [fill] * n
+    for i, v in enumerate(list(seq)[:n]):
+        out[i] = float(v)
+    return out
+
+
+def plot_curves(rows, ch, ch_name, label, have_state, have_current, png_path):
+    import matplotlib
+
+    matplotlib.use("Agg")
+    import matplotlib.pyplot as plt
+
+    t = np.array([r["t_s"] for r in rows], dtype=float)
+    cmd = np.array([r["cmd"][ch] for r in rows], dtype=float)
+    joint = np.array([r["joint"][ch] for r in rows], dtype=float)
+    current = np.array([r["current"][ch] for r in rows], dtype=float)
+
+    y = joint.copy()
+    if np.all(np.isnan(y)):
+        y = cmd.copy()
+    v = np.gradient(y, t)
+    if len(v) >= 5:
+        v = np.convolve(v, np.ones(5) / 5.0, mode="same")
+
+    fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+    src = "joint" if have_state else "cmd(as joint)"
+    fig.suptitle(
+        f"recorder  ch={ch}:{ch_name}  label={label}  src={src}",
+        fontsize=12,
+    )
+
+    axes[0].plot(t, cmd, "k--", lw=1.2, label="command_u8")
+    axes[0].plot(t, joint, "C0", lw=1.6, label="joint_u8")
+    axes[0].set_ylabel("position (0-255)")
+    axes[0].legend(loc="best")
+    axes[0].grid(True, alpha=0.3)
+
+    axes[1].plot(t, v, "C1", lw=1.6, label="d(joint)/dt")
+    axes[1].set_ylabel("velocity (u8/s)")
+    axes[1].legend(loc="best")
+    axes[1].grid(True, alpha=0.3)
+
+    if have_current:
+        axes[2].plot(t, current, "C3", lw=1.6, label="current/effort")
+    else:
+        axes[2].text(
+            0.5,
+            0.5,
+            "no current / state.effort",
+            ha="center",
+            va="center",
+            transform=axes[2].transAxes,
+        )
+    axes[2].set_ylabel("current / effort")
+    axes[2].set_xlabel("time (s)")
+    axes[2].legend(loc="best")
+    axes[2].grid(True, alpha=0.3)
+
+    fig.tight_layout()
+    fig.savefig(png_path, dpi=140)
+    plt.close(fig)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Hand curve recorder (ROS2, single file)")
+    parser.add_argument("--hand", default="left", choices=["left", "right"])
+    parser.add_argument("--channel", type=int, default=3, help="0拇指弯..3中指..5小指")
+    parser.add_argument("--label", default="run", help="sim / real / ...")
+    parser.add_argument("--hz", type=float, default=100.0)
+    parser.add_argument("--n", type=int, default=6)
+    parser.add_argument("--out", default="reports/hand_curves")
+    parser.add_argument("--cmd-topic", default="", help="空则 /cb_{hand}_hand_control_cmd")
+    parser.add_argument("--state-topic", default="", help="空则 /cb_{hand}_hand_state")
+    parser.add_argument(
+        "--current-topic",
+        default="",
+        help="可选 Float32MultiArray；空则用 state.effort",
+    )
+    parser.add_argument(
+        "--no-cmd-as-joint",
+        action="store_true",
+        help="没有 state 时不要用 cmd 代替 joint",
+    )
+    args = parser.parse_args()
+
+    os.environ.setdefault("MPLCONFIGDIR", str(Path.cwd() / ".mplconfig"))
+    Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import JointState
+    from std_msgs.msg import Float32MultiArray
+
+    cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd"
+    state_topic = args.state_topic or f"/cb_{args.hand}_hand_state"
+    out_dir = Path(args.out).expanduser()
+    if not out_dir.is_absolute():
+        out_dir = Path.cwd() / out_dir
+    out_dir.mkdir(parents=True, exist_ok=True)
+
+    n = args.n
+    ch = args.channel
+    use_cmd_as_joint = not args.no_cmd_as_joint
+
+    class Recorder(Node):
+        def __init__(self):
+            super().__init__("hand_curve_recorder_py")
+            self.cmd = [float("nan")] * n
+            self.joint = [float("nan")] * n
+            self.current = [float("nan")] * n
+            self.have_state = False
+            self.have_current = False
+            self.rows = []
+            self.t0 = time.perf_counter()
+
+            self.create_subscription(JointState, cmd_topic, self.on_cmd, 50)
+            self.create_subscription(JointState, state_topic, self.on_state, 50)
+            if args.current_topic:
+                self.create_subscription(
+                    Float32MultiArray, args.current_topic, self.on_current, 50
+                )
+            self.create_timer(1.0 / max(1.0, args.hz), self.on_timer)
+            self.get_logger().info(
+                f"recording | cmd={cmd_topic} | state={state_topic} | "
+                f"current={args.current_topic or 'state.effort'} | "
+                f"ch={ch} | out={out_dir}"
+            )
+            self.get_logger().info("Ctrl+C to stop and plot")
+
+        def on_cmd(self, msg: JointState):
+            self.cmd = _pad(msg.position, n)
+
+        def on_state(self, msg: JointState):
+            self.have_state = True
+            self.joint = _pad(msg.position, n)
+            if msg.effort:
+                self.have_current = True
+                self.current = _pad(msg.effort, n)
+
+        def on_current(self, msg: Float32MultiArray):
+            self.have_current = True
+            self.current = _pad(msg.data, n)
+
+        def on_timer(self):
+            joint = list(self.joint)
+            if (not self.have_state) and use_cmd_as_joint:
+                joint = list(self.cmd)
+            self.rows.append(
+                {
+                    "t_s": time.perf_counter() - self.t0,
+                    "cmd": list(self.cmd),
+                    "joint": joint,
+                    "current": list(self.current),
+                }
+            )
+
+    rclpy.init()
+    node = Recorder()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
+    rows = node.rows
+    have_state = node.have_state
+    have_current = node.have_current
+    node.destroy_node()
+    if rclpy.ok():
+        rclpy.shutdown()
+
+    if not rows:
+        print("no samples recorded")
+        return
+
+    ch_name = CHANNEL_NAMES[ch] if 0 <= ch < len(CHANNEL_NAMES) else f"ch{ch}"
+    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+    stem = f"{args.label}_{ch_name}_{stamp}"
+    csv_path = out_dir / f"{stem}.csv"
+    png_path = out_dir / f"{stem}_qvt.png"
+
+    with csv_path.open("w", newline="") as f:
+        w = csv.writer(f)
+        header = ["t_s"]
+        for i in range(n):
+            name = CHANNEL_NAMES[i] if i < len(CHANNEL_NAMES) else f"ch{i}"
+            header += [f"cmd_{name}", f"joint_{name}", f"current_{name}"]
+        w.writerow(header)
+        for row in rows:
+            line = [f"{row['t_s']:.6f}"]
+            for i in range(n):
+                line += [
+                    f"{row['cmd'][i]:.6f}",
+                    f"{row['joint'][i]:.6f}",
+                    f"{row['current'][i]:.6f}",
+                ]
+            w.writerow(line)
+
+    plot_curves(rows, ch, ch_name, args.label, have_state, have_current, png_path)
+    print(f"CSV  -> {csv_path}  ({len(rows)} samples)")
+    print(f"plot -> {png_path}")
+    print(f"state={'yes' if have_state else 'no'}  current={'yes' if have_current else 'no'}")
+
+
+if __name__ == "__main__":
+    main()

tools/middle_step_cmd_pub.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+"""O6 中指 step 控制指令（真机 / 仿真共用，时序完全一致）。
+
+时序（默认）：
+  pre_wait=3s  保持张开
+  hold=3s      中指握紧 (middle u8=0)
+  hold=3s      中指张开 (middle u8=255)
+
+用法（单独发令，配合录制脚本）：
+  export ROS_DOMAIN_ID=31
+  python3 tools/middle_step_cmd_pub.py
+"""
+
+from __future__ import annotations
+
+import argparse
+import time
+
+# 与 run_middle_step_experiment.py 共用，改这里两边一起变
+DEFAULT_PRE_WAIT = 3.0
+DEFAULT_HOLD = 3.0
+DEFAULT_RATE_HZ = 10.0
+FINGER_ROS_IDX = {"index": 2, "middle": 3, "ring": 4, "pinky": 5}
+
+
+def hold_finger_pose(pub, hand: str, finger: str, u8: float, duration: float, rate_hz: float):
+    from sensor_msgs.msg import JointState
+
+    idx = FINGER_ROS_IDX[finger]
+    end = time.perf_counter() + duration
+    dt = 1.0 / max(1.0, rate_hz)
+    while time.perf_counter() < end:
+        msg = JointState()
+        msg.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+        msg.position[idx] = float(u8)
+        pub.publish(msg)
+        time.sleep(dt)
+
+
+def run_step_sequence(
+    node,
+    *,
+    hand: str = "left",
+    finger: str = "middle",
+    pre_wait: float = DEFAULT_PRE_WAIT,
+    hold: float = DEFAULT_HOLD,
+    rate_hz: float = DEFAULT_RATE_HZ,
+    cmd_topic: str | None = None,
+):
+    from sensor_msgs.msg import JointState
+
+    topic = cmd_topic or f"/cb_{hand}_hand_control_cmd"
+    pub = node.create_publisher(JointState, topic, 10)
+    time.sleep(0.3)
+
+    t0 = time.perf_counter()
+    node.get_logger().info(
+        f"step cmd | finger={finger} | pre={pre_wait}s hold={hold}s | topic={topic}"
+    )
+
+    # 阶段0：保持张开
+    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] baseline open (255)")
+    hold_finger_pose(pub, hand, finger, 255.0, pre_wait, rate_hz)
+
+    # 阶段1：握紧
+    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] CLOSE middle u8=0")
+    hold_finger_pose(pub, hand, finger, 0.0, hold, rate_hz)
+
+    # 阶段2：张开
+    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] OPEN middle u8=255")
+    hold_finger_pose(pub, hand, finger, 255.0, hold, rate_hz)
+
+    node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] sequence done")
+    return t0
+
+
+def total_duration(pre_wait: float, hold: float, record_after: float = 0.0) -> float:
+    return pre_wait + hold + hold + record_after + 1.0
+
+
+def main():
+    parser = argparse.ArgumentParser(description="O6 middle finger step command publisher")
+    parser.add_argument("--hand", default="left", choices=["left", "right"])
+    parser.add_argument("--finger", default="middle", choices=list(FINGER_ROS_IDX))
+    parser.add_argument("--pre-wait", type=float, default=DEFAULT_PRE_WAIT)
+    parser.add_argument("--hold", type=float, default=DEFAULT_HOLD)
+    parser.add_argument("--rate", type=float, default=DEFAULT_RATE_HZ)
+    parser.add_argument("--cmd-topic", default="")
+    args = parser.parse_args()
+
+    import rclpy
+    from rclpy.node import Node
+
+    rclpy.init()
+    node = Node("middle_step_cmd_pub")
+    try:
+        run_step_sequence(
+            node,
+            hand=args.hand,
+            finger=args.finger,
+            pre_wait=args.pre_wait,
+            hold=args.hold,
+            rate_hz=args.rate,
+            cmd_topic=args.cmd_topic or None,
+        )
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

tools/plot_real_sim_compare.py

@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+"""真机 vs 仿真 q/v/力矩(或电流) 对比图。
+
+时间原点 = 首次收到「握紧」控制指令 (cmd_u8: 255→0) 的时刻 − 1 s。
+
+用法:
+  python3 tools/plot_real_sim_compare.py \\
+    reports/O6_middle_ros/ros_real_middle_left_20260716_172221.csv \\
+    reports/O6_middle_ros/ros_sim_middle_left_20260716_114515.csv
+"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+
+
+def load_csv(path: Path):
+    hdr = path.read_text().splitlines()[0].strip().split(",")
+    data = np.loadtxt(path, delimiter=",", skiprows=1)
+    col = {name: i for i, name in enumerate(hdr)}
+    return hdr, col, data
+
+
+def first_close_cmd_time(t: np.ndarray, cmd_u8: np.ndarray) -> float:
+    """首次 cmd 从 255 阶跃到 0（握紧）的时刻。"""
+    diff = np.diff(cmd_u8)
+    idx = np.where(diff < -100)[0]
+    if len(idx) == 0:
+        raise ValueError("未检测到 cmd 255→0 阶跃")
+    return float(t[idx[0] + 1])
+
+
+def align_series(t, *arrays, t_cmd: float, t_pre: float = 1.0):
+    t0 = t_cmd - t_pre
+    t_rel = t - t0
+    return (t_rel, *arrays)
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("real_csv", type=Path)
+    parser.add_argument("sim_csv", type=Path)
+    parser.add_argument("-o", "--out", type=Path, default=None)
+    parser.add_argument("--t-pre", type=float, default=1.0)
+    parser.add_argument("--t-max", type=float, default=8.0, help="相对原点后的最大时间(s)")
+    args = parser.parse_args()
+
+    _, cr, dr = load_csv(args.real_csv)
+    _, cs, ds = load_csv(args.sim_csv)
+
+    t_r, cmd_r = dr[:, cr["t_s"]], dr[:, cr["cmd_u8"]]
+    t_s, cmd_s = ds[:, cs["t_s"]], ds[:, cs["cmd_u8"]]
+
+    t_cmd_r = first_close_cmd_time(t_r, cmd_r)
+    t_cmd_s = first_close_cmd_time(t_s, cmd_s)
+
+    q_r = dr[:, cr["q_rad"]]
+    cmd_rad_r = dr[:, cr["cmd_rad"]]
+    v_r = dr[:, cr["v_rad_s"]]
+    q_s = ds[:, cs["q_rad"]]
+    cmd_rad_s = ds[:, cs["cmd_rad"]]
+    v_s = ds[:, cs["v_rad_s"]]
+
+    cur_r = dr[:, cr["current"]] if "current" in cr else np.full_like(q_r, np.nan)
+    tau_s = ds[:, cs["tau_Nm"]] if "tau_Nm" in cs else np.full_like(q_s, np.nan)
+
+    tr, qr, cmdr, vr, cur_r = align_series(
+        t_r, q_r, cmd_rad_r, v_r, cur_r, t_cmd=t_cmd_r, t_pre=args.t_pre
+    )
+    ts, qs, cmds, vs, tau_s = align_series(
+        t_s, q_s, cmd_rad_s, v_s, tau_s, t_cmd=t_cmd_s, t_pre=args.t_pre
+    )
+
+    mask_r = (tr >= -0.05) & (tr <= args.t_max)
+    mask_s = (ts >= -0.05) & (ts <= args.t_max)
+
+    out = args.out
+    if out is None:
+        out = args.real_csv.parent / "ros_real_vs_sim_middle_left_compare_qvt.png"
+
+    import os
+
+    root = Path(__file__).resolve().parents[1]
+    os.environ.setdefault("MPLCONFIGDIR", str(root / ".mplconfig"))
+    Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+
+    import matplotlib
+
+    matplotlib.use("Agg")
+    import matplotlib.pyplot as plt
+
+    fig, axes = plt.subplots(3, 1, figsize=(11, 9), sharex=True)
+    fig.suptitle(
+        "O6 middle_mcp_pitch  real vs sim\n"
+        f"t=0 = {args.t_pre:.0f}s before first close command (cmd_u8: 255->0)",
+        fontsize=12,
+    )
+
+    # --- angle ---
+    ax = axes[0]
+    # 控制指令：真机/仿真时序一致，只画一条黑色虚线
+    cmd_t = ts[mask_s] if np.any(mask_s) else tr[mask_r]
+    cmd_y = np.rad2deg(cmds[mask_s]) if np.any(mask_s) else np.rad2deg(cmdr[mask_r])
+    ax.plot(cmd_t, cmd_y, color="black", ls="--", lw=1.2, label="cmd")
+    ax.plot(ts[mask_s], np.rad2deg(qs[mask_s]), color="#1f77b4", lw=1.8, label="sim q")
+    ax.plot(tr[mask_r], np.rad2deg(qr[mask_r]), color="#ff7f0e", lw=1.8, label="real q")
+    ax.axvline(1.0, color="gray", ls=":", lw=1.0, alpha=0.8)
+    ax.set_ylabel("angle (deg)")
+    ax.legend(loc="best", fontsize=9)
+    ax.grid(True, alpha=0.3)
+
+    # --- velocity ---
+    ax = axes[1]
+    ax.plot(ts[mask_s], np.rad2deg(vs[mask_s]), color="#1f77b4", lw=1.8, label="sim v")
+    ax.plot(tr[mask_r], np.rad2deg(vr[mask_r]), color="#ff7f0e", lw=1.8, label="real v")
+    ax.axvline(1.0, color="gray", ls=":", lw=1.0, alpha=0.8)
+    ax.set_ylabel("velocity (deg/s)")
+    ax.legend(loc="best")
+    ax.grid(True, alpha=0.3)
+
+    # --- effort: sim tau vs real current ---
+    ax = axes[2]
+    ax.plot(ts[mask_s], tau_s[mask_s], color="#1f77b4", lw=1.8, label="sim τ (N·m)")
+    if np.any(np.isfinite(cur_r[mask_r])):
+        ax.plot(tr[mask_r], cur_r[mask_r], color="#ff7f0e", lw=1.8, label="real current")
+        ax.set_ylabel("τ (N·m) / current")
+    else:
+        ax.plot([], [], color="#ff7f0e", label="real current (no data)")
+        ax.set_ylabel("torque (N·m)")
+        ax.text(
+            0.5,
+            0.5,
+            "real current not recorded",
+            transform=ax.transAxes,
+            ha="center",
+            va="center",
+            fontsize=10,
+            color="#666",
+        )
+    ax.axvline(1.0, color="gray", ls=":", lw=1.0, alpha=0.8)
+    ax.set_xlabel("time (s)  [t=0: 1s before first close cmd]")
+    ax.legend(loc="best")
+    ax.grid(True, alpha=0.3)
+
+    fig.tight_layout()
+    fig.savefig(out, dpi=150)
+    plt.close(fig)
+
+    print(f"plot -> {out}")
+    print(f"real cmd↓ at abs t={t_cmd_r:.3f}s  -> origin t0={t_cmd_r - args.t_pre:.3f}s")
+    print(f"sim  cmd↓ at abs t={t_cmd_s:.3f}s  -> origin t0={t_cmd_s - args.t_pre:.3f}s")
+
+
+if __name__ == "__main__":
+    main()

tools/ros_mujoco_record_qvt.py

@@ -0,0 +1,339 @@
+#!/usr/bin/env python3
+"""ROS 控制指令 → 本地 MuJoCo → 记录并绘制 角度/速度/力矩。
+
+订 /cb_<hand>_hand_control_cmd（0～255），用与仿真节点相同的 O6 映射写 ctrl，
+从 MuJoCo 读 q / v / actuator_force，Ctrl+C 存 CSV + 三曲线图。
+
+用法：
+  # 终端1（可先不开 run_sim.sh，本脚本自带 MuJoCo）
+  python3 tools/ros_mujoco_record_qvt.py --hand left --finger middle --viewer
+
+  # 终端2 发 ROS 指令，或加 --self-sweep 自动发中指慢扫
+  python3 tools/ros_mujoco_record_qvt.py --hand left --finger middle --viewer --self-sweep
+"""
+
+from __future__ import annotations
+
+import argparse
+import sys
+import threading
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+PKG = (
+    ROOT
+    / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+)
+sys.path.insert(0, str(PKG.parent))
+
+from linker_hand_mujoco_ros2.utils.mapping import (  # noqa: E402
+    O6_JOINT_MAP,
+    O6_MIMIC,
+    apply_mimic,
+    range_to_arc_left,
+    range_to_arc_right,
+)
+
+FINGER_CFG = {
+    "index": {
+        "mcp": "index_mcp_pitch",
+        "mcp_act": "index_mcp_pitch_pos",
+        "ros_idx": 2,
+    },
+    "middle": {
+        "mcp": "middle_mcp_pitch",
+        "mcp_act": "middle_mcp_pitch_pos",
+        "ros_idx": 3,
+    },
+    "ring": {
+        "mcp": "ring_mcp_pitch",
+        "mcp_act": "ring_mcp_pitch_pos",
+        "ros_idx": 4,
+    },
+    "pinky": {
+        "mcp": "pinky_mcp_pitch",
+        "mcp_act": "pinky_mcp_pitch_pos",
+        "ros_idx": 5,
+    },
+}
+
+
+def map_position_array(position, joint_map):
+    mapped = [0.0] * len(joint_map)
+    for target_idx, source_idx in joint_map.items():
+        if source_idx < len(position):
+            mapped[target_idx] = position[source_idx]
+    return mapped
+
+
+def u8_to_ctrl(position_u8, hand_type: str):
+    if hand_type == "left":
+        tmp = range_to_arc_left(position_u8, "O6")
+    else:
+        tmp = range_to_arc_right(position_u8, "O6")
+    res = map_position_array(tmp, O6_JOINT_MAP)
+    from linker_hand_mujoco_ros2.utils.mapping import O6_JOINT_ARC
+
+    res = apply_mimic(res, O6_MIMIC, O6_JOINT_ARC)
+    return res
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--hand", choices=["left", "right"], default="left")
+    parser.add_argument("--finger", choices=list(FINGER_CFG.keys()), default="middle")
+    parser.add_argument("--cmd-topic", default="")
+    parser.add_argument("--out", type=Path, default=None)
+    parser.add_argument("--viewer", action="store_true")
+    parser.add_argument(
+        "--self-sweep",
+        action="store_true",
+        help="本进程自动往控制话题发 255→0→255 慢扫（方便自测）",
+    )
+    parser.add_argument(
+        "--exit-after-sweep",
+        action="store_true",
+        help="配合 --self-sweep：扫完自动结束并画图",
+    )
+    parser.add_argument("--duration", type=float, default=8.0, help="self-sweep 单向时长")
+    parser.add_argument(
+        "--record-duration",
+        type=float,
+        default=0.0,
+        help=">0 时录满该秒数自动结束（配合 middle_step_cmd_pub）",
+    )
+    parser.add_argument("--label", default="ros_sim")
+    args = parser.parse_args()
+    if args.self_sweep and not args.exit_after_sweep:
+        # 自测默认扫完就退出，避免一直挂着
+        args.exit_after_sweep = True
+
+    cfg = FINGER_CFG[args.finger]
+    cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd"
+    out_dir = args.out or (ROOT / f"reports/O6_{args.finger}_ros")
+    out_dir.mkdir(parents=True, exist_ok=True)
+
+    xml = (
+        PKG
+        / f"urdf/O6/linker_hand_o6_{args.hand}/linker_hand_o6_{args.hand}.xml"
+    )
+
+    import mujoco
+    import mujoco.viewer
+
+    model = mujoco.MjModel.from_xml_path(str(xml))
+    data = mujoco.MjData(model)
+    model.opt.disableflags = 0
+
+    mcp_jnt = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, cfg["mcp"])
+    mcp_act = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, cfg["mcp_act"])
+    if mcp_jnt < 0 or mcp_act < 0:
+        raise RuntimeError(f"joint/actuator not found for {args.finger}")
+    qadr = int(model.jnt_qposadr[mcp_jnt])
+    dadr = int(model.jnt_dofadr[mcp_jnt])
+    mcp_lo, mcp_hi = map(float, model.jnt_range[mcp_jnt])
+
+    ctrl_lock = threading.Lock()
+    ctrl_values = np.zeros(model.nu)
+    last_cmd_u8 = [255.0] * 6
+    running = True
+    sweep_done = threading.Event()
+    logs = {"t": [], "cmd_rad": [], "q": [], "v": [], "tau": [], "cmd_u8": []}
+    t0 = time.perf_counter()
+    t_stop = (
+        t0 + args.record_duration if args.record_duration > 0 else None
+    )
+
+    import signal
+
+    def _stop(*_):
+        nonlocal running
+        running = False
+
+    signal.signal(signal.SIGINT, _stop)
+    signal.signal(signal.SIGTERM, _stop)
+
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import JointState
+
+    class CmdNode(Node):
+        def __init__(self):
+            super().__init__("ros_mujoco_record_qvt")
+            self.create_subscription(JointState, cmd_topic, self.on_cmd, 50)
+            self.get_logger().info(
+                f"MuJoCo+ROS record | topic={cmd_topic} | finger={args.finger} | "
+                f"joint={cfg['mcp']} range=[{mcp_lo:.2f},{mcp_hi:.2f}]"
+            )
+            self.get_logger().info("Ctrl+C to stop and plot")
+
+        def on_cmd(self, msg: JointState):
+            nonlocal last_cmd_u8
+            pos = list(msg.position)
+            if len(pos) < 6:
+                pos = pos + [255.0] * (6 - len(pos))
+            last_cmd_u8 = pos[:6]
+            mapped = u8_to_ctrl(last_cmd_u8, args.hand)
+            with ctrl_lock:
+                n = min(len(mapped), model.nu)
+                ctrl_values[:n] = mapped[:n]
+
+    rclpy.init()
+    node = CmdNode()
+
+    def spin_ros():
+        while running and rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.01)
+
+    ros_thread = threading.Thread(target=spin_ros, daemon=True)
+    ros_thread.start()
+
+    pub = None
+    if args.self_sweep:
+        pub = node.create_publisher(JointState, cmd_topic, 10)
+        time.sleep(0.3)
+
+        def sweep_pub():
+            nonlocal running
+            half = args.duration
+            ros_i = cfg["ros_idx"]
+            # 255 -> 0
+            t_start = time.time()
+            while running and time.time() - t_start < half:
+                u = (time.time() - t_start) / half
+                mid = 255.0 * (1.0 - u)
+                m = JointState()
+                m.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+                m.position[ros_i] = mid
+                pub.publish(m)
+                time.sleep(0.01)
+            # 0 -> 255
+            t_start = time.time()
+            while running and time.time() - t_start < half:
+                u = (time.time() - t_start) / half
+                mid = 255.0 * u
+                m = JointState()
+                m.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+                m.position[ros_i] = mid
+                pub.publish(m)
+                time.sleep(0.01)
+            print("self-sweep finished", flush=True)
+            sweep_done.set()
+            if args.exit_after_sweep:
+                running = False
+
+        threading.Thread(target=sweep_pub, daemon=True).start()
+
+    viewer = None
+    if args.viewer:
+        viewer = mujoco.viewer.launch_passive(model, data)
+
+    try:
+        while running:
+            if viewer is not None and not viewer.is_running():
+                break
+            if t_stop is not None and time.perf_counter() >= t_stop:
+                break
+            with ctrl_lock:
+                data.ctrl[:] = ctrl_values
+            mujoco.mj_step(model, data)
+            if viewer is not None:
+                viewer.sync()
+
+            t = time.perf_counter() - t0
+            q = float(data.qpos[qadr])
+            v = float(data.qvel[dadr])
+            tau = float(data.actuator_force[mcp_act])
+            cmd_rad = float(ctrl_values[mcp_act]) if mcp_act < len(ctrl_values) else 0.0
+            logs["t"].append(t)
+            logs["cmd_rad"].append(cmd_rad)
+            logs["q"].append(q)
+            logs["v"].append(v)
+            logs["tau"].append(tau)
+            logs["cmd_u8"].append(float(last_cmd_u8[cfg["ros_idx"]]))
+            time.sleep(0.001)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        running = False
+        if viewer is not None:
+            viewer.close()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+    if len(logs["t"]) < 2:
+        print("no samples")
+        return
+
+    for k in logs:
+        logs[k] = np.asarray(logs[k], dtype=float)
+
+    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+    stem = f"{args.label}_{args.finger}_{args.hand}_{stamp}"
+    csv_path = out_dir / f"{stem}.csv"
+    png_path = out_dir / f"{stem}_qvt.png"
+
+    np.savetxt(
+        csv_path,
+        np.column_stack(
+            [
+                logs["t"],
+                logs["cmd_u8"],
+                logs["cmd_rad"],
+                logs["q"],
+                logs["v"],
+                logs["tau"],
+            ]
+        ),
+        delimiter=",",
+        header="t_s,cmd_u8,cmd_rad,q_rad,v_rad_s,tau_Nm",
+        comments="",
+    )
+
+    import os
+
+    os.environ.setdefault("MPLCONFIGDIR", str(ROOT / ".mplconfig"))
+    Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+    import matplotlib
+
+    matplotlib.use("Agg")
+    import matplotlib.pyplot as plt
+
+    fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+    fig.suptitle(
+        f"ROS->MuJoCo {args.hand} {cfg['mcp']}  "
+        f"range=[{mcp_lo:.2f}, {mcp_hi:.2f}] rad",
+        fontsize=12,
+    )
+    axes[0].plot(logs["t"], np.rad2deg(logs["cmd_rad"]), "k--", lw=1.2, label="cmd")
+    axes[0].plot(logs["t"], np.rad2deg(logs["q"]), "C0", lw=1.6, label="q")
+    axes[0].set_ylabel("angle (deg)")
+    axes[0].legend(loc="best")
+    axes[0].grid(True, alpha=0.3)
+
+    axes[1].plot(logs["t"], np.rad2deg(logs["v"]), "C1", lw=1.6, label="v")
+    axes[1].set_ylabel("velocity (deg/s)")
+    axes[1].legend(loc="best")
+    axes[1].grid(True, alpha=0.3)
+
+    axes[2].plot(logs["t"], logs["tau"], "C3", lw=1.6, label="tau (actuator_force)")
+    axes[2].set_ylabel("torque (N·m)")
+    axes[2].set_xlabel("time (s)")
+    axes[2].legend(loc="best")
+    axes[2].grid(True, alpha=0.3)
+    fig.tight_layout()
+    fig.savefig(png_path, dpi=140)
+    plt.close(fig)
+
+    print(f"CSV  -> {csv_path}")
+    print(f"plot -> {png_path}")
+    print(f"samples={len(logs['t'])}")
+
+
+if __name__ == "__main__":
+    main()

tools/ros_real_record_qvt.py

@@ -0,0 +1,346 @@
+#!/usr/bin/env python3
+"""真机 ROS 录制：订 control_cmd + hand_state，画与仿真相同的三曲线（deg / deg/s / current）。
+
+配合 step 指令（先握中指再张开）：
+  python3 tools/ros_real_record_qvt.py --hand left --finger middle --auto-step
+
+手动：终端1 本脚本；终端2 发 ros2 topic pub ...
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import sys
+import threading
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+PKG = (
+    ROOT
+    / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+)
+sys.path.insert(0, str(PKG.parent))
+
+from linker_hand_mujoco_ros2.utils.mapping import (  # noqa: E402
+    range_to_arc_left,
+    range_to_arc_right,
+)
+
+FINGER_CFG = {
+    "index": {"ros_idx": 2, "mcp": "index_mcp_pitch", "lo": 0.0, "hi": 1.60},
+    "middle": {"ros_idx": 3, "mcp": "middle_mcp_pitch", "lo": 0.0, "hi": 1.60},
+    "ring": {"ros_idx": 4, "mcp": "ring_mcp_pitch", "lo": 0.0, "hi": 1.60},
+    "pinky": {"ros_idx": 5, "mcp": "pinky_mcp_pitch", "lo": 0.0, "hi": 1.60},
+}
+
+
+def u8_to_rad(u8: float, hand: str, ros_idx: int) -> float:
+    pos = [255.0] * 6
+    pos[ros_idx] = float(u8)
+    if hand == "left":
+        arc = range_to_arc_left(pos, "O6")
+    else:
+        arc = range_to_arc_right(pos, "O6")
+    return float(arc[ros_idx])
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--hand", choices=["left", "right"], default="left")
+    parser.add_argument("--finger", choices=list(FINGER_CFG.keys()), default="middle")
+    parser.add_argument("--cmd-topic", default="")
+    parser.add_argument("--state-topic", default="")
+    parser.add_argument("--info-topic", default="")
+    parser.add_argument("--out", type=Path, default=None)
+    parser.add_argument("--label", default="ros_real")
+    parser.add_argument("--hz", type=float, default=100.0)
+    parser.add_argument(
+        "--auto-step",
+        action="store_true",
+        help="自动发 255..255,0,255..255 再张开，录完退出",
+    )
+    parser.add_argument("--pre-wait", type=float, default=2.0, help="开录后等待(s)")
+    parser.add_argument("--hold", type=float, default=3.0, help="每步保持(s)")
+    parser.add_argument("--record-after", type=float, default=1.0, help="第二步后多录(s)")
+    parser.add_argument(
+        "--duration",
+        type=float,
+        default=0.0,
+        help=">0 时录满该秒数自动结束（配合外部 middle_step_cmd_pub）",
+    )
+    args = parser.parse_args()
+
+    cfg = FINGER_CFG[args.finger]
+    ros_i = cfg["ros_idx"]
+    cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd"
+    state_topic = args.state_topic or f"/cb_{args.hand}_hand_state"
+    info_topic = args.info_topic or f"/cb_{args.hand}_hand_info"
+    out_dir = args.out or (ROOT / f"reports/O6_{args.finger}_ros")
+    out_dir.mkdir(parents=True, exist_ok=True)
+
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import JointState
+    from std_msgs.msg import String
+
+    logs = {
+        "t": [],
+        "cmd_u8": [],
+        "joint_u8": [],
+        "cmd_rad": [],
+        "q_rad": [],
+        "v_rad_s": [],
+        "current": [],
+    }
+    t0 = time.perf_counter()
+    running = True
+    state_lock = threading.Lock()
+    last_cmd_u8 = [255.0] * 6
+    last_joint_u8 = [float("nan")] * 6
+    last_current = [float("nan")] * 6
+    have_state = False
+    have_current = False
+
+    class Recorder(Node):
+        def __init__(self):
+            super().__init__("ros_real_record_qvt")
+            self.pub = self.create_publisher(JointState, cmd_topic, 10)
+            self.create_subscription(JointState, cmd_topic, self.on_cmd, 50)
+            self.create_subscription(JointState, state_topic, self.on_state, 50)
+            self.create_subscription(String, info_topic, self.on_info, 10)
+            self.create_timer(1.0 / max(1.0, args.hz), self.on_timer)
+            self.get_logger().info(
+                f"real record | cmd={cmd_topic} | state={state_topic} | "
+                f"finger={args.finger} idx={ros_i}"
+            )
+
+        def on_cmd(self, msg: JointState):
+            nonlocal last_cmd_u8
+            pos = list(msg.position)
+            if len(pos) >= 6:
+                with state_lock:
+                    last_cmd_u8 = [float(x) for x in pos[:6]]
+
+        def on_state(self, msg: JointState):
+            nonlocal have_state, last_joint_u8, have_current, last_current
+            with state_lock:
+                pos = list(msg.position)
+                if len(pos) >= 6 and all(float(x) >= 0 for x in pos[:6]):
+                    have_state = True
+                    last_joint_u8 = [float(x) for x in pos[:6]]
+                if msg.effort and any(abs(float(x)) > 1e-6 for x in msg.effort):
+                    have_current = True
+                    last_current = [float(x) for x in msg.effort[:6]]
+
+        def on_info(self, msg: String):
+            nonlocal have_current, last_current
+            try:
+                info = json.loads(msg.data)
+                cur = info.get("current")
+                if isinstance(cur, list) and len(cur) >= 6:
+                    if any(float(x) >= 0 for x in cur[:6]):
+                        with state_lock:
+                            have_current = True
+                            last_current = [float(x) for x in cur[:6]]
+            except json.JSONDecodeError:
+                pass
+
+        def publish_pose(self, middle_u8: float):
+            nonlocal last_cmd_u8
+            m = JointState()
+            m.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+            m.position[ros_i] = float(middle_u8)
+            with state_lock:
+                last_cmd_u8 = list(m.position)
+            self.pub.publish(m)
+
+        def hold_pose(self, middle_u8: float, duration: float, rate_hz: float = 10.0):
+            """持续重发控制帧（SDK 只响应订阅到的 cmd；单发 --once 可能丢）。"""
+            end = time.perf_counter() + duration
+            dt = 1.0 / max(1.0, rate_hz)
+            while time.perf_counter() < end and running:
+                self.publish_pose(middle_u8)
+                time.sleep(dt)
+
+        def on_timer(self):
+            nonlocal running
+            if not running:
+                return
+            with state_lock:
+                joint_u8 = list(last_joint_u8)
+                current = list(last_current)
+            cmd_u8 = float(last_cmd_u8[ros_i])
+            if have_state and not np.isnan(joint_u8[ros_i]):
+                q_u8 = float(joint_u8[ros_i])
+            else:
+                q_u8 = cmd_u8
+            t = time.perf_counter() - t0
+            q_rad = u8_to_rad(q_u8, args.hand, ros_i)
+            cmd_rad = u8_to_rad(cmd_u8, args.hand, ros_i)
+            cur = current[ros_i] if len(current) > ros_i else float("nan")
+            logs["t"].append(t)
+            logs["cmd_u8"].append(cmd_u8)
+            logs["joint_u8"].append(q_u8)
+            logs["cmd_rad"].append(cmd_rad)
+            logs["q_rad"].append(q_rad)
+            logs["current"].append(cur)
+            if len(logs["t"]) >= 2:
+                dt = logs["t"][-1] - logs["t"][-2]
+                if dt > 1e-9:
+                    v = (logs["q_rad"][-1] - logs["q_rad"][-2]) / dt
+                else:
+                    v = 0.0
+            else:
+                v = 0.0
+            logs["v_rad_s"].append(v)
+
+    rclpy.init()
+    node = Recorder()
+
+    def spin():
+        while running and rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.01)
+
+    threading.Thread(target=spin, daemon=True).start()
+
+    if args.auto_step:
+        deadline = time.perf_counter() + max(5.0, args.pre_wait)
+        while time.perf_counter() < deadline and not have_state:
+            time.sleep(0.05)
+        if not have_state:
+            node.get_logger().warn(
+                "仍未收到 hand_state（可能 SDK 未跑 / Domain 不对 / 全是 -1）"
+            )
+        time.sleep(max(0.5, args.pre_wait))
+        node.get_logger().info("step1: middle close (u8=0)")
+        node.hold_pose(0.0, args.hold)
+        node.get_logger().info("step2: middle open (u8=255)")
+        node.hold_pose(255.0, args.hold + args.record_after)
+        running = False
+    else:
+        node.get_logger().info(
+            "listen mode: 外部发令 (middle_step_cmd_pub) 或 Ctrl+C 结束"
+        )
+        t_end = (
+            time.perf_counter() + args.duration if args.duration > 0 else None
+        )
+        try:
+            while running:
+                if t_end is not None and time.perf_counter() >= t_end:
+                    break
+                time.sleep(0.05)
+        except KeyboardInterrupt:
+            running = False
+
+    time.sleep(0.2)
+    node.destroy_node()
+    if rclpy.ok():
+        rclpy.shutdown()
+
+    if len(logs["t"]) < 2:
+        print("no samples")
+        return 1
+
+    for k in logs:
+        logs[k] = np.asarray(logs[k], dtype=float)
+
+    if len(logs["v_rad_s"]) >= 5:
+        logs["v_rad_s"] = np.convolve(
+            logs["v_rad_s"], np.ones(5) / 5.0, mode="same"
+        )
+
+    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+    stem = f"{args.label}_{args.finger}_{args.hand}_{stamp}"
+    csv_path = out_dir / f"{stem}.csv"
+    png_path = out_dir / f"{stem}_qvt.png"
+
+    np.savetxt(
+        csv_path,
+        np.column_stack(
+            [
+                logs["t"],
+                logs["cmd_u8"],
+                logs["joint_u8"],
+                logs["cmd_rad"],
+                logs["q_rad"],
+                logs["v_rad_s"],
+                logs["current"],
+            ]
+        ),
+        delimiter=",",
+        header="t_s,cmd_u8,joint_u8,cmd_rad,q_rad,v_rad_s,current",
+        comments="",
+    )
+
+    import os
+
+    os.environ.setdefault("MPLCONFIGDIR", str(ROOT / ".mplconfig"))
+    Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+    import matplotlib
+
+    matplotlib.use("Agg")
+    import matplotlib.pyplot as plt
+
+    mcp_lo, mcp_hi = cfg["lo"], cfg["hi"]
+    fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+    fig.suptitle(
+        f"ROS->Real {args.hand} {cfg['mcp']}  "
+        f"range=[{mcp_lo:.2f}, {mcp_hi:.2f}] rad  "
+        f"(255=open/0deg, 0=close/~92deg)",
+        fontsize=11,
+    )
+    axes[0].plot(logs["t"], np.rad2deg(logs["cmd_rad"]), "k--", lw=1.2, label="cmd")
+    axes[0].plot(logs["t"], np.rad2deg(logs["q_rad"]), "C0", lw=1.6, label="q")
+    axes[0].set_ylabel("angle (deg)")
+    axes[0].legend(loc="best")
+    axes[0].grid(True, alpha=0.3)
+
+    axes[1].plot(logs["t"], np.rad2deg(logs["v_rad_s"]), "C1", lw=1.6, label="v")
+    axes[1].set_ylabel("velocity (deg/s)")
+    axes[1].legend(loc="best")
+    axes[1].grid(True, alpha=0.3)
+
+    if have_current and np.any(np.isfinite(logs["current"])):
+        axes[2].plot(
+            logs["t"], logs["current"], "C3", lw=1.6, label="current (hand_info)"
+        )
+        axes[2].set_ylabel("current")
+    else:
+        axes[2].text(
+            0.5,
+            0.5,
+            "no current (subscribe hand_info or state.effort)",
+            ha="center",
+            va="center",
+            transform=axes[2].transAxes,
+        )
+        axes[2].set_ylabel("current")
+    axes[2].set_xlabel("time (s)")
+    axes[2].legend(loc="best")
+    axes[2].grid(True, alpha=0.3)
+    fig.tight_layout()
+    fig.savefig(png_path, dpi=140)
+    plt.close(fig)
+
+    print(f"CSV  -> {csv_path}")
+    print(f"plot -> {png_path}")
+    print(f"samples={len(logs['t'])}  state={'yes' if have_state else 'NO'}")
+    cmd_span = float(np.max(logs["cmd_u8"]) - np.min(logs["cmd_u8"]))
+    joint_span = float(np.max(logs["joint_u8"]) - np.min(logs["joint_u8"]))
+    print(f"cmd_u8 span={cmd_span:.1f}  joint_u8 span={joint_span:.1f}")
+    if cmd_span < 1.0:
+        print("[warn] cmd 全程几乎不变 → 控制指令可能没发出去或未写入录制")
+    if not have_state:
+        print("[warn] 未收到有效 state → q 只能用 cmd 代替，且真机可能没动")
+    if cmd_span >= 1.0 and joint_span < 3.0 and have_state:
+        print("[warn] 有 cmd 变化但 joint 几乎不动 → 查 CAN / SDK / 力矩限制")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/run_middle_step_experiment.py

@@ -0,0 +1,231 @@
+#!/usr/bin/env python3
+"""O6 中指 step 实验：真机 / 仿真共用同一套控制时序。
+
+时序（默认，两边完全一致）：
+  3s 张开 → 3s 握紧 → 3s 张开 → 2s 余量录制
+
+用法：
+  export ROS_DOMAIN_ID=31
+
+  # 真机（需 can0 UP + SDK，或脚本尝试自动启 SDK）
+  python3 tools/run_middle_step_experiment.py real
+
+  # 仿真（自带 MuJoCo，无需 run_sim.sh）
+  python3 tools/run_middle_step_experiment.py sim
+
+  # 两次跑完后出对比图（自动找目录下最新 real/sim csv）
+  python3 tools/run_middle_step_experiment.py compare
+
+  # 指定 csv
+  python3 tools/run_middle_step_experiment.py compare \\
+    --real-csv reports/O6_middle_ros/ros_real_....csv \\
+    --sim-csv  reports/O6_middle_ros/ros_sim_....csv
+"""
+
+from __future__ import annotations
+
+import argparse
+import glob
+import os
+import signal
+import subprocess
+import sys
+import threading
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+OUT_DIR = ROOT / "reports/O6_middle_ros"
+
+# 与 middle_step_cmd_pub.py 保持一致
+PRE_WAIT = 3.0
+HOLD = 3.0
+RECORD_AFTER = 2.0
+RATE_HZ = 10.0
+
+
+def record_duration() -> float:
+    return PRE_WAIT + HOLD + HOLD + RECORD_AFTER + 1.5
+
+
+def setup_ros_env():
+    os.environ.setdefault("ROS_DOMAIN_ID", "31")
+    os.environ.setdefault("ROS_LOCALHOST_ONLY", "0")
+
+
+def setup_mujoco_pythonpath():
+    venv_site = ROOT / "venv/lib/python3.12/site-packages"
+    if venv_site.is_dir():
+        os.environ["PYTHONPATH"] = f"{venv_site}{os.pathsep}{os.environ.get('PYTHONPATH', '')}"
+
+
+def run_real_experiment(args):
+    setup_ros_env()
+    OUT_DIR.mkdir(parents=True, exist_ok=True)
+    dur = record_duration()
+
+    rec_cmd = [
+        sys.executable,
+        str(ROOT / "tools/ros_real_record_qvt.py"),
+        "--hand",
+        args.hand,
+        "--finger",
+        args.finger,
+        "--label",
+        "ros_real",
+        "--out",
+        str(OUT_DIR),
+        "--duration",
+        str(dur),
+    ]
+    print(f"[real] 启动录制 {dur:.1f}s ...")
+    rec = subprocess.Popen(rec_cmd, cwd=str(ROOT))
+    time.sleep(1.0)
+
+    pub_cmd = [
+        sys.executable,
+        str(ROOT / "tools/middle_step_cmd_pub.py"),
+        "--hand",
+        args.hand,
+        "--finger",
+        args.finger,
+        "--pre-wait",
+        str(PRE_WAIT),
+        "--hold",
+        str(HOLD),
+        "--rate",
+        str(RATE_HZ),
+    ]
+    print("[real] 发送统一 step 指令 ...")
+    pub = subprocess.run(pub_cmd, cwd=str(ROOT))
+    if pub.returncode != 0:
+        rec.send_signal(signal.SIGINT)
+        rec.wait()
+        return pub.returncode
+
+    print("[real] 等待录制结束 ...")
+    try:
+        rec.wait(timeout=dur + 5)
+    except subprocess.TimeoutExpired:
+        rec.send_signal(signal.SIGINT)
+        rec.wait()
+    return rec.returncode
+
+
+def run_sim_experiment(args):
+    setup_ros_env()
+    setup_mujoco_pythonpath()
+    OUT_DIR.mkdir(parents=True, exist_ok=True)
+    dur = record_duration()
+
+    rec_cmd = [
+        sys.executable,
+        str(ROOT / "tools/ros_mujoco_record_qvt.py"),
+        "--hand",
+        args.hand,
+        "--finger",
+        args.finger,
+        "--label",
+        "ros_sim",
+        "--out",
+        str(OUT_DIR),
+        "--record-duration",
+        str(dur),
+    ]
+    if args.viewer:
+        rec_cmd.append("--viewer")
+
+    print(f"[sim] 启动 MuJoCo 录制 {dur:.1f}s ...")
+    rec = subprocess.Popen(rec_cmd, cwd=str(ROOT))
+    time.sleep(1.0)
+
+    pub_cmd = [
+        sys.executable,
+        str(ROOT / "tools/middle_step_cmd_pub.py"),
+        "--hand",
+        args.hand,
+        "--finger",
+        args.finger,
+        "--pre-wait",
+        str(PRE_WAIT),
+        "--hold",
+        str(HOLD),
+        "--rate",
+        str(RATE_HZ),
+    ]
+    print("[sim] 发送统一 step 指令 ...")
+    pub = subprocess.run(pub_cmd, cwd=str(ROOT))
+    if pub.returncode != 0:
+        rec.send_signal(signal.SIGINT)
+        rec.wait()
+        return pub.returncode
+
+    print("[sim] 等待录制结束 ...")
+    try:
+        rec.wait(timeout=dur + 10)
+    except subprocess.TimeoutExpired:
+        rec.send_signal(signal.SIGINT)
+        rec.wait()
+    return rec.returncode
+
+
+def latest_csv(pattern: str) -> Path | None:
+    files = sorted(glob.glob(str(OUT_DIR / pattern)), key=os.path.getmtime)
+    return Path(files[-1]) if files else None
+
+
+def run_compare(args):
+    real_csv = Path(args.real_csv) if args.real_csv else latest_csv("ros_real_*middle*.csv")
+    sim_csv = Path(args.sim_csv) if args.sim_csv else latest_csv("ros_sim_*middle*.csv")
+    if not real_csv or not real_csv.is_file():
+        print("[error] 找不到真机 csv，先跑: python3 tools/run_middle_step_experiment.py real")
+        return 1
+    if not sim_csv or not sim_csv.is_file():
+        print("[error] 找不到仿真 csv，先跑: python3 tools/run_middle_step_experiment.py sim")
+        return 1
+
+    out = OUT_DIR / "ros_real_vs_sim_middle_left_compare_qvt.png"
+    cmd = [
+        sys.executable,
+        str(ROOT / "tools/plot_real_sim_compare.py"),
+        str(real_csv),
+        str(sim_csv),
+        "-o",
+        str(out),
+    ]
+    print(f"[compare] real={real_csv.name}")
+    print(f"[compare] sim ={sim_csv.name}")
+    return subprocess.call(cmd, cwd=str(ROOT))
+
+
+def main():
+    parser = argparse.ArgumentParser(description="O6 middle step experiment (real/sim/compare)")
+    parser.add_argument(
+        "mode",
+        choices=["real", "sim", "compare"],
+        help="real=真机录制 | sim=MuJoCo录制 | compare=对比图",
+    )
+    parser.add_argument("--hand", default="left")
+    parser.add_argument("--finger", default="middle")
+    parser.add_argument("--viewer", action="store_true", help="sim 模式打开 MuJoCo 窗口")
+    parser.add_argument("--real-csv", default="")
+    parser.add_argument("--sim-csv", default="")
+    args = parser.parse_args()
+
+    print("=" * 60)
+    print("O6 middle step 实验时序（真机 / 仿真相同）")
+    print(f"  pre_wait={PRE_WAIT}s  hold={HOLD}s  record_after={RECORD_AFTER}s")
+    print(f"  ROS_DOMAIN_ID={os.environ.get('ROS_DOMAIN_ID', '31')}")
+    print("=" * 60)
+
+    if args.mode == "real":
+        print("\n[提示] 真机请先: sudo ip link set can0 up type can bitrate 1000000")
+        print("       并运行 ./run_real_hand.sh（或本脚本前自行启动 SDK）\n")
+        return run_real_experiment(args) or 0
+    if args.mode == "sim":
+        return run_sim_experiment(args) or 0
+    return run_compare(args)
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())