Add project sources for Mujoco LinkerHand ROS2

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

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()