Add --diag-log CSV diagnostic for head rotation compensation

Writes per-frame CSV to /tmp/teleop_diag.csv with 73 columns capturing
the full compensation pipeline: head pose, calibration reference,
R_head_delta, raw/compensated wrist positions, IK inputs/outputs, and
robot joint positions. Enabled with --diag-log flag.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

teleop/teleop_hand_and_arm.py

@@ -1,10 +1,12 @@
 import time
 import argparse
 import struct
+import csv
 import numpy as np
 from multiprocessing import Value, Array, Lock
 import threading
 import logging_mp
+from scipy.spatial.transform import Rotation as ScipyR
 logging_mp.basicConfig(level=logging_mp.INFO)
 logger_mp = logging_mp.getLogger(__name__)
 
@@ -95,6 +97,10 @@ def scale_rotation(R, alpha):
     rv = rotation_to_rotvec(R)
     return rotvec_to_rotation(rv * alpha)
 
+def mat_to_euler(R):
+    """Extract yaw, pitch, roll in degrees from a 3x3 rotation matrix."""
+    return ScipyR.from_matrix(R).as_euler('ZYX', degrees=True)
+
 class ThreadedWebcam:
     """Non-blocking webcam reader. Captures frames in a background thread
     so cv2.VideoCapture.read() never blocks the control loop."""
@@ -202,6 +208,8 @@ if __name__ == '__main__':
                         help='Rotation tracking blend (0=position-only, 1=full rotation, default: 0.7)')
     parser.add_argument('--head-rot-comp', type=float, default=1.0,
                         help='Head rotation compensation blend (0=none, 1=full, default: 1.0)')
+    parser.add_argument('--diag-log', action='store_true', default=False,
+                        help='Write per-frame diagnostic CSV to /tmp/teleop_diag.csv')
     # webcam
     parser.add_argument('--webcam', type=int, default=None,
                         help='USB webcam device index (e.g. 0 for /dev/video0). Bypasses teleimager.')
@@ -468,6 +476,29 @@ if __name__ == '__main__':
             logger_mp.info(f"[calibration] Initial reference captured. "
                            f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]}")
 
+        # Diagnostic CSV logger
+        diag_file = None
+        diag_writer = None
+        diag_frame = 0
+        if args.diag_log:
+            diag_file = open('/tmp/teleop_diag.csv', 'w', newline='')
+            diag_writer = csv.writer(diag_file)
+            diag_writer.writerow([
+                'frame', 'time',
+                'head_x', 'head_y', 'head_z', 'head_yaw', 'head_pitch', 'head_roll',
+                'cal_head_yaw', 'cal_head_pitch', 'cal_head_roll',
+                'delta_yaw', 'delta_pitch', 'delta_roll',
+                'raw_L_x', 'raw_L_y', 'raw_L_z', 'raw_R_x', 'raw_R_y', 'raw_R_z',
+                'comp_L_x', 'comp_L_y', 'comp_L_z', 'comp_R_x', 'comp_R_y', 'comp_R_z',
+                'delta_L_x', 'delta_L_y', 'delta_L_z', 'delta_R_x', 'delta_R_y', 'delta_R_z',
+                'ref_L_x', 'ref_L_y', 'ref_L_z', 'ref_R_x', 'ref_R_y', 'ref_R_z',
+                'ik_L_x', 'ik_L_y', 'ik_L_z', 'ik_R_x', 'ik_R_y', 'ik_R_z',
+                'q0','q1','q2','q3','q4','q5','q6','q7','q8','q9','q10','q11','q12','q13',
+                'sq0','sq1','sq2','sq3','sq4','sq5','sq6','sq7','sq8','sq9','sq10','sq11','sq12','sq13',
+                'settle_alpha',
+            ])
+            logger_mp.info("[diag] CSV logging to /tmp/teleop_diag.csv")
+
         # main loop. robot start to follow VR user's motion
         while not STOP:
             start_time = time.time()
@@ -638,6 +669,7 @@ if __name__ == '__main__':
                     R_head_delta = tele_data.head_pose[:3, :3] @ head_R_at_cal.T
                     R_comp = scale_rotation(R_head_delta.T, args.head_rot_comp)
                 else:
+                    R_head_delta = np.eye(3)
                     R_comp = np.eye(3)
 
                 # De-rotate wrist positions, then compute delta from calibration.
@@ -741,6 +773,36 @@ if __name__ == '__main__':
             sol_q, sol_tauff = arm_ik.solve_ik(left_wrist_adjusted, right_wrist_adjusted, current_lr_arm_q, current_lr_arm_dq)
             time_ik_end = time.time()
             logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}")
+
+            # Diagnostic CSV: write one row per frame
+            if diag_writer is not None and calibrated:
+                he = mat_to_euler(tele_data.head_pose[:3, :3])
+                ce = mat_to_euler(head_R_at_cal) if head_R_at_cal is not None else [0,0,0]
+                de = mat_to_euler(R_head_delta) if head_R_at_cal is not None else [0,0,0]
+                diag_writer.writerow([
+                    diag_frame, f'{now - calibration_time:.3f}',
+                    *[f'{v:.4f}' for v in tele_data.head_pose[:3, 3]],
+                    *[f'{v:.2f}' for v in he],
+                    *[f'{v:.2f}' for v in ce],
+                    *[f'{v:.2f}' for v in de],
+                    *[f'{v:.4f}' for v in tele_data.left_wrist_pose[:3, 3]],
+                    *[f'{v:.4f}' for v in tele_data.right_wrist_pose[:3, 3]],
+                    *[f'{v:.4f}' for v in left_pos_comp],
+                    *[f'{v:.4f}' for v in right_pos_comp],
+                    *[f'{v:.4f}' for v in left_delta_pos],
+                    *[f'{v:.4f}' for v in right_delta_pos],
+                    *[f'{v:.4f}' for v in vp_ref_left[:3, 3]],
+                    *[f'{v:.4f}' for v in vp_ref_right[:3, 3]],
+                    *[f'{v:.4f}' for v in left_wrist_adjusted[:3, 3]],
+                    *[f'{v:.4f}' for v in right_wrist_adjusted[:3, 3]],
+                    *[f'{v:.4f}' for v in current_lr_arm_q],
+                    *[f'{v:.4f}' for v in sol_q],
+                    f'{settle_alpha:.3f}',
+                ])
+                diag_frame += 1
+                if diag_frame % 150 == 0:
+                    diag_file.flush()
+
             arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff)
 
             # record data
@@ -927,6 +989,13 @@ if __name__ == '__main__':
         except Exception as e:
             logger_mp.error(f"Failed to stop keyboard listener or ipc server: {e}")
         
+        if diag_file is not None:
+            try:
+                diag_file.close()
+                logger_mp.info(f"[diag] Wrote {diag_frame} frames to /tmp/teleop_diag.csv")
+            except:
+                pass
+
         if webcam_cap is not None:
             try:
                 webcam_cap.release()