From e9d37880d30279ed94e33bfbc8e576eb5b3072bd Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Thu, 19 Feb 2026 16:05:21 -0600 Subject: [PATCH] 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 --- teleop/teleop_hand_and_arm.py | 69 +++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index c0b71cf..45c406b 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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()