jUdZddlZddlZddlZddlZddlZddlmZddl m Z m Z m Z m Z e rddlmZddlZddlmZddlZddlZddlZddlZddlZddlmZejdejeje dddd dd l!m"Z"dd l#m$Z$dd l%m&Z&dd l'm(Z(ddl)m*Z+ddl)m,Z,ddl-m.Z.ddlm/Z/e"j0Z1dDdZ2e2e"_0gdZ3ej4gdZ5ej4gdZ6ej4gdZ7ej4gdZ8dZ9dZ:e;ee+<dZ=e;ee+j>dz Z?ej4gdgdgdgd gej@!ZAejBeCd"<ej4gd#gd$gdgd gej@!ZDejBeCd%<daEd&ZFd'ZGd(eHfd)ZIGd*d+ZJ dEd.e(d/ejBd0e ejBd1eHd2eKd3e ejLd4dfd5ZMd4eNe;fd6ZOdFd8e;d9eKd4ePfd:ZQd8e;d4ePfd;ZR dGd=ejBd>eHd?eHd@ePd4ejBf dAZSGdBdCZTdS)Hz6Modular robot controller for YAM bimanual robot systemN)Path) TYPE_CHECKINGDictOptionalTuple) FootPedal)Rotationz.. third_partyi2rt) CanInterface) get_yam_robot)MotorChainRobot)Robot)ARM_YAM_XML_PATH) GripperType) jparse_step)try_open_footpedal PCAN_USBBUS1cZt||g|Ri|t|ds ||_dSdS)Nchannel)_orig_can_interface_inithasattrr)selfrargskwargss >/home/robot-lab/cameron/raiden_fork/raiden/robot/controller.py_patched_can_interface_initr+sIT7._pkg_handlersK eS ! ! Me&6&67J&K&K M==!4o6KLL L rFT)filename_handler load_meshesload_collision_meshesN)yourdfpyURDFload_YAM_URDF_PATH)rFs r_load_yam_urdfrOsC =  %"   rdtc t}tj|}t |jjd}tj td}tj dtj }||||tj dtj tjgdtj d|| \}}tj||||fS) a Load YAM robot with pyroki, JIT-compile jparse_step, and run warmup. Args: dt: Control loop period in seconds. Returns: (pk_robot, link6_idx, step_jit) where step_jit is the JIT-compiled IK step function ready for real-time calls. link_6r3r5r-r0jparserobotcfgtarget_link_indextarget_position target_wxyzr4rPhome_cfg)rOpkr from_urdflistlinksnamesindexr8r9rnpzerosfloat64arrayblock_until_ready)rPurdfpk_robot link6_idxstep_jit_dummyresult_s r _setup_pyrokiros   Dx!!$''HX^)**00::Iw{K@@@HXarz * * *F #"*555H111DDD    IFA&!!! Y ((rceZdZdZdefdZdeejejffdZ de dejfdZ dejfdZ d ejdd fd Z d ejd ejdd fdZd S)rz-Wrapper for leader robot with teaching handlerWc,||_|j|_dSrJ)_robot motor_chain _motor_chain)rrWs r__init__zYAMLeaderRobot.__init__s !-rreturnc|jd}|j}t jdd|djz }tj||gg}||dj fS)z1Get joint positions with gripper and button state joint_pos{Gz?r) rrget_observationsrtget_same_bus_device_statestimesleeppositionrc concatenate io_inputs)rqpos encoder_obs gripper_cmdqpos_with_grippers rget_infozYAMLeaderRobot.get_infosx{++--k:'BBDD  4+a.11 ND;-+@AA +a.":::r encoder_idxcN|j}||jS)z/Get io_inputs for the encoder at *encoder_idx*.)rtr|r)rrrs rget_encoder_iozYAMLeaderRobot.get_encoder_ios$'BBDD ;'11rc4|jS)zGet joint positions (6 DoF))rr get_joint_posrs rrzYAMLeaderRobot.get_joint_poss{((***rrxNc`|jddksJ|j|dS)z0Command joint positions (6 DoF only, no gripper)rrSN)shaperrcommand_joint_pos)rrxs rrz YAMLeaderRobot.command_joint_poss6q!Q&&&& %%i00000rkpkdc<|j||dS)zUpdate PD gainsN)rr update_kp_kd)rrrs rrzYAMLeaderRobot.update_kp_kds    R(((((r)__name__ __module__ __qualname____doc__rrurrcndarrayrintrrrrr:rrrrs77.o....;% BJ 67;;;;2#2"*2222 +rz++++12:1$1111 )rz)rz)d))))))rrr(rWtarget_joint_positionsstart_joint_positionstime_interval_ssteps stop_eventrvc|!tj|tj}n|}t |t |ksJt |dzD]Z}||rdS||z }d|z |z||zz} || tj ||z [dS)aMove the robot to target joint positions with smooth interpolation. Args: start_joint_positions: Starting configuration for the interpolation. Pass the previous *commanded* target (not the measured position) so the trajectory matches what was executed during data collection. Falls back to ``robot.get_joint_pos()`` when ``None`` (e.g. first step). Nr-rz) rcasarrayrerlenrangeis_setrr}r~) rWrrrrr current_posialpha target_poss rr!r!s (j!6bjIII ))++ {  s#9:: : : : : 519  ,,  !j&7&7&9&9 ! FFE %i;.9O1OO   +++ ?U*++++ ,,rctjgdddd}g}|jD]}|d}t |dkr]|ddd }|d r|||S) z1Return all CAN interface names visible to the OS.)ipz-olinkshowTFcapture_outputtextcheck:rz@rcan) subprocessrunstdout splitlinessplitrstriprAappend)rm interfaceslinepartsnames rlist_can_interfacesrs ^$$$  F J ((**(( 3 u::??8>>##))#..q1Du%% (!!$''' r@B interfacebitratec tjdkrgndg}|ddd|dgz|ddd|ddd d t|g zfD]}}tj|d d d }|jdkrXt dd||jr)t d|j d S~d S)zrBring a CAN interface down then back up at the given bitrate. Returns True on success, False on failure. rsudorrsetdownuptyperrTFru ✗  z ) osgeteuidr@rr returncodeprintjoinstderrr)rrrrrms rreset_can_interfacers ""22D feY77         LL  TERRR   ! ! +388D>>++ , , ,} 64V]00224455555 " 4rc tjddd|gddd}|jdkrdSd|jvs d |jvrdSt d |d dS#t $r }t d |d |Yd}~dSd}~wwxYw)z0Check if a CAN interface exists and is availablerrrTFrrzstate UPz state UNKNOWNzWarning: CAN interface z exists but is not UPzError checking CAN interface z: N)rrrrr Exception)rrmes rcheck_can_interfacer$s 669 -       ! !5  & &/V]*J*J4 LILLL M M M5  >i>>1>>???uuuuus!'AAA A>A99A>F T_current vel_scale rot_scaleinvert_rotationc\|j|z}|j |z}|j|z}|j|z}|j|z} |j |z} |} | dddfxx|||gz cc<tjd|| | g |ddddfz| ddddf<| S)uConvert a SpaceMouse state into a target EE pose. Translation and rotation are both applied in the world (Cartesian) frame. Axis mapping (tuned in test_spacemouse_sim.py): SpaceMouse y → world +x SpaceMouse x → world −y SpaceMouse z → world +z roll/pitch/yaw → world-frame roll/pitch/yaw Args: state: pyspacemouse state object (has .x .y .z .roll .pitch .yaw). T_current: 4×4 current EE pose in the arm base frame. vel_scale: Max translational speed in m/s at full deflection. rot_scale: Max rotational speed in rad/s at full deflection. invert_rotation: If True, negate all rotation axes. Returns: 4×4 target EE pose. NrTxyz) yxzrollpitchyawcopyr from_euler as_matrix) staterrrrdxdydzdrxdrydrzT_targets rspacemouse_to_target_poser;s: 9 B 'I B 9 B *y C + !C 9*y C~~H RaRUOOOB|#OOOECc?33==??)BQBPRQRPRFBSS RaR!V Orc eZdZdZ d?dedededefdZdefd Zd@d edd fd ZdAdZdAdZ dBdedd fdZ de e e e e jfffdZde e ee jffdZde e e jffdZdee fdZdee fdZdefdZdefdZdAdZdAdZdCdedd fdZdAdZedefdZd e fd!Zd"Zd#Z dDd&e d'e dd fd(Z!dEd*edd fd+Z" dFd.ed/ed*ed0edd f d1Z#dAd2Z$ d@d e d.ed/ed*ed0edd f d3Z%dAd4Z& dGd5ee d6ed7ee'dd fd8Z(d9Z)dAd:Z*dAd;Z+d<Z,d=Z-dAd>Z.d S)Hr z?Manages YAM robot initialization, state access, and terminationTuse_right_leaderuse_left_leaderuse_right_followeruse_left_followerc8||_||_||_||_d|_d|_d|_d|_i|_i|_ i|_ i|_ i|_ d|_ d|_d|_t!j|_d|_ddd|_dS)aInitialize robot controller Args: use_right_leader: Initialize right leader arm use_left_leader: Initialize left leader arm use_right_follower: Initialize right follower arm use_left_follower: Initialize left follower arm Nr$Frightleft)rrrrleader_rleader_l follower_r follower_llast_button_state_last_verdict_state_last_failure_button_statekp_gainskd_gains _pause_until _pause_timer_estop_enabled threadingEvent_session_estop_event _footpedal_last_commanded_pos)rrrrrs rruzRobotController.__init__js!1."4!237 26 +/+/4657 <>'02 /1 $'7;%*%.O$5$5!26 E E    rrvcg}|jr|d|jr|d|jr|d|jr|dg}|D]&}t |s||'|r%t dd|tddS) z.Check if required CAN interfaces are availablecan_follower_rcan_follower_l can_leader_r can_leader_lz'Missing or unavailable CAN interfaces: z, u-✓ All required CAN interfaces are availableT) rrrrrr RuntimeErrorrr)rrequired_interfacesmissing_interfacesrs rcheck_can_interfacesz$RobotController.check_can_interfacess   " 9  & &'7 8 8 8  ! 9  & &'7 8 8 8   7  & &~ 6 6 6   7  & &~ 6 6 6, 5 5I&y11 5")))444  Y$))DV:W:WYY  =>>>trFgravity_comp_modeNc tdi idtdtddf fd }g}|jr7|t j|ddt jfd |jr7|t j|d d t jfd |j r7|t j|d dt j fd |j r7|t j|ddt j fd |D]}| |D]}| rtd|jru d|_|jt"t$t"|jd<t$|jd<|jru d |_|jt"t$t"|jd<t$|jd<d}|j r d }||_t1||_d|jd<d|jd<d|jd<d|jd<|jjj|jd<|jjj|jd<|j r d}||_t1||_ d|jd<d|jd<d|jd<d|jd<|j jj|jd<|j jj|jd<tdtdt"tdt$|r|!dSdS) z|Initialize robots Args: gravity_comp_mode: If True, disable position control for free movement z Initializing robots...rrrvNctd|d t||d|<dS#t$r}||<Yd}~dSd}~wwxYw)Nz - Initializing ...F)r gripper_typezero_gravity_mode)rr r)rrrexcerrorsresultss r_initz0RobotController.initialize_robots.._inits} /d/// 0 0 0 # -#!-&+!!!  # # #"t  #s- AAAzright followerr Ttargetrdaemonz left followerr z right leaderr z left leaderr zRobot initialization failed: rrrrg?r$r leader_r_topleader_r_bottomr leader_l_topleader_l_bottomu✓ All robots initializedz Follower kp: z Follower kd: )"rr@rrrThreadr LINEAR_4310rrYAM_TEACHING_HANDLErstartrr rr FOLLOWER_KP FOLLOWER_KDrrrrgravity_comp_factorrrrrrrr_kp_kdrenable_gravity_compensation) rrrthreadst_LEADER_GRAVITY_COMP_FACTOR leader_r_base leader_l_baserrs @@rinitialize_robotsz!RobotController.initialize_robotssp ()))&(') # #c #D # # # # # # #  "  NN  *,UV     !  NN  )+;[=TU      NN  &&#7         NN  %&#7       A GGIIII  A FFHHHH  IGvGGHH H  " =%&67DO O ( (KK ( H H H*5*:*:*<*.move_to_homeesT-D---..."5*cQTUUUU0T00011111rright_follower)rr left_follower right_leader left_leaderz - Moving right follower...z - Moving left follower...z - Moving right leader...z - Moving left leader...u✓ All arms at home positionsN)rrrrr#r"rrrrr#rr&rr!)rr7r-r<threads rmove_to_home_positionsz&RobotController.move_to_home_positionsZsc 6777 6 JG 2 2 2  $+"o/@BRS  $+"o/@/R } $+"m2O^T } $+"m2O]S"   !     G4555"4?4EFFF G3444"4?4EFFF} J2333"4=#7III} J1222"4=#7III ./////rc6i}|jr!|jj|d<|jr!|jj|d<|jr|j|d<|jr|j|d<|S)zGet full observations (joint_pos, joint_vel, joint_torque) from all robots. Returns: Dict mapping robot name to observation dict. Leaders return 6-DoF arrays; followers return 7-DoF (arm + gripper). rrrr)rrrr{rrr)robss rget_all_observationsz$RobotController.get_all_observationss13 = F"m2CCEEC O = F"m2CCEEC O ? C $ @ @ B BC  ? C $ @ @ B BC  rc|jd|jdnd|jd|jdnddS)a7Return the last 7-DOF position commanded to each follower arm. Returns a dict with keys ``"follower_r"`` and ``"follower_l"``, each mapping to a ``(7,)`` float32 array or ``None`` if no command has been issued yet (e.g. at the start of an episode before the first teleop step). rNr)rr)rrrs rget_last_commanded_positionsz,RobotController.get_last_commanded_positionssm'0<27;@@BBB'/;26:??AAA    rc"i}|jr|j|d<|jr|j|d<|jr|j|d<|jr|j|d<|S)zGet current joint positions of all robots Returns: Dictionary mapping robot names to joint positions rrrr)rrrrr)r positionss rget_joint_positionsz#RobotController.get_joint_positionss  ? F&*o&C&C&E&EIl # ? F&*o&C&C&E&EIl # = B$(M$?$?$A$AIj ! = B$(M$?$?$A$AIj !rcf|jrQ|j\}}|d}|dkr|jddkr ||jd<dS||jd<|jrQ|j\}}|d}|dkr|jddkr ||jd<dS||jd<dS)zCheck if any leader button was pressed (rising edge detection) Returns: Name of leader that was pressed ("leader_r" or "leader_l"), or None rr*rrN)rrrr)rrn button_statecurrent_buttons rcheck_button_pressz"RobotController.check_button_presss = @"m4466OA|)!_N##(>z(JS(P(P5C&z2!z1?D ": . = @"m4466OA|)!_N##(>z(JS(P(P5C&z2!z1?D ": .trcd|jfd|jffD]\}}| |\}}t|dkrt |dnd}t|dkrt |dnd}|j|dd}|j|dd}||j|d<||j|d<|d kr |d krd S|d kr |d krd S#t$rYwxYwdS) a&Check if a verdict button was pressed on any leader arm. The top button (io_inputs[0]) signals "success". The bottom button (io_inputs[1]) signals "failure". Uses rising-edge detection per button. Returns: "success", "failure", or None. rrNrr$rz_top_bottomr*successfailure)rrrrfloatrgetr) rrleaderrnrtopbottomprev_top prev_bottoms rcheck_verdict_buttonz$RobotController.check_verdict_buttonsX)$-8:t}:UV  LD&~ %00 9-0^^a-?-?eIaL)))S03I0B0By|,,,3774 sKK"6::d;K;K;KSQQ :=(D7=C(D)9)9)9:99C$99C<#   (   $ $ & & & $D    ) (rr(durationc>|jsdStj|z|_t d|dd|j|jtj||j |_d|j_ |j dS)aqFreeze all arms at their current positions for *duration* seconds, then call return_to_home(). Safe to call from any thread (e.g. the footpedal callback thread). Pressing the pedal again while paused cancels the running timer and resets the countdown. No-op when recording is not active (i.e. _estop_enabled is False). Nu3 [FootPedal] Soft pause — holding all arms for z.0fzs, then returning to homeT) rr} monotonicrrrrerTimer_on_pause_expiredrr&)rrgs r soft_pausezRobotController.soft_pause>s"  F N,,x7  %8 T % % %     (   $ $ & & &%OHd6LMM#'  !!!!!rcdd|_td|jdS)uCalled by the soft-pause timer thread after the hold duration elapses. Only signals the main thread to exit — robot operations (return_to_home, close) are intentionally left to the main thread to avoid racing on the CAN bus with check_button_press(). Nu9 [FootPedal] Hold complete — signalling session stop.)rrrrrs rrkz!RobotController._on_pause_expiredVs6! JKKK !%%'''''rc4|jS)zTrue after the footpedal has triggered a soft-pause and the arms have returned home. Signals the recording loop to exit cleanly.)rrrs rsession_estop_requestedz'RobotController.session_estop_requestedas(//111rsidecx|dkr|j}|j}d}n|j}|j}d}|r|sdSd}d}d}|d}|js tj|j k} | r|s`|}|}||j vr-| |j ||j |d}| || |tjd |jrdS|\} } | | ddt#t%j| dd d }t%j| dd|} | | | |j|<tjd n-#t,$r } t/d |d | Yd} ~ dSd} ~ wwxYw|jdSdS)z+Run teleoperation control loop for one siderrrNFrSrTryr$r%z - Error in z teleoperation: )rrrrr_teleop_shutdownrr}rirrrrrr~rrrTrccliprrrr)rrprVfollower leader_namehold_leader_poshold_follower_pos was_pausedfollower_gripper_cmd now_paused leader_posrn follower_cmdrs r_teleoperation_control_loopz+RobotController._teleoperation_control_loopgs 7??]FH$KK]FH$K X  F0426 '5577:'..00) ( !^--0AA % **0*>*>*@*@,4,B,B,D,D)&$-77"//#'=#=#'=#=0&* ,,_===../@AAAJt$$$,3355E!' 1 1 A((BQB888(-RWZ]C-M-M'N'N$!yBQB9MNN **<8881=(. 4       ?d??A??@@@ O'..00) ) ) ) ) s&)B>G1(G1B-G11 H;HHctj|_g|_|jrR|jrKtj|jddd}||j ||j rR|j rKtj|jddd}||j ||jr%tj dtdd Sd S) z=Start teleoperation control loops (followers follow leaders).rz teleop-rightTrrrrrz teleop-left皙?u8✓ Teleoperation active (followers will follow leaders)N)rrrr_teleop_threadsrrr#r}r&rrrr}r~rrrAs rstart_teleoperationz#RobotController.start_teleoperations!* 1 1! = 0T_ 0%7# F LLNNN  ' ' / / / = 0T_ 0%7" F LLNNN  ' ' / / /   N JsOOO L M M M M M N Nrct|drb|jt|dr9|jD]}|d|j|dS)zFStop all teleoperation control loops (leader-follower and SpaceMouse).rrrr%timeoutN)rrrrrrclearstop_spacemouse_teleoprs rstop_teleoperationz"RobotController.stop_teleoperations 4+ , , -  ! % % ' ' 't.// -"2--FKKK,,,,$**,,, ##%%%%%r /dev/hidraw4 /dev/hidraw5path_rpath_lctd|j,tj||_td|d|j.tj||_td|ddSdS)aOpen two SpaceMice for Cartesian velocity teleop. Uses ``open_by_path`` so each handle unambiguously refers to one physical device regardless of how many HID interfaces each exposes. Run ``uv run scripts/test_spacemouse_read.py --list`` to see the hidraw paths for your connected devices. Args: path_r: hidraw path for the right-arm SpaceMouse (e.g. /dev/hidraw4). path_l: hidraw path for the left-arm SpaceMouse (e.g. /dev/hidraw5). zOpening SpaceMice...Nu ✓ SpaceMouse R ()u ✓ SpaceMouse L ()rr pyspacemouse open_by_path _spacemouse_rr _spacemouse_l)rrrs rattach_spacemicez RobotController.attach_spacemices $%%% ? &!-!:6!B!BD  2222 3 3 3 ? &!-!:6!B!BD  2222 3 3 3 3 3 ' &rryrPci_itjdtddffd }g}j*|tj|ddj*|tj|dd|D]}||D]}| rtd dS) aPre-warm the pyroki/J-PARSE JIT for both SpaceMouse arms. Runs ``_setup_pyroki`` for each arm in parallel background threads and blocks until both finish. Call this *before* showing the READY prompt so that no JIT compilation happens after the user starts recording. Stores results in ``self._prewarmed_ik`` keyed by side ("right"/"left"). rprvNc t}5|j|<ddddS#1swxYwYdS#t$r0}5||<dddn#1swxYwYYd}~dSYd}~dSd}~wwxYwrJ)ro _prewarmed_ikr)rprmrrPrlockrs r_warmz3RobotController.warmup_spacemouse_ik.._warmsK '&r**66/5D&t,666666666666666666 ' ' '''#&F4L''''''''''''''''''''''''''' 'sQ9 , 90909 A3A.A A.A A.A A..A3rTrrzSpaceMouse IK warmup failed: ) rrLockr@rrr#rr&rr )rrPrr-r.rrs`` @@rwarmup_spacemouse_ikz$RobotController.warmup_spacemouse_iks&$&~ ' ' ' ' ' ' ' ' ' ' ' ? & NN9+5zRVWWW X X X ? & NN9+5yQUVVV W W W  A GGIIII  A FFHHHH  IGvGGHH H I Ir皙?r*rrrc *tdstdstdtj_g_ddd_tj_dtddfdtddffD]Y\}}|||fdfd }tj |d |d }| j |Zdtddj fdtddjffD]_\}}} | | tj j||||||fd |d }| j |`t!ddS)ukStart SpaceMouse Cartesian velocity teleop threads. SpaceMouse axis values (−1 to 1) are treated as velocities: each step the EE pose is advanced by ``axis_value * scale * dt`` in the current EE frame, then differential IK maps the new target pose to joint angles. Args: vel_scale: Max translational speed in m/s at full deflection. rot_scale: Max rotational speed in rad/s at full deflection. dt: Control loop period in seconds (default 50 Hz). invert_rotation: If True, negate all rotation axes. rrz9Call attach_spacemice() before start_spacemouse_teleop().NrrrrvcjsX|}j5|j|<dddn #1swxYwYjVdSdSrJ)_spacemouse_shutdownrread_spacemouse_state_lock_spacemouse_states)sdrrs r_readerz8RobotController.start_spacemouse_teleop.._reader3s3::<<;FFHHE4;;5:/2;;;;;;;;;;;;;;;3::<<;;;;;s A  AAzspacemouse-reader-T)rrrz spacemouse-ru✓ SpaceMouse teleop activervN)rr rrr_spacemouse_threadsrrrgetattrr#r&rrr_spacemouse_control_loopr) rrrrPrrpdevicerr.rts ` rstart_spacemouse_teleopz'RobotController.start_spacemouse_teleop s&t_-- gdO6T6T K %.O$5$5!)+ 26(E(E&/n&6&6#gdOT:: ; WT?D99 :  / /LD&~& ; ; ; ; ; ; ;  %@$%@%@A GGIII  $ + +A . . . .gdOT::DO L WT?D994? K' / / "D&(6> 4FIy"oN)4)) A GGIII  $ + +A . . . . ,-----rct|drd|jt|dr=|jD]}|d|jdSdSdS)zStop SpaceMouse teleop threads.rrr%rN)rrrrrr)rr.s rrz&RobotController.stop_spacemouse_teleopPs 4/ 0 0 1  % ) ) + + +t233 11((AFF3F''''(..00000  1 1 1 1rc\|dkr|jn|j}t|di|}||\n=t d|ddt \t d|ddt jd t j d t j d t j ffd } d t j dt j d t j ffd } d} d} | } | dd ddd }| d }|j s tj}tj|jk}|rT| s(| } d} || tj|j rdS| rA| }|dd ddd }|d }d} |j5|j|}dddn #1swxYwY|tjQ| d }| |}t-|||||}| ||}|}t|dg}t/|dkr$|drt1d|dt2zz }n6t/|dkr#|drt5d|dt2zz}t j|ddd|}||| |j|<tj|z }|z }|dkrtj|n@#t:$r3}t d|d|tjYd}~nd}~wwxYw|j dSdS)uVelocity integration + J-PARSE IK + command loop for one arm. Delegates axis mapping and target pose computation to ``spacemouse_to_target_pose()`` (world-frame translation, EE-frame rotation). Uses pyroki + J-PARSE for singularity-aware velocity IK. The JIT-compiled step function is warmed up here before the loop starts so the first command is not delayed by JAX tracing. Gripper: Button 0 → close (0.0), Button 1 → open (1.0). rrNz [SpaceMouse z'] Setting up J-PARSE IK (JIT warmup)...T)flushz ] IK ready.rSr-qrvctj|}tjt j|}|tzS)uBFK of the grasp_site: link_6 pose × fixed offset → 4×4 matrix.) forward_kinematicsjnprrcrfjaxlieSE3rr/)rposesT_link6rjris r_fk_tcpz9RobotController._spacemouse_control_loop.._fk_tcp|sS// A??Ehvz% *:;;EEGGHHG_, ,r T_target_tcpc |tz}|dddf}tj|ddddf}t j|d|d|d|dg} |tj ||d \}}t j|S)u9One J-PARSE step: grasp_site target → new joint config.NrTrrzrrUrV) r1r from_matrixas_quatrcrfastyperer) rrT_target_link6rxyzwr[q_newrn _home_cfgrPrjrirks r_ik_stepz:RobotController._spacemouse_control_loop.._ik_steps*O;N'A.J'rr2A2v(>??GGIID(DGT!Wd1gtAw#GHHKxHHRZ(("+ *'"   HE1:e$$ $rFbuttonsrr$g?rzr%z SpaceMouse z loop error: )rrrrUrrorcrdrerrrrrr}rirrr~rrrrrmax_GRIPPER_SAFETY_THRESHOLDminrrr) rrprrrrPrrt prewarmedrrhold_posrx q_full_initq_armgripper loop_startrz q_full_resyncrgripper_actualrrrcmdelapsed remainingrrrjrirks ` @@@@rrz(RobotController._spacemouse_control_loopYs('+goo4??4?D/266::4@@  ,5 )Hi NNNN    -:",=,= )Hi 44444D A A A AHQbj111  -rz -bj - - - - - - -  %  %"* % % % % % % % % % % %&*. ,,.. BQB"%**,,a.+2244F E !^-- "^--0AA %*#+#9#9#;#;#@#@#B#B%) ..x888JrNNN,3355E/$,$:$:$<$> 377==E>>>>>>>>>>>>>>>=JrNNN"*!7!7!9!9!!<#GENN 49iO !11)!%B77w<.cycles`1XX  "5*cQSTTTT",c  rTr) rr"rrrrrr#rr&r)rrr-r.rs @rsignal_ready_with_grippersz*RobotController.signal_ready_with_gripperss EFFF&++--  1   4       ?  NN T_4FtTTT    ?  NN T_4FtTTT     A GGIIII  A FFHHHH  r device_path direct_estopcallbackcDddlmt|_j{jfdn?|rjfdnjfdjdSdS)uAttach a footpedal for soft e-stop. Optional — warns and continues if absent. Only the LEFT pedal triggers the e-stop action. The middle and right pedals are reserved for success/failure verdict marking in recorder.py and must not trigger a robot e-stop. Args: device_path: explicit /dev/input/eventN. Auto-detected if None. direct_estop: if True, pressing the left pedal calls ``emergency_stop()`` immediately (skips the 5-second ``soft_pause`` timer). Use this in contexts that have no teleoperation loop (e.g. the policy server). callback: if provided, called (with no arguments) when the left pedal is pressed, overriding both ``direct_estop`` and ``soft_pause`` behaviour. Use this to inject pre-estop bookkeeping (e.g. setting a server-level flag) before the robot stop sequence runs. r) PEDAL_LEFTNc(|kr ndSrJr:)coderrs rz2RobotController.attach_footpedal..&stz/A/Atrc<|krndSrJ)emergency_stoprrrs rrz2RobotController.attach_footpedal..*s#$*:L:L!4!4!6!6!6RVrc@|krdndS)Nr()rg)rlrs rrz2RobotController.attach_footpedal...s)9=9K9K555QUr)raiden.robot.footpedalrrron_pressr&)rrrrrs` `@rattach_footpedalz RobotController.attach_footpedals0 655555,[99 ? &#((KKKKK ((VVVVV(( O ! ! # # # # # ' &rcjtd||d|dtd|jr@|jt jdt jd|jr@|jt jdt jdtd | td | d S) zKComplete setup for teleoperation-based recording: init -> home -> grav compzChecking CAN interfaces...F)rTr7zJ - Disabling position control on leaders for gravity compensation mode...rSru(✓ Leaders in gravity compensation modezInitializing footpedal...N) rrr2rBrrrcrdrrrrs rsetup_for_teleop_recordingz*RobotController.setup_for_teleop_recording4s( *+++ !!### 777 ###666  X    = G M & &"(1++"(1++ & F F F = G M & &"(1++"(1++ & F F F 8999 ''))) )*** rc|j |jd|_|j |jd|_|jr|j|jr|j|jr|jj|jr |jjdSdS)zMClose all robot instances, stopping their server threads and CAN connections.N) rrercloserrrrrrrs rrzRobotController.closeQs   (   $ $ & & & $D  ? & O ! ! # # #"DO ? $ O ! ! # # # ? $ O ! ! # # # = ) M & & ( ( ( = ) M & & ( ( ( ( ( ) )rc<d|_|jtd|td||dtd|jr@|jtj dtj d|j rB|j tj dtj dd Sd S) uStop teleop, move to home, re-enable leader gravity comp. Does NOT close robot connections — safe to call between recording episodes when the session should continue. Fz Stopping teleoperation...z& Moving arms back to home positions...Tru✓ All arms returned to homerSrN) rrrrrr6rBrrrcrdrrs rreturn_to_homezRobotController.return_to_homebs  $ !''))) +,,, !!! 7888 ))+++ ###666 -... = G M & &"(1++"(1++ & F F F = G M & &"(1++"(1++ & F F F F F G Grctjd||tjddS)zLComplete shutdown: stop teleop -> restore control -> go home -> close robotsr%N)r}r~rrrs rshutdownzRobotController.shutdownys? 3   3rc ztdtdtdtd|tdtd|}|D]#\}}td|d|d d d $td t j}d }t j|z dkr,|jr$d|vr |j|d|jr$d|vr |j|d|jr)d|vr%|jj |d|j r)d|vr%|j j |dd tt j|z z }||kr|dkrtd|d |}t j dt j|z dk,td| |dd |_|tdtdtdt#jdd S)zDEmergency stop: hold current positions for 5s, then move all to homez= !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!z EMERGENCY STOP ACTIVATEDz Step 4: Moving all arms to home positions simultaneously...Trz" All arms reached home position.z System will now shut down.z=!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! )rrrJitemsr}rrrrrrrrr~r6rBrrr_exit)rhold_positionsrpos start_timecountdown_printedrs rrzRobotController.emergency_stops o *+++ h =>>> !!! ())) FGGG1133'--// A AID# ?$??3rr7??? @ @ @ @ RSSSY[[ ikkJ&,, P<>#A#A11.2NOOO P<>#A#A11.2NOOO} S~!=!= $66~j7QRRR} S~!=!= $66~j7QRRRC j 8999I,,,Q+Y+++,,,$-! Jt   #ikkJ&,,( OPPP ))+++ ###666   3444 ,--- o  rc~||tjddS)z0Cleanup resources and stop robot control threadsrN)rrrrrs rcleanupzRobotController.cleanups2 !!!   r)TTTTFr)T)r()rr)ry)rr*ryF)NFN)/rrrrboolrurr2r,r6rBrr@rcrrErrGrJrNr[r_rarcrfrTrlrkpropertyror}rrrrrrrrcallablerrrrrrrr:rrr r gsII"& $#'"& 9 9 9 9 ! 9  9 9 9 9 vd4s/s/4s/Ds/s/s/s/j U U U U////.@0@04@04@0@0@0@0Dd3S"*_0E+E&F( d38L3L.M    T#rz/%:&HSM<hsmBd(  D     ####%%%%""5"4""""0 ( ( ( (2222X2 <<<<<|NNN<&&& %$4444  44440"I"Iu"I"I"I"I"IL  % A.A.A.A.  A.  A.  A.A.A.A.F1111 !&PPP P  P  PP PPPPd>&*"'+ *$*$c]*$*$8$ *$  *$*$*$*$X   :))))"GGGG.777rrr )r)Nr(rN)rr)Urrrsysrr}pathlibr_Pathtypingrrrrrrr8 jax.numpynumpyrrrcpyrokir]rrKscipy.spatial.transformr pathinsertrdirname__file__ i2rt.motor_drivers.can_interfacer i2rt.robots.get_robotr i2rt.robots.motor_chain_robotri2rt.robots.robotri2rt.robots.utilsr_ARM_YAM_XML_PATHrraiden.robot._jparserrrurr__all__rfr"r#r'r(r_GRIPPER_SPEEDr@ with_suffixrNparentrCrer/r__annotations__r1r7r;rOrTrorrrr!r_rrrrrr r:rrrs_<<<   !!!!!!7777777777771000000  ,,,,,,rw||BGOOH--tT=&QQ:99999//////999999######CCCCCC)))))),,,,,,555555(0 4    BH@@@AA"(999:: bhAAABB bh:::;; ' UU,--99'BBCC#ee-..5@AA 'bh  *'bh  *    )e))))D ) ) ) ) ) ) ) )L37 ,0 ,, ,J,$BJ/, ,  , ) , ,,,,>T#Y&34<348" ))z)) )  ) Z ))))XVVVVVVVVVVr