[software] 添加16DOF早期训练仿真与Sim2Real闭环

05_software/train/rc_mjlab/sim2sim/tools/logger.py

@@ -0,0 +1,26 @@
+import time
+import numpy as np
+from pathlib import Path
+from tools.math_utils import get_body_velocity
+
+class SimpleLogger:
+    def __init__(self, log_dir="."):
+        self.log_file = Path(log_dir) / f"sim2sim_log_{int(time.time())}.txt"
+        self.data = []
+        print(f"[Logger] Will write diagnostic data to: {self.log_file}")
+        
+    def update(self, sim_time, m, d, command):
+        true_vel_body = get_body_velocity(m, d)
+        # 记录 仿真时间、控制指令、真实的机体线速度
+        self.data.append(
+            f"{sim_time:.4f}, {command[0]:.4f}, {command[1]:.4f}, {command[2]:.4f}, "
+            f"{true_vel_body[0]:.4f}, {true_vel_body[1]:.4f}, {true_vel_body[2]:.4f}"
+        )
+        
+    def save(self):
+        print(f"\n[Logger] Saving {len(self.data)} records to {self.log_file}...")
+        with open(self.log_file, "w") as f:
+            f.write("time, cmd_vx, cmd_vy, cmd_yaw, true_vx, true_vy, true_vz\n")
+            for row in self.data:
+                f.write(row + "\n")
+        print("[Logger] Save complete.")