Add project sources for Mujoco LinkerHand ROS2
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
sunxianghui
Executable
+241
@@ -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()
|
||||
Reference in New Issue
Block a user