@ -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 ( )
