
    j                         d Z ddlZddlZddlZddlZddlmZmZ ddlm	Z	 	 	 	 	 	 	 	 	 dde
dededede
de
dedefdZdS )z%Bimanual teleoperation implementation    N)TeleopInterfacebuild_interface)RobotController        leader/dev/hidraw4/dev/hidraw5Q?      @Fbimanualbilateral_kpcontrolspacemouse_path_rspacemouse_path_l	vel_scale	rot_scaleinvert_rotationarmsc                    |dk    }d}	t          ||||||          }
t          |
j        o||
j        o|	||	          |
                                 	 fd}t	          j        t          j        |           t	          j        t          j        |                                                                             |
	                               |

                               t          |
j                   	 j        rt          d           n?|
                              rt          j        d           nt          j        d           V|
                                                                t          d	           ng# t&          $ rZ}t          d
|            r(                                r                                 t-          j        d           Y d}~nd}~ww xY w|
                                 t3          j        d           dS # |
                                 t3          j        d           w xY w)z%Run the bimanual teleoperation systemr   T)r   r   r   r   r   )use_right_leaderuse_left_leaderuse_right_followeruse_left_followerc                 0                                      d S )N)emergency_stop)signumframerobot_controllers     :/home/robot-lab/cameron/raiden_fork/raiden/robot/teleop.pysignal_handlerz+run_bimanual_teleop.<locals>.signal_handler/   s    ++-----    z0
[FootPedal] Returning arms to home and exiting.g      ?g?z
Teleoperation session ended.z
Error:    Nr   )r   r   uses_leadersopensignalSIGTERMSIGINTsetup_for_teleop_recordingenable_estopsetupstartprintbannersession_estop_requestedpolltimesleepstopshutdown	Exception
has_robotsr   sysexitcloseos_exit)r   r   r   r   r   r   r   r   	use_rightuse_left	interfacer    er   s                @r   run_bimanual_teleopr?      st    
"IH!0++'" " "I '"/=I!.;8$"	   NN'	. 	. 	. 	. 	. 	fnn555fm^44433555%%'''()))()))i	7 IJJJ~~.// 
3JsOOO	 	'(((!!###.////   o!oo 	. 0 ; ; = = 	.++--- 	
 	
s2   D8F H 
G2AG-(H -G22H *I	)r   r   r   r	   r
   r   Fr   )__doc__r9   r%   r6   r0   raiden.controlr   r   raiden.robot.controllerr   floatstrboolr?    r!   r   <module>rG      s    + + 				  



  ; ; ; ; ; ; ; ; 3 3 3 3 3 3 ++!H HHH H 	H
 H H H H H H H H Hr!   