Repository navigation
Expand file tree
/
Copy pathquick_plot_en.py
More file actions
134 lines (113 loc) · 5.91 KB
/
Copy pathquick_plot_en.py
File metadata and controls
134 lines (113 loc) · 5.91 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
#!/usr/bin/env python3
"""
Quick data visualization script (English version)
Simplified version for quick robot data viewing
"""
import pandas as pd
import matplotlib.pyplot as plt
import numpy as np
import glob
import os
def find_latest_file():
"""Find the latest data file"""
files = glob.glob("robot_data_*.csv")
if not files:
print("No data files found!")
return None
return max(files, key=os.path.getmtime)
def quick_plot():
"""Quick plot of all important data"""
# Find and load data
filename = find_latest_file()
if not filename:
return
print(f"Loading file: {filename}")
df = pd.read_csv(filename)
# Convert time
df['time_sec'] = (df['timestamp'] - df['timestamp'].iloc[0]) / 1000.0
# Create large figure with more subplots
fig, axes = plt.subplots(4, 2, figsize=(16, 16))
fig.suptitle(f'Robot Data Analysis - {filename}', fontsize=16)
# 1. Leg lengths
axes[0,0].plot(df['time_sec'].values, df['leg_length_R'].values, 'b-', label='leglengthR', linewidth=2)
axes[0,0].plot(df['time_sec'].values, df['leg_length_L'].values, 'r-', label='leglengthL', linewidth=2)
axes[0,0].set_title('Leg Length Changes')
axes[0,0].set_ylabel('Length (m)')
axes[0,0].legend()
axes[0,0].grid(True, alpha=0.3)
# 2. Leg angles (theta)
axes[0,1].plot(df['time_sec'].values, np.degrees(df['theta_R'].values), 'b-', label='Right theta', linewidth=2)
axes[0,1].plot(df['time_sec'].values, np.degrees(df['theta_L'].values), 'r-', label='Left theta', linewidth=2)
axes[0,1].set_title('Leg Angles (theta)')
axes[0,1].set_ylabel('Angle (deg)')
axes[0,1].legend()
axes[0,1].grid(True, alpha=0.3)
# 3. Leg angular velocities (dtheta)
axes[1,0].plot(df['time_sec'].values, np.degrees(df['dtheta_R'].values), 'b-', label='Right dtheta', linewidth=2)
axes[1,0].plot(df['time_sec'].values, np.degrees(df['dtheta_L'].values), 'r-', label='Left dtheta', linewidth=2)
axes[1,0].set_title('Leg Angular Velocities (dtheta)')
axes[1,0].set_ylabel('Angular Velocity (deg/s)')
axes[1,0].legend()
axes[1,0].grid(True, alpha=0.3)
# 4. Body pitch angles (phi)
axes[1,1].plot(df['time_sec'].values, np.degrees(df['phi_R'].values), 'g-', label='Right phi', linewidth=2)
axes[1,1].plot(df['time_sec'].values, np.degrees(df['phi_L'].values), 'orange', label='Left phi', linewidth=2)
axes[1,1].set_title('Body Pitch Angles (phi)')
axes[1,1].set_ylabel('Angle (deg)')
axes[1,1].legend()
axes[1,1].grid(True, alpha=0.3)
# 5. Body pitch angular velocities (dphi)
axes[2,0].plot(df['time_sec'].values, np.degrees(df['dphi_R'].values), 'g-', label='Right dphi', linewidth=2)
axes[2,0].plot(df['time_sec'].values, np.degrees(df['dphi_L'].values), 'orange', label='Left dphi', linewidth=2)
axes[2,0].set_title('Body Pitch Angular Velocities (dphi)')
axes[2,0].set_ylabel('Angular Velocity (deg/s)')
axes[2,0].legend()
axes[2,0].grid(True, alpha=0.3)
# 6. Body position and velocity
ax_pos = axes[2,1]
ax_vel = ax_pos.twinx()
line1 = ax_pos.plot(df['time_sec'].values, df['x'].values, 'purple', label='Position x', linewidth=2)
line2 = ax_vel.plot(df['time_sec'].values, df['dx'].values, 'brown', label='Velocity dx', linewidth=2)
ax_pos.set_title('Body Motion')
ax_pos.set_ylabel('Position (m)', color='purple')
ax_vel.set_ylabel('Velocity (m/s)', color='brown')
# Combine legends
lines = line1 + line2
labels = [l.get_label() for l in lines]
ax_pos.legend(lines, labels, loc='upper left')
ax_pos.grid(True, alpha=0.3)
# 7. Wheel torques
axes[3,0].plot(df['time_sec'].values, df['wheel_T_R'].values, 'b-', label='wheel_T_R', linewidth=2)
axes[3,0].plot(df['time_sec'].values, df['wheel_T_L'].values, 'r-', label='wheel_T_L', linewidth=2)
axes[3,0].set_title('Wheel Control Torques')
axes[3,0].set_ylabel('Torque (N*m)')
axes[3,0].set_xlabel('Time (s)')
axes[3,0].legend()
axes[3,0].grid(True, alpha=0.3)
# 8. Thigh torques
axes[3,1].plot(df['time_sec'].values, df['Tp_R'].values, 'b-', label='Right TP', linewidth=2)
axes[3,1].plot(df['time_sec'].values, df['Tp_L'].values, 'r-', label='Left TP', linewidth=2)
axes[3,1].set_title('Thigh Control Torques')
axes[3,1].set_ylabel('Torque (N*m)')
axes[3,1].set_xlabel('Time (s)')
axes[3,1].legend()
axes[3,1].grid(True, alpha=0.3)
plt.tight_layout()
# Print basic statistics
print(f"\n=== Basic Statistics ===")
print(f"Data duration: {df['time_sec'].iloc[-1]:.2f} seconds")
print(f"Right leg length: {df['leg_length_R'].mean():.4f} ± {df['leg_length_R'].std():.4f} m")
print(f"Left leg length: {df['leg_length_L'].mean():.4f} ± {df['leg_length_L'].std():.4f} m")
print(f"Right leg angle: {np.degrees(df['theta_R']).mean():.1f} ± {np.degrees(df['theta_R']).std():.1f} deg")
print(f"Left leg angle: {np.degrees(df['theta_L']).mean():.1f} ± {np.degrees(df['theta_L']).std():.1f} deg")
print(f"Right leg angular velocity: {np.degrees(df['dtheta_R']).mean():.1f} ± {np.degrees(df['dtheta_R']).std():.1f} deg/s")
print(f"Left leg angular velocity: {np.degrees(df['dtheta_L']).mean():.1f} ± {np.degrees(df['dtheta_L']).std():.1f} deg/s")
print(f"Right pitch angle: {np.degrees(df['phi_R']).mean():.1f} ± {np.degrees(df['phi_R']).std():.1f} deg")
print(f"Left pitch angle: {np.degrees(df['phi_L']).mean():.1f} ± {np.degrees(df['phi_L']).std():.1f} deg")
print(f"Right pitch angular velocity: {np.degrees(df['dphi_R']).mean():.1f} ± {np.degrees(df['dphi_R']).std():.1f} deg/s")
print(f"Left pitch angular velocity: {np.degrees(df['dphi_L']).mean():.1f} ± {np.degrees(df['dphi_L']).std():.1f} deg/s")
print(f"Body position: {df['x'].mean():.3f} ± {df['x'].std():.3f} m")
print(f"Body velocity: {df['dx'].mean():.3f} ± {df['dx'].std():.3f} m/s")
plt.show()
if __name__ == "__main__":
quick_plot()