
    jV                     n    d Z ddlZddlZddlZddlZddlmZ ddlm	Z	 ddl
mZ dZ G d de          ZdS )	a  VLA Foundry DiffusionPolicy bridge for YAM robot deployment.

Same pattern as the OpenPI bridge: connects to a policy server, sends
observations as a simple dict, gets back joint actions.

Usage::

    # Terminal 1: start the VLA Foundry policy server
    cd ~/vla_foundry
    uv run --group inference python serve_vla_foundry.py \
        --checkpoint_directory /home/robot-lab/checkpoints/stack_blocks_vla_foundry/ \
        --checkpoint_name checkpoint_11 \
        --num_flow_steps 10 \
        --open_loop_steps 26 \
        --guidance_scale 1.0 \
        --port 8200

    # Terminal 2: run raiden inference
    cd ~/raiden_pi05/raiden
    uv run rd infer \
        --bridge vla_foundry_bridge:VlaFoundryBridge \
        --ckpt_path unused \
        --action_hz 30 \
        --max_joint_delta 0.5 \
        --resize_images '' \
        --bridge-kwargs host=localhost port=8200 prompt='stack the blocks'

Server returns: [l_joints(6), l_grip(1), r_joints(6), r_grip(1)]
Raiden expects:  [r_joints(6), r_grip(1), l_joints(6), l_grip(1)]
    N)Observation)connect)ModelBridgei  c                   Z    e Zd ZdZd ZdeddfdZddZdede	j
        fd	Zdedefd
ZdS )VlaFoundryBridgez9ModelBridge that connects to a VLA Foundry policy server.c                 >    d | _         d| _        d| _        d| _        d S )N r           )_ws_prompt_step_t_sumselfs    8/home/robot-lab/raiden_pi05/raiden/vla_foundry_bridge.py__init__zVlaFoundryBridge.__init__0   s"    
    	ckpt_pathreturnNc                    |                     dd          }t          |                     dd                    }|                     dd          | _        d| d| }t          d	|            t	          |d
          | _        | j                            t          j        ddi                     t          j	        | j        
                                d          }|d         dk    sJ t          d| j         d           d S )Nhost	localhostporti   promptr	   zws://:z#[vla_foundry_bridge] Connecting to i )max_sizetyperesetFraw	reset_ackz([vla_foundry_bridge] Connected. prompt='')getintr   printr   r   sendmsgpackpackbunpackbrecv)r   r   kwargsr   r   uriresps          r   loadzVlaFoundryBridge.load6   s    zz&+..6::fd++,,zz(B//#d##T##9C99:::3555gmVW$566777tx}}E:::F|{****HHHHIIIIIr   c                     | j         [| j                             t          j        ddi                     t          j        | j                                         d           d| _        d| _        d S )Nr   r   Fr   r   r
   )r   r&   r'   r(   r)   r*   r   r   r   s    r   r   zVlaFoundryBridge.resetD   s^    8HMM'-(9::;;;ODHMMOO7777
r   obsc           	         t          j                    }|                     |          }| j                            t          j        |d                     t          j        | j                                        d          }t          j
        |d         t          j                  }t          |          dk    r't          j        |dd         |d	d         g          }n|}t          j                    |z
  d
z  }| xj        |z  c_        | xj        dz  c_        | j        dz  dk    r.t!          d| j        dd|dd| j        | j        z  dd           |S )NT)use_bin_typeFr   actiondtype      r   g     @@   2   z[vla_foundry_bridge] step=4dz  rtt=z.1fzms  avg=ms)timeperf_counter_preprocessr   r&   r'   r(   r)   r*   nparrayfloat32lenconcatenater   r   r%   )r   r0   t0obs_dictr-   r3   action_raideninfer_mss           r   predictzVlaFoundryBridge.predictK   sb     ##C((gmH4@@@AAAtx}}E:::$x.
;;;v;;"NF1R4L&1++FGGMM #M%''",3x

a

:?a8TZ> 8 8)8 8{TZ/78 8 8   r   c                 D   i }|j         D ]Y}t          j        |j        t          t          f          }|                                t          |j                  d||j        <   Z|j	        
                    dt          j        dt          j                            }|j	        
                    dt          j        dt          j                            }i }dD ]S}|j	        
                    |          }	|	5t          j        |	t          j                                                  ||<   Td|t          j        |t          j                                                  t          j        |t          j                                                  ||j        | j        d	S )
uF   Pack observation into a simple dict — images + joint state + prompt.)datashapefollower_r_joint_posr7   r4   follower_l_joint_pos)z$robot__actual__poses__left::yam__xyzz'robot__actual__poses__left::yam__rot_6dz'robot__actual__grippers__left::yam_handz%robot__actual__poses__right::yam__xyzz(robot__actual__poses__right::yam__rot_6dz(robot__actual__grippers__right::yam_handNstep)r   imagesjoint_pos_rjoint_pos_lee_poses	timestampr   )camerascv2resizeimage	_IMG_SIZEtobyteslistrK   namepropriosr#   r?   zerosrA   asarrayfloat64tolistrS   r   )
r   r0   rO   camimgr_posl_posrR   keyvals
             r   r>   zVlaFoundryBridge._preprocessi   ss    ; 	 	C*SYI(>??Cci   F38   !7!2:9V9V9VWW  !7!2:9V9V9VWW 
 
	K 
	KC ,""3''C "
3bj A A A H H J J :e2:>>>EEGG:e2:>>>EEGG l
 
 	
r   )r   N)__name__
__module____qualname____doc__r   strr.   r   r   r?   ndarrayrH   dictr>    r   r   r   r   -   s        CC  Jc J J J J J   ; 2:    <%
{ %
t %
 %
 %
 %
 %
 %
r   r   )rj   r<   rU   r'   numpyr?   chiral.typesr   websockets.sync.clientr   raiden.inferencer   rX   r   rn   r   r   <module>rs      s    >  



      $ $ $ $ $ $ * * * * * * ( ( ( ( ( (	a
 a
 a
 a
 a
{ a
 a
 a
 a
 a
r   