From cd9f8e204d8187977f0778de6e03e40390eb0b1d Mon Sep 17 00:00:00 2001
From: cyy_mac <cyy@mac.com>
Date: Sat, 25 Jul 2026 22:14:54 +0800
Subject: [PATCH] update monitor

---
 examples/monitor_state.py | 71 +++++++++++++++++++++++++++++++++------
 1 file changed, 61 insertions(+), 10 deletions(-)

diff --git a/examples/monitor_state.py b/examples/monitor_state.py
index 3872477..62e1838 100644
--- a/examples/monitor_state.py
+++ b/examples/monitor_state.py
@@ -26,6 +26,12 @@ def main():
     p.add_argument('--duration', type=float, default=30)
     p.add_argument('--rate', type=float, default=5, help='打印频率 Hz')
     p.add_argument('--verbose', '-v', action='store_true')
+    p.add_argument('--show-feet', action='store_true',
+                   help='显示 4 脚足端力 (raw ADC) + 估计值')
+    p.add_argument('--feet-only', action='store_true',
+                   help='只显示足端力, 不显示关节角 (聚焦脚力)')
+    p.add_argument('--imu-only', action='store_true',
+                   help='只显示 IMU + 姿态 (四元数/角速度/加速度/RPY)')
     args = p.parse_args()
     signal.signal(signal.SIGINT, sigint)
 
@@ -43,9 +49,19 @@ def main():
         last_print = 0
         n_decoded = 0
         damping = __import__('go1_pro_sdk').LowCmd().all_damping()
+        # 足端力统计
+        ff_hist = {'FR': [], 'FL': [], 'RR': [], 'RL': []}
+        ffest_hist = {'FR': [], 'FL': [], 'RR': [], 'RL': []}
 
-        print(f"\n{'时间':>6} {'电量':>5} {'FR_0':>8} {'FR_1':>8} {'FR_2':>8} {'rpy[°]':>20}")
-        print('-' * 70)
+        if args.imu_only:
+            print(f"\n{'时间':>6} {'四元数(w,x,y,z)':>44} {'gyro(°/s)':>28} {'acc(m/s²)':>28} {'RPY(°)':>28}")
+            print('-' * 140)
+        elif args.feet_only:
+            print(f"\n{'时间':>6} {'FR 右前':>10} {'FL 左前':>10} {'RR 右后':>10} {'RL 左后':>10}")
+            print('-' * 65)
+        else:
+            print(f"\n{'时间':>6} {'电量':>5} {'FR_0':>8} {'FR_1':>8} {'FR_2':>8} {'rpy[°]':>20}")
+            print('-' * 70)
 
         while running and time.time() - start < args.duration:
             client.send(damping)
@@ -54,18 +70,46 @@ def main():
             if state is None:
                 continue
             n_decoded += 1
+            ff = state.footForce
+            ffest = state.footForceEst
+            for i, leg in enumerate(['FR', 'FL', 'RR', 'RL']):
+                ff_hist[leg].append(ff[i])
+                ffest_hist[leg].append(ffest[i])
 
             now = time.time()
             if now - last_print >= 1.0 / args.rate:
                 t = now - start
-                rpy_str = (f"{math.degrees(state.imu.rpy[0]):+5.1f},"
-                           f"{math.degrees(state.imu.rpy[1]):+5.1f},"
-                           f"{math.degrees(state.imu.rpy[2]):+5.1f}")
-                print(f'{t:6.1f} {state.bms.SOC:4d}% '
-                      f'{state.motorState[0].q:+8.3f} '
-                      f'{state.motorState[1].q:+8.3f} '
-                      f'{state.motorState[2].q:+8.3f} '
-                      f'{rpy_str:>20}')
+                imu = state.imu
+
+                if args.imu_only:
+                    qw,qx,qy,qz = imu.quaternion
+                    gx,gy,gz = imu.gyroscope
+                    ax,ay,az = imu.accelerometer
+                    r,p,y = imu.rpy
+                    print(f'{t:6.1f} '
+                          f'({qw:+7.4f},{qx:+7.4f},{qy:+7.4f},{qz:+7.4f}) '
+                          f'({math.degrees(gx):+7.2f},{math.degrees(gy):+7.2f},{math.degrees(gz):+7.2f}) '
+                          f'({ax:+7.2f},{ay:+7.2f},{az:+7.2f}) '
+                          f'({math.degrees(r):+7.2f},{math.degrees(p):+7.2f},{math.degrees(y):+7.2f})')
+                elif args.feet_only:
+                    print(f'{t:6.1f}       {ff[0]:>5d}     {ff[1]:>5d}     '
+                          f'{ff[2]:>5d}     {ff[3]:>5d}')
+                else:
+                    rpy_str = (f"{math.degrees(state.imu.rpy[0]):+5.1f},"
+                               f"{math.degrees(state.imu.rpy[1]):+5.1f},"
+                               f"{math.degrees(state.imu.rpy[2]):+5.1f}")
+                    print(f'{t:6.1f} {state.bms.SOC:4d}% '
+                          f'{state.motorState[0].q:+8.3f} '
+                          f'{state.motorState[1].q:+8.3f} '
+                          f'{state.motorState[2].q:+8.3f} '
+                          f'{rpy_str:>20}')
+
+                if args.show_feet or args.feet_only:
+                    print(f"     🦶 足力: [FR={ff[0]:5d} FL={ff[1]:5d} "
+                          f"RR={ff[2]:5d} RL={ff[3]:5d}]  "
+                          f"估: [FR={ffest[0]:5d} FL={ffest[1]:5d} "
+                          f"RR={ffest[2]:5d} RL={ffest[3]:5d}]")
+
                 if args.verbose:
                     print(f'    SN={decode_sn(state.SN)} '
                           f'acc_z={state.imu.accelerometer[2]:.2f} m/s²  '
@@ -73,6 +117,13 @@ def main():
                 last_print = now
 
         print(f'\n解码 {n_decoded} 帧, 退出.')
+        if ff_hist['FR']:
+            print(f'\n🦶 足端力统计 (全程 {n_decoded} 帧):')
+            for leg in ['FR', 'FL', 'RR', 'RL']:
+                vals = ff_hist[leg]
+                vest = ffest_hist[leg]
+                print(f'  {leg}: 力 min={min(vals):5d} max={max(vals):5d} range={max(vals)-min(vals)}  '
+                      f'估 min={min(vest):5d} max={max(vest):5d} range={max(vest)-min(vest)}')
 
 
 if __name__ == '__main__':