
    ܱi                         d dl Zd ZddZdS )    Nc                    d }ddd| d         gddt           j         dz  | d         gddt           j        dz  | d         gddt           j        dz  | d         gd	d
t           j         dz  | d         gddt           j        dz  | d         gddt           j        dz  | d         gg ddddt           j         dz  gg dg
}t          j        d          }t          d          D ]}| |||          z  }|S )Nc           
         ||          d         }||          d         }||          d         }||          d         }|}t          j        t          j        |          t          j        |           d|gt          j        |          t          j        |          z  t          j        |          t          j        |          z  t          j        |           t          j        |           |z  gt          j        |          t          j        |          z  t          j        |          t          j        |          z  t          j        |          t          j        |          |z  gg dg          S )Nr            )r   r   r   r   )nparraycossin)idhadalphathetaqs          //data/cameron/vidgen/Ctrl-World/models/utils.py
get_tf_matz#get_fk_solution.<locals>.get_tf_mat   s%   qE!HqE!H1a1ax"&))bfQiiZA6RVE]]2BF1IIu4MPRPVW\P]P]~`b`fgl`m`m_mpq_qrRVE]]2BF1IIu4MrvV[}}^`^dej^k^kno^op$& ' ' 	'    r   gZd;O?r   r   gMbX9?gQ?r   gQg~jt?      gI+?   )r   g1Zd?r   r   )        g	h"lx?r   r      )r   pieyerange)joint_anglesr   	dh_paramsTr   s        r   get_fk_solutionr!      s   
' 
' 
' UA|A/Qq,q/2UBE!G\!_5!RU1Wl1o6525&(LO<Qaa1BE!G\!_5!!!QBE6!8$$$$	&I 	q		A5\\ ) )

1i(((Hr   1799{Gz?c                    g }t          j        d          }|dk    r| d          dz   |d<   n||dk    r| d          dz   |d<   nf|dk    r| |d<   nY|d	k    r|
 |d<   nL|d
k    r|
 |d<   n?|dk    r| |d<   n2|dk    r|
 |d<   n%|dk    r| |d<   nt          d           d|dd d f<   |                    |            t	          d          D ]#}|                    | ||dz   z  dz  z              $t          j        |d          }|dk    rqt          j        | d         dz
            dk     r|t          j        | d         dz
            dk     r[t          j        | d         dz
            dk     r:|dk    r4t          j        g dg dg dg dg dg          }| d          |d d d!f<   nt          j        | d         d"z
            dk     r|t          j        | d         d#z
            dk     r[t          j        | d         d$z
            dk     r:|dk    r4t          j        g dg dg dg dg dg          }| d          |d d d!f<   n7t          j        | d         d"z
            dk     r{t          j        | d         d#z
            dk     rZt          j        | d         d$z
            dk     r9|dk    r3t          j        g dg d%g d&g d'g d(g          }| d          |d d d!f<   nt          j        | d         d)z
            dk     rzt          j        | d         d*z
            dk     rYt          j        | d         d+z
            dk     r8|dk    r2t          j        g d(g d'g d&g d%g dg          }| d          |d d d!f<   t          j        |d d df         d,d-          |d d df<   t          j        |d d df         d.d/          |d d df<   t          j        |d d d0f         dd/          |d d d0f<   |S )1N)r      o)r   r   r   cgffffff?l)r   r   rf)r   r   bu)r   r   r   z,wrong action key, please use l,r,f,b,u,d,o,cr   r   r   g      @)axisr"   g/$?g{Gz?gmͿgl?)g   `j?g   Ϳg    -?g   2@g   @g   1r   )g   6?g   LͿg   u?g   HD@g   `Ug   r   )g    c?g   Ϳg   ?g   @g    g    Xr   )g   ?g   Ͽg   Q?g    @g    =g   (r   )g   ?g   пg   2c?g   _@g   Kg    #r   )r   r.   g rh?gL7A`пgZd;?)g   ?g   @ѿg   @?g   q	@g   ~g   `.r   )g   ?g   `<ѿg   	?g   
	g   g   `˦r   )g    ?g    J0ѿg    ;?g   g    g   `Ur   )g   @"?g    пg    DL?g   `g   @g   `Jr   g&1?gS㥛пg-C6?g333333?g?g      g      ?r   )	r   zerosprintappendr   concatenateabsr	   clip)pose1actiontask_iddistanceaction_chunkdelatar   s          r   key_board_controlr;   '   s    LXe__F}}c
{S(s	3c
{T)s	3is	3is	3is	3is	3is	3is<===qs1XX 6 6EFAaCL$445555>,Q777L &6%*e#$$T))bfU3Z5G.H.H.M.MRTRXY^_bYcejYjRkRklpRpRpu{  @C  vC  vC8 S  S  S R  R  R S  S  S R  R  R Q  Q  Q%S T TL "'tL2VE#J%&&t++uSz67I0J0J40O0OTVTZ[`ad[egl[lTmTmnrTrTrw}  BE  xE  xE8 Q  Q  Q R  R  R S  S  S R  R  R S  S  S%U V VL "'tL2VE#J%&&t++uSz67I0J0J40O0OTVTZ[`ad[egl[lTmTmnrTrTrw}  BE  xE  xE8  &f  &f  &f T  T  T W  W  W V  V  V R  R  R	%T U UL
 "'tL2VE#J%&&t++uSz67I0J0J40O0OTVTZ[`ad[egm[mTnTnosTsTsx~  CF  yF  yF8  &g  &g  &g V  V  V W  W  W T  T  T Q  Q  Q	%S T TL
 "'tL2
 QQQqS 13<<L1QQQqS 14==L1QQQqS 14==L1r   )r"   r#   )numpyr   r!   r;    r   r   <module>r>      sE         @H H H H H Hr   