*iTH bdZddlZddlZddlZddlZddlmZmZddlmZddl m Z ddl m Z m Z mZddlmZddlZddlZddlmZmZdd lmZdd lmZdd lmZdd lmZeGd dZeGddZ GddZ!deedfdede"de#de#deef dZ$dS)z4Interactive recording of robot poses for calibrationN)asdict dataclass)datetime)Path)DictListOptional)quote)CALIBRATION_POSES_FILE CAMERA_CONFIG) CameraConfig)Camera)TeleopInterface)RobotControllerc`eZdZUdZdZeed<dZeed<dZe ed<dZ e ed<d Z e ed <d Z d S) ChArUcoBoardConfigz+Configuration for ChArUco calibration board squares_x squares_ygQ? square_lengthgZd;O? marker_length DICT_6X6_250 dictionaryc t|SN)rselfs 9/home/robot-lab/raiden_cmu/raiden/calibration/recorder.pyto_dictzChArUcoBoardConfig.to_dict!sd||N)__name__ __module__ __qualname____doc__rint__annotations__rrfloatrrstrrr rrrs{55IsIsM5 M5   $J$$$r rceZdZUdZeed<eed<dZee e ed<dZ ee e ed<dZ eed<dZ eed <d ZdS) CalibrationPosez4A single calibration pose with robot joint positionsidnameN follower_r follower_l timestampnotesc|j|j|j|jd}|j |j|d<|j |j|d<|S)Nr,r-r1r2r.r/)r,r-r1r2r.r/)rresults rrzCalibrationPose.to_dict0sQ'IZ    ? &#'?F< ? &#'?F<  r )r!r"r#r$r%r&r(r.r rr'r/r1r2rr)r rr+r+%s>> GGG III(,Je%,,,(,Je%,,,IsE3OOO     r r+c eZdZdZeedfdedededee fdZ dZ dd ed e fd Z d efd ZdZd efdZdZdded dfdZddZddZddZddefdZdS)CalibrationPoseRecorderz*Records robot poses for camera calibrationN interface output_filecamera_config_filecharuco_configc,||_t||_|jjdd||_|p t |_g|_d|_ g|_ i|_ i|_ tj|_dS)NT)parentsexist_ok)r8rr9parentmkdirr:rr;posesrobot_controllercalibration_targetscameras _camera_roles threadingEvent_rerun_stop_event)rr8r9r:r;s r__init__z CalibrationPoseRecorder.__init__As# ,, %%dT%BBB"4,D0B0D0D,. ;?.0 *, 79!*!2!2r ct|j}|ddu}|ddu}|r/|d}|j||r/|d}|j||dD]%}||jvr|j|&t |jjo||jjo||||_ |j |j |j |j |j dS)zBInitialize robots based on which wrist cameras are in camera.json. right_wristN left_wristscene)use_right_leaderuse_left_leaderuse_right_followeruse_left_follower) r r:get_camera_by_rolerCappendget_cameras_by_rolerr8 uses_leadersrBsetup_for_teleop_recordingsetupstart)rcfg has_righthas_leftcams rinitialize_robotsz)CalibrationPoseRecorder.initialize_robotsWss4233**=99E )),77tC  1((77C  $ + +C 0 0 0  1((66C  $ + +C 0 0 0**733 5 5C$222(//444 /!^8FY N7DH(& ! ! !  88::: T2333 T233333r r0r2returnct|j}tj}t |d|||}|j}d|vr|d|_ d|vr|d|_ |j ||S)zRecord the current robot posepose_r4r.r/) lenrArnow isoformatr+rBget_joint_positionstolistr.r/rS)rr2pose_idr1posejoint_positionss rrecord_current_posez+CalibrationPoseRecorder.record_current_posexsdj//LNN,,.. """    /CCEE ? * *-l;BBDDDO ? * *-l;BBDDDO $ r c|jr:|j}td|jd|jdStddS)z&Delete the most recently recorded poseu✓ Deleted pose : TzNo poses to deleteF)rApopprintr,r-)rdeleteds rdelete_last_posez(CalibrationPoseRecorder.delete_last_posesY : jnn&&G BgjBBGLBB C C C4 & ' ' '5r c |jstddStdt|jd|jD]G}td|jd|jd|j|jrtd|jHdS) zPrint all recorded poseszNo poses recorded yetNz Recorded z pose(s):z rkz - z Notes: )rArmrar,r-r1r2)rrgs r list_posesz"CalibrationPoseRecorder.list_posessz  ) * * * F 6C OO666777J 3 3D @tw@@$)@@@@ A A Az 31TZ11222 3 3r c<|j}|duS)zCheck if any leader button was pressed Returns: True if a button was pressed (rising edge), False otherwise N)rBcheck_button_press)rpressed_leaders rrsz*CalibrationPoseRecorder.check_button_presss# .AACCT))r cd|jd|jD|jdt jt jd}t|jd5}tj ||ddddn #1swxYwYtd t|jd |jdS) z Save recorded poses to JSON filez1.0c6g|]}|Sr))r).0rgs r z6CalibrationPoseRecorder.save_poses..s <<<dllnn<<rrKframe)sequencezcameras/g?)rrinit serve_grpcserve_web_viewerr rmrHis_setrDitemsr get_framecolorrEgetcv2rotate ROTATE_180set_timelogImagerwait) rrr server_uri viewer_url frame_idxr-r\rrimg_rgbs r _rerun_streamz%CalibrationPoseRecorder._rerun_streams $%%% ]Y777  XEBBBBTTTjr8R8R8RTT  0J00111 rx r rH r r) r r`i r r r     (//11 -!\//11   c xxzzE #  % -11$77=HH$'Jucn$E$EE"'111ddd "3 Gi@@@@0$00"(72C2CDDD D NI  " ' ' , , ,(//11 - - - - -sB>F FFc|jsdS|jtj|jd|_|jtdt|jddS)NT)targetdaemonu ✓ Rerun stream started (z camera(s))) rDrHclearrFThreadr _rerun_threadrXrmrars r_start_rerun_streamz+CalibrationPoseRecorder._start_rerun_streams|  F $$&&&&-T5GPTUUU   """ KS->->KKKLLLLLr c |jt|dr|jd|jD]}||jdS)Nrg@)timeout) rHsethasattrrjoinrDrcloser)rr\s r_stop_rerun_streamz*CalibrationPoseRecorder._stop_rerun_streams ""$$$ 4 ) ) 1   # #C # 0 0 0<&&((  C IIKKKK r  min_posescddl}ddl}ddl}ddl}t dt dt dt dj fd}||j |t dj D]}t d |t d t d j j d j jd t dj jdzddt dj jdzddt dj jt dt djjrt dnt dt djjrt dnt dt d|dt dt dt dt d t d!t d"jjp jj}|rt d#t d$t dt d%d} t.jr|t.j} t7j} d'| } | |kr | d(|d)z } n| d*z } t | t d+d,d&-d} | jjr't d.t=jd/d0} nPt.jr| r|t.jgggd1dr| t.j!t.j"d2} |#t.j|j$| | d3krtJ| d4ks| d5krEt d6d,d&-tM'(} n1| d7vr| } t d8| nnt=jd/| | d0kr)}t d9|j*d:|j+jj,r2|j,r+|j,dd;}t dd?|Dn| d@kr.n| dAkr/n| dBkr| |krt dC| dD|dEt dFd,d&-| r&|#t.j|j$| tM'(}|dGkrt dH0t dIj1n| dJkrt dt dt dt d t d!t d"|r*jjrt d#t dKn!t dL| t dMkn:#tJ$rt dN| r&|#t.j|j$| jr^t dOd,d&-tM'(}|dPkr0j1Ynytd$rm}t dQ|ddl3}|4jr2j5rj6Yd}~nd}~wwxYwj78jrj9dSdS#j78jrj9wwxYw)Rz&Run interactive pose recording sessionrNzG ======================================================================z, Raiden - Camera Calibration Pose RecordingzF======================================================================z* Opening cameras for live visualization...c:jdSr)rBemergency_stop)signumrrs rsignal_handlerzICalibrationPoseRecorder.run_interactive_recording..signal_handlers  ! 0 0 2 2 2 2 2r z Target Cameras:z - z ChArUco Board Configuration:z - Grid size: xz squaresz - Checker size: iz.1fz mmz - Marker size: z - Dictionary: z Instructions:z3 1. Position the ChArUco board in a fixed locationz? 2. Move the LEADER arms - followers will automatically followz/ 2. Move the robot arms using the input devicez@ 3. Position so the board is clearly visible from the camera(s)z> 4. Press button on any leader arm OR type 'r' to record posez 4. Type 'r' to record posez 5. Record at least z( diverse poses (vary distance and angle)z Commands:z r - Record current posez d - Delete last posez l - List recorded posesz q - Quit and savez h - Show helpz Button Input:z9 - Press button on any leader arm to record current posez Ready to record poses!Tz Poses recorded: z / z (minimum)u ✓ (minimum reached)z/ Waiting for input (button press or command)...r0)endflushz# Input received! Recording pose...g?rg?r  z Command [r/d/l/q/h]: )rdlqhz Command: u✓ Recorded pose rkz, Right follower position (first 3 joints): cg|]}|dSz.3fr)rwps rrxzECalibrationPoseRecorder.run_interactive_recording..sKdKdKd[\qJJKdKdKdr z+ Left follower position (first 3 joints): cg|]}|dSrr)rs rrxzECalibrationPoseRecorder.run_interactive_recording..sJcJcJcZ[a::JcJcJcr rrrz Warning: Only z poses recorded (minimum: z).zContinue anyway? [y/N]: yzContinuing recording...z% Calibration pose recording complete!rz, - Press button on any leader arm to recordzUnknown command: zType 'h' for helpz Interrupted by userz+Save recorded poses before exiting? [Y/n]: nz Error: ):selectsignaltermiosttyrmrrr]r8rSIGTERMrCr;rrrrrrUwaits_for_button_startrBleader_rleader_lsysstdinisatty tcgetattrrarApolltimesleepsetrawfilenoread tcsetattr TCSADRAINKeyboardInterruptinputstriplowerrir,r-r.r/rorqrshutdownr traceback print_exc has_robotsrrrcleanup)rrrrrrrr has_leaders old_settings poses_countstatuscommandcharrgposconfirmsaverrs` rrun_interactive_recordingz1CalibrationPoseRecorder.run_interactive_recordings5     o <=== h ;<<<    """      3 3 3 3 3  fnn555 !""". # #F /// " " " " ./// ed1; e ed>Q>[ e e e    T4#6#Dt#KTTTTUUU S$"5"Cd"JSSSSTTT A!4!?AABBB     CDDD > & E S T T T T C D D D PQQQ > 0 2 R S S S S 0 1 1 1 WI W W W    m )*** &''' )*** #$$$    +4V8M8V  O # $ $ $ M N N N o ())) 9     8",,SY77LM 0p /!$*oo ;k;;**9I9999FF66Ff Fo~**4+@AAEFFF 3"%y''))(l(!==#)b"dCCAF&JJsy'7'7'9'9:::#&9>>!#4#4D#-- # 7+>3355DEtwEE$)EEFFF,7DO"obqb1fKdKd`cKdKdKdff,7DO"obqb1eJcJc_bJcJcJcee^^))++++^^OO%%%%^^"Y..c{ccV_ccc8bMMMM'#-- # 7+>!";<<<$OO%%%BCCC)22444^^-(((566623335666/000+,,,"Nt~'LN/000LMMM7g77888-...ap /B"! - - - + , , , N!!#)W-> MMMz &CSWXXXXww}},,..3;;OO%%%  ! * * , , , , , 7 7 7 /a// " " "        ! ! !$ 7)>)I)I)K)K 7%44666 7 N " " "  # # % % %$ 0%--///// 0 0 N " " "  # # % % %$ 0%--//// 0s@ Q,]97d9B?b08d: b0A#b+&d+b00dAe)r0)r)r^N)r)r!r"r#r$r r rr(r rrIr]r+riboolrorqrsrr%rrrrrr)r rr7r7>s44 2"/7; 33"33 3 !!34 3333,444Bo.$ 3 3 3*D****LLL"3"----<MMMMX0X03X0X0X0X0X0X0r r7rr8rr9r:r;cXt||||}||dS)z/Main entry point for calibration pose recording)r8r9r:r;)rN)r7r)r8rr9r:r;recorders rrun_calibration_pose_recordingrsC'-% H  &&&;;;;;r )%r$rrrFr dataclassesrrrpathlibrtypingrrr urllib.parser rrerunrraiden._configr r raiden.camera_configr raiden.cameras.baserraiden.controlrraiden.robot.controllerrrr+r7r%r(rr)r rrs::  ))))))))'''''''''' @@@@@@@@------&&&&&&******333333            0X0X0X0X0X0X0X0X0z -+37 <<<<< < /0 <<<<<