fix teleop

examples/test_torque/log_and_plot_feetech.py

@@ -0,0 +1,44 @@
+
+
+import matplotlib.pyplot as plt
+import time
+from typing import List, Tuple
+def log_and_plot_params(bus, params_to_log: list, servo_names: list, 
+                        test_id="servo_log", interval=0.1, duration=5, save_plot=True) -> Tuple[dict, List[float]]:
+    
+    """
+    Logs specific servo parameters for a given duration and generates a plot.
+    """
+
+    servo_data = {servo_name: {param: [] for param in params_to_log} for servo_name in servo_names}
+    timestamps = []
+
+    start_time = time.time()
+
+    while time.time() - start_time < duration:
+        timestamp = time.time() - start_time
+        timestamps.append(timestamp)
+        for param in params_to_log:
+            values = bus.read(param, servo_names)
+            for servo_name, value in zip(servo_names, values):
+                servo_data[servo_name][param].append(value)
+
+        time.sleep(interval)
+
+    if save_plot:
+        for servo_name, data in servo_data.items():
+            plt.figure(figsize=(10, 6))
+            for param in params_to_log:
+                if all(v is not None for v in data[param]):
+                    plt.plot(timestamps, data[param], label=param)
+            plt.xlabel("Time (s)")
+            plt.ylabel("Parameter Values")
+            plt.title(f"Parameter Trends for Servo: {servo_name}")
+            plt.legend()
+            plt.grid(True)
+            plt.tight_layout()
+            plot_filename = f"{test_id}_{servo_name}.png"
+            plt.savefig(plot_filename)
+            print(f"Plot saved as {plot_filename}")
+            
+    return servo_data, timestamps