
    fi,                        U d Z ddlZddlZddlmZ dZddddZee	e	f         e
d	<   d
ej        dej        dej        fdZdej        dej        fdZdej        dededej        fdZ G d de          Zd Zedk    r e             dS dS )u  OpenPI Pi0.5 bridge for YAM robot deployment.

Implements the ``ModelBridge`` interface from raiden. Connects to a running
OpenPI policy server (``serve_policy.py``) via websocket and translates
between raiden observations and the OpenPI observation format.

The OpenPI server handles all model inference, normalization, and
delta→absolute action conversion. This bridge is purely an observation
reformatter and action reorderer.

Usage (server + bridge)::

    # Terminal 1: start the OpenPI policy server
    python scripts/serve_policy.py \
        --policy.config pi05_yam \
        --policy.dir /path/to/checkpoint \
        --port 8000

    # Terminal 2: run raiden inference (note: --resize_images must be empty
    # so the bridge receives raw 1280x720 frames and can letterbox correctly)
    rd infer \
        --bridge openpi_bridge:OpenPiBridge \
        --ckpt_path unused \
        --action_hz 30.0 \
        --resize_images '' \
        --bridge-kwargs host=localhost port=8000 action_horizon=10 \
            prompt='Sort cans shortest to heighest / left to right (robot'\''s perspective)'

OpenPI 14D layout: [left_joint(6), left_grip(1), right_joint(6), right_grip(1)]
Raiden 14D layout: [right_joint(6), right_grip(1), left_joint(6), left_grip(1)]
    N)ModelBridge   observation/image_headobservation/image_left_wristobservation/image_right_wrist)scene_cameraleft_wrist_cameraright_wrist_camera_CAM_MAPr_joint_posl_joint_posreturnc                     t          j        |dd         |dd         | dd         | dd         g                              t           j                  S )zConvert raiden proprio (7,) pairs to OpenPI 14D state.

    Raiden: r_joint_pos = [r_joints(6), r_grip(1)]
            l_joint_pos = [l_joints(6), l_grip(1)]
    OpenPI: [l_joints(6), l_grip(1), r_joints(6), r_grip(1)]
    N      npconcatenateastypefloat32)r   r   s     3/home/robot-lab/raiden_pi05/raiden/openpi_bridge.py_raiden_to_openpi_stater   0   s\     >O!O!		
  fRZ    
action_14dc                     t          j        | dd         | dd         g                              t           j                  S )zConvert OpenPI 14D action to raiden 14D motor command.

    OpenPI: [l_joints(6), l_grip(1), r_joints(6), r_grip(1)]
    Raiden: [r_joints(6), r_grip(1), l_joints(6), l_grip(1)]
    r      r   r   )r   s    r   _openpi_action_to_raidenr   D   sE     >qtqsO	
 
 fRZr   imageheightwidthc           	      n   | j         dd         \  }}t          ||z  ||z            }t          ||z            }t          ||z            }t          j        | ||ft          j                  }||z
  dz  }	||z
  |	z
  }
||z
  dz  }||z
  |z
  }t          j        ||	|
||t          j        d          }|S )zAspect-ratio preserving resize with zero-padding.

    Replicates the server-side resize_with_pad behavior so that inference
    images match what the model saw during training (letterboxed, not squashed).
    N   )interpolation)r   r   r   )value)shapemaxintcv2resizeINTER_LINEARcopyMakeBorderBORDER_CONSTANT)r   r   r    cur_hcur_wrationew_wnew_hresizedpad_h0pad_h1pad_w0pad_w1paddeds                 r   _resize_with_padr8   R   s     ;rr?LE5uv~..EEEjc>NOOOGun"Fe^f$Fem!FU]V#F  F Mr   c                   ^    e Zd ZdZddedefdZdedd	fd
ZddZ	de
j        fdZdefdZd	S )OpenPiBridgezModelBridge implementation for OpenPI Pi0.5 on YAM.

    Connects to a running OpenPI websocket server and translates between
    raiden observations and OpenPI's expected input format.
    
         ?action_horizonaction_scalec                 h    d | _         || _        || _        d| _        d| _        d| _        d| _        d S )N r           )_broker_action_horizon_action_scale_prompt_step_n_infer_t_infer_sum)selfr=   r>   s      r   __init__zOpenPiBridge.__init__v   s<    -)
r   	ckpt_pathr   Nc                 r   ddl m} ddl m} |                    dd          }t	          |                    dd                    }t	          |                    d| j                            }t          |                    d	| j                            | _        |                    d
d          | _        t          d| d|            |
                    ||          }|                                }	t          d|	            |                    ||          | _        t          d| d           dS )a  Connect to the OpenPI policy server.

        The ckpt_path is unused (the server loads the model). Connection
        parameters are passed via kwargs.

        Keyword args:
            host: Server host (default: "localhost").
            port: Server port (default: 8000).
            action_horizon: Number of actions to execute per inference call.
            prompt: Language instruction for the task.
            action_scale: Interpolation factor (1.0 = full predicted action).
        r   )action_chunk_broker)websocket_client_policyhost	localhostport@  r=   r>   promptr@   z([openpi_bridge] Connecting to server at :)rO   rQ   z![openpi_bridge] Server metadata: )policyr=   z&[openpi_bridge] Ready (action_horizon=)N)openpi_clientrM   rN   getr'   rC   floatrD   rE   printWebsocketClientPolicyget_server_metadataActionChunkBrokerrB   )
rI   rK   kwargsrM   _wsrO   rQ   r=   	ws_policymetadatas
             r   loadzOpenPiBridge.load   sN    	655555@@@@@@zz&+..6::fd++,,VZZ(8$:NOOPP"6::nd>P#Q#QRRzz(B//FFFFFGGG--4d-CC	0022<(<<===*<<) = 
 
 	H~HHHIIIIIr   c                 p    | j         | j                                          d| _        d| _        d| _        dS )zReset action chunk buffer.Nr   rA   )rB   resetrF   rG   rH   )rI   s    r   rd   zOpenPiBridge.reset   s:    <#L   
r   c                 x   ddl }|                                }|                     |          }| j                            |          }|                                |z
  dz  }| xj        |z  c_        | xj        dz  c_        | j        dk    r:|d         }t          d| j                    t          dt          j	        |d	
                      t          dt          j	        |d         d	
                      dD ]F}||v r@||         }	t          d| d|	j
         d|	j         d|	                                d           G| j        dk    ruddl}
|
                    dd           dD ]H}||v rB|                    dd          dz   }t!          j        d| ||         ddddf                    It          d           | j        dz  dk    r.t          d| j        dd |dd!| j        | j        z  dd"           |d         }t%          |          }| j        d#k    rt          j        |j                            d$t          j        d%t          j        &                    |j                            d't          j        d%t          j        &                    g          }|| j        ||z
  z  z   }|S )(u   Convert raiden observation → OpenPI input → inference → 14D action.

        Returns (14,) float32 motor command in raiden order:
        [r_joints(6), r_grip(1), l_joints(6), l_grip(1)]
        r   Ng     @@      observation/statez[DEBUG] step=z  state sent (openpi order):    )	precisionz)  raw action from server (openpi order): actions)r   r   r   z  z: shape=z dtype=z mean=z.1fz/tmp/openpi_debugT)exist_okzobservation/r@   z.pngz/tmp/openpi_debug/.z,  [saved debug images to /tmp/openpi_debug/]2   z[openpi_bridge] step=4dz  infer=zms  avg=msr<   follower_r_joint_posr   dtypefollower_l_joint_pos)timeperf_counter_preprocessrB   inferrH   rF   rZ   r   array2stringr%   rs   meanosmakedirsreplacer(   imwriter   rD   r   propriosrX   zerosr   )rI   obsru   t0obs_dictresultinfer_msstatecam_keyimgr{   fnamerk   action_raidencurrents                  r   predictzOpenPiBridge.predict   s1    	  ##C((##H--%%''",3X%

a

:??01E.$*..///W"/%ST2U2U2UWWXXXobofU^N_kl>m>m>moopppv e eh&&"7+Ccwcc	cc#)ccSVS[S[S]S]cccdddzQ			/$???z ` `G(** ' C Cf L$@$@$@(7BSTWY]Y][]Y]T]B^___EFFF:?a>
9 > >!+> >(4:5=> > >   #099$$nL$$."*0M0M0M  L$$."*0M0M0M 		 	G $d&8MG<S&TTMr   c                    i }|j         D ]I}t                              |j                  }|$t	          |j        t          t                    }|||<   J|j                            dt          j	        dt          j
                            }|j                            dt          j	        dt          j
                            }t          ||          |d<   | j        r
| j        |d<   |S )u  Convert raiden Observation to OpenPI observation dict.

        Key details:
        - Images arrive from raiden as RGB uint8 (raiden converts BGR→RGB
          in the capture loop). We do NOT apply another color conversion.
        - Images are resized using aspect-ratio-preserving letterbox padding
          to match the server-side resize_with_pad used during training.
        Nrq   r   rr   rt   rh   rS   )camerasr   rX   namer8   r   MODEL_IMG_SIZEr   r   r   r   r   rE   )rI   r   r   camobs_keyrgbr_posl_poss           r   rw   zOpenPiBridge._preprocess   s     ; 	$ 	$Cll38,,G #39nnMMC #HW   !7!2:9V9V9VWW  !7!2:9V9V9VWW(?u(M(M$% < 	.!%HXr   )r;   r<   )r   N)__name__
__module____qualname____doc__r'   rY   rJ   strrb   rd   r   ndarrayr   dictrw    r   r   r:   r:   o   s            s  u         Jc  J  J  J  J  JD       9bj 9 9 9 9v$      r   r:   c            
      T   dd l } |                     d          }|                    ddd           |                    dd	d
           |                    dt          dd           |                    dt          dd           |                    dt          d           |                    ddd           |                    dd           |                    dd           |                    ddddg            |                    d!d"           |                                }dd#lm} t          |j	        $          } |||j
        |j        |j        |j        |j	        |j        d%|j        |j        |j        |j        &          }|                                 d S )'Nr   z'Deploy OpenPI Pi0.5 policy on YAM robot)descriptionz--ckpt_pathunusedzUnused (server loads model))defaulthelpz--hostrP   zOpenPI server hostz--portrR   zOpenPI server port)typer   r   z--action_horizonr;   zActions per inferencez--action_hzg      >@)r   r   z--promptr@   zLanguage instructionz--camera_config_filez./config/camera_config.json)r   z--calibration_filez!./config/calibration_results.jsonz--stereo_methodzedffs)r   choicesz--depth_modeNEURAL_LIGHT)RaidenInferenceLoop)r=   )rO   rQ   r=   rS   )bridgerK   	action_hzbridge_kwargscamera_config_filecalibration_filestereo_method
depth_mode)argparseArgumentParseradd_argumentr'   rY   
parse_argsraiden.inferencer   r:   r=   rK   r   rO   rQ   rS   r   r   r   r   run)r   parserargsr   r   loops         r   mainr   	  s	   OOO$$= %  F x.K     +<PQQQ
sD?STTT
b7N     E4@@@

B5KLLL
.8UVVV
&I     )55%.QQQ
???D444444)<===F..II"1k	
 
  2.(?  D 	HHJJJJJr   __main__)r   r(   numpyr   r   r   r   r   r   r   __annotations__r   r   r   r'   r8   r:   r   r   r   r   r   <module>r      sh    @ 


     ( ( ( ( ( ( -79 $sCx.    Z   ( 
    BJ  C BJ    :R R R R R; R R Rt, , ,^ zDFFFFF r   