
    qiw                        d dl Z d dlmZ d dlZd dlZd dlmZ defdZ	 e j
        d           G d d	ej                              Z e j
        d           G d
 dej                              Zdej        fdZd Zd Zd Zd Zd ZdddededefdZdddej        dedej        fdZdddej        dedej        fdZdddej        dedej        fdZdS )    N)ClassVar)
transformsreturnc            	         t          j        d          t           j                            ddt           j                  t           j                            ddt           j                  t           j                            ddt           j                  t           j                            ddt           j                  dddS )z4Creates a random input example for the Aloha policy.)      )      r
   )sizedtypecam_highcam_lowcam_left_wristcam_right_wristzdo something)stateimagesprompt)nponesrandomrandintuint8     =/home/robot-lab/Pi0.5_yam/src/openpi/policies/aloha_policy.pymake_aloha_exampler   
   s     	))#M)RRy((=(QQ i//-rx/XX!y00=PRPX0YY	
 
 !	 	 	r   T)frozenc                   \    e Zd ZU dZdZeed<   dZee	e
df                  ed<   dedefd	Zd
S )AlohaInputszInputs for the Aloha policy.

    Expected inputs:
    - images: dict[name, img] where img is [channel, height, width]. name must be in EXPECTED_CAMERAS.
    - state: [14]
    - actions: [action_horizon, 14]
    Tadapt_to_pir   .EXPECTED_CAMERASdatar   c                 |   t          || j                  }|d         }t          |          t          | j                  z
  r't	          d| j         dt          |                     |d         }d|i}dt          j        i}ddd	}|                                D ]J\  }}||v r||         ||<   t          j        ||<   $t          j	        |          ||<   t          j
        ||<   K|||d
         d}	d|v r5t          j        |d                   }
t          |
| j                  }
|
|	d<   d|v r|d         |	d<   |	S )Nr!   r   zExpected images to contain z, got r   
base_0_rgbr   r   )left_wrist_0_rgbright_wrist_0_rgbr   )image
image_maskr   actionsr   )_decode_alohar!   setr"   
ValueErrortupler   True_items
zeros_likeFalse_asarray_encode_actions_inv)selfr#   	in_images
base_imager   image_masksextra_image_namesdestsourceinputsr+   s              r   __call__zAlohaInputs.__call__*   s   Tt/?@@@N	y>>C 5666 	lj4;PjjX]^gXhXhjjkkk z*
 *
 "(
 !1!2
 
 .3355 	. 	.LD&""(0t$&HD!!!}Z88t$&ID!! %']
 
 ji11G)'t?OPPPG 'F9t#H~F8r   N)__name__
__module____qualname____doc__r!   bool__annotations__r"   r   r/   strdictr>   r   r   r   r    r       su           K 3ohuS#X/nnn-T -d - - - - - -r   r    c                   2    e Zd ZU dZdZeed<   dedefdZdS )AlohaOutputszOutputs for the Aloha policy.Tr!   r#   r   c                 ~    t          j        |d         d d d df                   }dt          || j                  iS )Nr+   r   r%   )r   r4   _encode_actionsr!   )r6   r#   r+   s      r   r>   zAlohaOutputs.__call__b   sA    *T)_QQQV455?7@PQQQRRr   N)	r?   r@   rA   rB   r!   rC   rD   rF   r>   r   r   r   rH   rH   Z   sX         '' KST Sd S S S S S Sr   rH   c                  .    t          j        g d          S )z2Used to convert between aloha and pi joint angles.)   rM   rL   rL   rL   rL   rL   rM   rM   rL   rL   rL   rL   )r   arrayr   r   r   _joint_flip_maskrO   h   s    8BBBCCCr   c                     | |z
  ||z
  z  S Nr   xmin_valmax_vals      r   
_normalizerV   m   s    KGg-..r   c                     | ||z
  z  |z   S rQ   r   rR   s      r   _unnormalizerX   q   s    '!"W,,r   c                 l    t          | dd          } d } || dd          } t          | dd	          S )
Ngs?gV-?rT   rU   c                     |dz  | dz  z   |dz  z
  d|z  | z  z  }t          j        t          j        |dd                    S )N   g      g      ?)r   arcsinclip)linear_position
arm_lengthhorn_radiusvalues       r   linear_to_radianz-_gripper_to_angular.<locals>.linear_to_radian   sK    a/1"44z1}D[[jIjkyc22333r   g;On?gI+?)r`   ra   gDio?gPs?)rX   rV   )rb   rc   s     r   _gripper_to_angularre   u   sW     AAAE4 4 4
 Uu%HHHE
 eVV<<<<r   c                 0    | dz   } t          | dd          S )Nrd   R'B`"?rZ   )rV   rb   s    r   _gripper_from_angularrj      s$     FNE eWf====r   c                 0    t          | dd          } | dz
  S )Nrg   rh   rZ   rd   )rX   ri   s    r   _gripper_from_angular_invrl      s     @@@E6>r   Fr%   r#   r!   c                    t          j        | d                   }t          ||          }d | d         }fd|                                D             }|| d<   || d<   | S )Nr   r%   c                     t          j        |           } t          j        | j        t           j                  r"d| z                      t           j                  } t          j        | d          S )N   zc h w -> h w c)	r   r4   
issubdtyper   floatingastyper   einops	rearrange)imgs    r   convert_imagez$_decode_aloha.<locals>.convert_image   sT    joo=BK00 	/9$$RX..C%5666r   r   c                 .    i | ]\  }}| |          S r   r   ).0nameru   rv   s      r   
<dictcomp>z!_decode_aloha.<locals>.<dictcomp>   s)    LLL	c4s++LLLr   )r   r4   _decode_stater1   )r#   r!   r   r   images_dictrv   s        @r   r,   r,      s|     JtG}%%E%[999E7 7 7 (^FLLLLV\\^^LLLK DNDMKr   r   c                d    |r-t                      | z  } t          | ddg                   | ddg<   | S N      )rO   re   )r   r!   s     r   r{   r{      s<     = ""U*,UAr7^<<q"gLr   r+   c                t    |r5t                      | z  } t          | d d ddgf                   | d d ddgf<   | S r~   )rO   rj   r+   r!   s     r   rJ   rJ      sO     I"$$w.3GAAA2wJ4GHHAr7
Nr   c                t    |r5t                      | z  } t          | d d ddgf                   | d d ddgf<   | S r~   )rO   rl   r   s     r   r5   r5      sO     M"$$w.7Ar7
8KLLAr7
Nr   )dataclassestypingr   rs   numpyr   openpir   rF   r   	dataclassDataTransformFnr    rH   ndarrayrO   rV   rX   re   rj   rl   rC   r,   r{   rJ   r5   r   r   r   <module>r      sj                       D     d###> > > > >*, > > $#>B d###
S 
S 
S 
S 
S:- 
S 
S $#
SD"* D D D D
/ / /- - -= = =.
> 
> 
>   6;    d t    , =B    T bj     AF   RZ  "*     EJ    T bj      r   