
    qiW2                     f   d dl Z d dlZd dlmZ d dlmc mZ d dlZd dlm	Z
 d dlmZ d dlmZ d dlmZ d dlmc mZ d dlmc mZ d dlmZ  e j        d          Zd Zej        dej        ej         d	f         d
e!de"de"dej#        ej         df         f
d            Z$ G d dej%                  Z&dS )    N)override)model)
pi0_config)array_typingopenpic                 
   t          j        || j                  }t          j        |d          }|dddddf         |dddddf         k    }| dddddf         | dddddf         z  }t          j        ||          S )a  Adapted from big_vision.

    Tokens can attend to valid inputs tokens which have a cumulative mask_ar
    smaller or equal to theirs. This way `mask_ar` bool[?B, N] can be used to
    setup several types of attention, for example:

      [[1 1 1 1 1 1]]: pure causal attention.

      [[0 0 0 1 1 1]]: prefix-lm attention. The first 3 tokens can attend between
          themselves and the last 3 tokens have a causal attention. The first
          entry could also be a 1 without changing behaviour.

      [[1 0 1 0 1 0 0 1 0 0]]: causal attention between 4 blocks. Tokens of a
          block can attend all previous blocks and all tokens on the same block.

    Args:
      input_mask: bool[B, N] true if its part of the input, false if padding.
      mask_ar: bool[?B, N] mask that's true where previous tokens cannot depend on
        it and false where it shares the same attention mask as the previous token.
       axisN)jnpbroadcast_toshapecumsumlogical_and)
input_maskmask_arr   	attn_mask
valid_masks        2/home/robot-lab/Pi0.5_yam/src/openpi/models/pi0.pymake_attn_maskr      s    * w
(899GZa(((Fqqq$z"fQQQ4Z&88IAAAtQQQJ'*QQQ4Z*@@J?9j111    pos bembedding_dim
min_period
max_periodreturnzb {embedding_dim}c                 x   |dz  dk    rt          d| d          t          j        dd|dz            }|||z  |z  z  }t          j        d| d|z  dz  t          j        z  t
          j        j        j                  }t          j	        t          j
        |          t          j        |          gd	
          S )zGComputes sine-cosine positional embedding vectors for scalar positions.   r   zembedding_dim (z) must be divisible by 2g              ?zi,j->ij)	precisionr
   )
ValueErrorr   linspaceeinsumpijaxlax	PrecisionHIGHESTconcatenatesincos)r   r   r   r   fractionperiodsinusoid_inputs          r   posemb_sincosr1   /   s    
 qAR=RRRSSS|Cmq&899H:
2x??FZfq36!'#+	  N ?CGN33SW^5L5LMTVWWWWr   c                       e Zd Zdej        dej        f fdZej	        de
j        deej        ej        df         ej        ej        df         ej        ej        df         f         fd	            Zej	        de
j        d
e
j        dej        ej        df         deej        ej        df         ej        ej        df         ej        ej        df         ej        ej        df         dz  f         fd            Zedddej        de
j        de
j        dedej        ej        df         f
d            Zeddddej        de
j        deej        ej        df         z  dej        ej        df         dz  de
j        f
d            Z xZS )Pi0configrngsc           	      *   t                                          |j        |j        |j                   |j        | _        t          j        |j                  }t          j        |j	                  }t          j        t          j        ||g|j        |j                            }|                    |d|j        rddgnddg           t          j        t          j        |j        ddd|j                            }|                    t#          t%          |                                j                                                            d|	           t-          j        ||
          | _        t-          j        |j        |j        |          | _        |j        rMt-          j        |j        |j        |          | _        t-          j        |j        |j        |          | _        nut-          j        |j        |j        |          | _        t-          j        d|j        z  |j        |          | _        t-          j        |j        |j        |          | _        t-          j        |j        |j        |          | _         d| _!        d S )N)configsembed_dtypeadarmsinitFT)r5   method
use_adarmsz	So400m/14none)num_classesvariant	pool_typescandtype_mm)trainr5   )llmimg)r5   r   )"super__init__
action_dimaction_horizonmax_token_lenpi05_gemma
get_configpaligemma_variantaction_expert_variant
nnx_bridgeToNNXModuledtype	lazy_init_siglipwidthnextiterfake_obsimagesvaluesnnxDict	PaliGemmaLinearaction_in_projtime_mlp_intime_mlp_out
state_projaction_time_mlp_inaction_time_mlp_outaction_out_projdeterministic)selfr4   r5   paligemma_configaction_expert_configrD   rE   	__class__s          r   rG   zPi0.__init__C   sc   *F,A6CWXXXK	!,V-EFF%01MNNM)+?@"L{  
 
 	4FK;kE4==^cej]klllN,2#   
 
 	d4 1 1 8 ? ? A ABBCC5W[\\\cs333!j):<P<V]abbb; 	u"z*>*DFZF`gklllD #
+?+EG[Gahl m m mD!j):<P<V]abbbDO&)j5I5O1OQeQkrv&w&w&wD#'*z2F2LNbNhos't't'tD$"z*>*DfFW^bccc "r   obsr   zb s embzb sz sc           	         g }g }g }|j         D ]}| j                            |j         |         d          \  }}|                    |           |                    t	          j        |j        |         d|j        d                              |dg|j        d         z  z  }|j        d| j        	                    |j        d          }|                    |           |                    |j
                   |dg|j        d         z  z  }t          j        |d          }t          j        |d          }t          j        |          }|||fS )	NFrC   zb -> b sr	   sembed)r;   r
   )rZ   r^   rE   appendeinopsrepeatimage_masksr   tokenized_promptrD   tokenized_prompt_maskr   r+   array)	rh   rl   r   ar_masktokensnameimage_tokens_tokenized_inputss	            r   embed_prefixzPi0.embed_prefixi   sm    
J 	7 	7D"n00D1A0OOOL!MM,'''OD)"(+     w!3A!666GG +#~11#2Fw1WWMM*+++c7888w!1!7!:::Ga000_Za888
)G$$z7**r   noisy_actionstimestepr   zb embNc                    g }g }g }| j         s|                     |j                  d d d d d f         }|                    |           |                    t	          j        |j        j        d         dft          j                             |dgz  }|                     |          }t          || j        j
        dd          }	| j         rW|                     |	          }	t          j        |	          }	|                     |	          }	t          j        |	          }	|}
|	}nvt          j        |	d| j        	          }t	          j        ||gd
          }|                     |          }t          j        |          }|                     |          }|}
d }|                    |
           |                    t	          j        |
j        d d         t          j                             |dgdg| j        dz
  z  z   z  }t	          j        |d          }t	          j        |d          }t	          j        |          }||||fS )Nr   r	   )rS   TgMbp?g      @)r   r   zb emb -> b s embro   r"   r
   r   F)rK   rc   staterr   r   onesr   bool_r`   r1   out_featuresra   r\   swishrb   rs   rt   rI   r+   rd   re   rx   )rh   rl   r   r   r   ry   rz   state_tokenaction_tokenstime_embaction_expert_tokensadarms_condtime_tokensaction_time_tokenss                 r   embed_suffixzPi0.embed_suffix   sH    
y 	//#)44QQQaaaZ@KMM+&&&ch	(:A'>ciPPPQQQvG++M:: 4+>+KX\ilmmm9 	''11Hy**H((22Hy**H#0 "KK !-2DH[\\\K!$-1MTV!W!W!W!%!8!89K!L!L!$+=!>!>!%!9!9:L!M!M#5 K*+++#(#7#=bqb#ASSSTTTD6eW(;a(?@AAa000_Za888
)G$$z7K77r   Frn   rngobservationactionsrC   z*b ahc                v   t           j                            |d          \  }}}t          j        |||          }|j        d d         }t           j                            ||j                  }	t           j                            |dd|          dz  dz   }
|
d         }||	z  d|z
  |z  z   }|	|z
  }|                     |          \  }}}| 	                    |||
          \  }}}}t          j        ||gd	          }t          j        ||gd
	          }t          ||          }t          j        |d	          dz
  }| j                            ||g||d |g          \  \  }}}|                     |d d | j         d f                   }t          j        t          j        ||z
            d	          S )N   rn   g      ?r	   g+?gMbP?).NNr
   r   )mask	positionsr   r"   )r'   randomsplit_modelpreprocess_observationr   normalbetar   r   r   r+   r   r   r^   rD   rf   rI   meansquare)rh   r   r   r   rC   preprocess_rng	noise_rngtime_rngbatch_shapenoisetimetime_expandedx_tu_tprefix_tokensprefix_maskprefix_ar_masksuffix_tokenssuffix_masksuffix_ar_maskr   r   ry   r   r   
prefix_out
suffix_outr}   v_ts                                r   compute_losszPi0.compute_loss   s    /2j.>.>sA.F.F+	83NKW\]]]mCRC(
!!)W];;zxa==EM_-e#q='8G&CCgo 6:5F5F{5S5S2{NBFBSBST_adfjBkBk?{NK_k;%?aHHH
/>>"BKKK":w77	Jz222Q6	&*n&8&8M*i^bdo]p '9 '
 '
# Z! "":aaa$2E1E1G1G.G#HIIx
39--B7777r   
   )	num_stepsr   r    r   zb ah adc                    t          j        d d          d|z  j        j        d         |-t          j                            | j         j        f          } 	                              \  }t          |          }t          j        d          dz
  } j                            d g||          \  } fd}	fd	}
t          j                            |
|	|d
f          \  }}|S )NFrn   g      r   r	   r
   )r   r   c                    | \  }}                     |t          j        |                    \  }}}}t          ||          }t	          j        d|j        d                   }t          j        ||gd          }	|	j        |j        d         j        d         |j        d         z   fk    sJ t          j        d          d d d f         t          j	        |d          z   dz
  }
j
                            d |g|	|
d |g          \  \  }}}|J                     |d d j         d f                   }||z  z   |z   fS )Nzb p -> b s pr	   ro   r"   r
   )r   r   kv_cacher   )r   r   r   r   rs   rt   r   r+   sumr   r^   rD   rf   rI   )carryr   r   r   r   r   r   suffix_attn_maskprefix_attn_maskfull_attn_maskr   r   r   r}   r   
batch_sizedtr   r   r   r   rh   s                  r   stepz Pi0.sample_actions.<locals>.step   s   ICFJFWFWS#"24"D"DG GCM;
  .k>JJ  &}[.ML_`aLbccc !_.>@P-QXZ[[[N!'#A&#A&)<Q)??,     "555aaag>K^`AaAaAaadeeI*..*<*<}%##!!;/ += + +'$Za %%%&&z!!!d6I5I5K5K2K'LMMCc>4"9,,r   c                 "    | \  }}| dz  k    S )Nr    )r   r   r   r   s      r   condz Pi0.sample_actions.<locals>.cond  s    ICB37?"r   r    )r   r   r   r   r'   r   r   rI   rH   r   r   r   r   r^   rD   r(   
while_loop)rh   r   r   r   r   r   r   r   r}   r   r   x_0r   r   r   r   r   s   ` `         @@@@@r   sample_actionszPi0.sample_actions   sK    3D+USSS I &,Q/
=J%%cJ8KT_+]^^E 6:5F5F{5S5S2{N)+~FFJ{333a7	n((-)>EUaj(kk8 	-  	-  	-  	-  	-  	-  	-  	-  	-  	-  	-D	# 	# 	# 	# 	#
 ##D$==Q
r   )__name__
__module____qualname__r   	Pi0Configr\   RngsrG   at	typecheckr   ObservationtupleFloatArrayBoolr   Actionsr   r   KeyArrayLikeboolr   intIntr   __classcell__)rk   s   @r   r3   r3   B   sb       $"z3 $"38 $" $" $" $" $" $"L \+%+	rx)+,bgbho.FPRPXZ^P^H__	`+ + + \+B \.8%.86<n.8PRPXY[YacgYgPh.8	
9$%
% 
$
7"#d*	,
.8 .8 .8 \.8` ot8 8 8?8171C8NTn8hl8	"(G#	$8 8 8 X86  136:> > >_> '>
 "--> x)+,t3> 
> > > X> > > > >r   r3   )'loggingrs   flax.nnxr\   flax.nnx.bridgebridgerP   r'   	jax.numpynumpyr   typing_extensionsr   openpi.modelsr   r   r   openpi.models.gemmamodelsgemmarL   openpi.models.siglipsigliprU   openpi.sharedr   r   	getLoggerloggerr   r   Realr   r   floatr   r1   	BaseModelr3   r   r   r   <module>r      s           $ $ $ $ $ $ $ $ $ 



       & & & & & & ) ) ) ) ) ) $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ & & & & & & & & & , , , , , ,		8	$	$2 2 28 X	4	 X14XBGXUZXXbh++,X X X X$U U U U U&
 U U U U Ur   