ó
t™xTc           @   s�  d  d l  m Z d  d l m Z d  d l m Z d  d l m Z m Z m Z m	 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 Z m Z d  d
 l Z d  d
 l Z d e f d „  ƒ  YZ d e f d „  ƒ  YZ d e f d „  ƒ  YZ d e f d „  ƒ  YZ d „  Z d „  Z d g Z d „  Z d „  Z d d d „ Z  d „  Z! e" d k r}e! e j# d ƒ n  d
 S(   iÿÿÿÿ(   t   division(   t   featherstone(   t	   joint_lib(   t   spatial_inertia_from_sitest   T_as_Xt   featherstone_system_modelt   e_kin_from_model(   t
   test_utils(   t   flex(   t   matrix(   t   approx_equal(   t   null_outt   show_times_at_exitNt   random_revolutec           B   s   e  Z d  „  Z RS(   c            s  ‡  f d †  } ‡  f d †  } | ƒ  g |  _  xw t d ƒ D]c } t j d | ƒ  d | ƒ  j ƒ  ƒ |  _ t |  j j j |  j  d |  j j	 ƒ ƒ d k r: Pq: q: Wt
 ‚ t d |  j  d	 |  j j ƒ |  _ | ƒ  g |  _ t j d
 t j | ƒ  g ƒ ƒ |  _ t j | ƒ  g ƒ |  _ d  S(   Nc              s!   t  j ˆ  j d d ƒ d d ƒ S(   Nt   sizei   i   i   (   R	   t   colt   random_double(    (   t   mersenne_twister(    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   random_vector   s    c              s   ˆ  j  ƒ  d d t j S(   Ni   i   (   R   t   matht   pi(    (   R   (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   random_angle   s    id   t   pivott   normali    gü©ñÒMbP?t   sitest   alignment_Tt   qE(   R   t   xrangeR   t   revolute_alignmentt	   normalizet   At   absR   t	   cos_angleR   t   RuntimeErrorR   t   T0bt   It   wellst   revoluteR	   R   t   Jt   qd(   t   OR   R   R   t   i_trial(    (   R   sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   __init__   s    		2!$(   t   __name__t
   __module__R*   (    (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR      s   t
   revolute_zc           B   s   e  Z d d „ Z RS(   i   g      @c         C   s´   t  j | d d f ƒ | g |  _ t j d | d t  j d ƒ ƒ |  _ t d |  j d |  j j ƒ |  _ |  j |  _	 t j
 d	 t  j t j d
 g ƒ ƒ |  _ t  j d g ƒ |  _ d  S(   Ni    i   g      @R   R   i   R   R   R   g       @gUUUUUUå?(   i    i    i   (   R	   R   R   R   R   R   R   R"   R#   R$   R%   R   R   R&   R'   (   R(   R   t   x(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR*   +   s    "	!(gUUUUUUÕ?(   R+   R,   R*   (    (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR-   )   s   t
   revolute_xc           B   s   e  Z d  „  Z RS(   c         C   s´   t  j d d d f ƒ | g |  _ t j d | d t  j d ƒ ƒ |  _ t d |  j d |  j j ƒ |  _ |  j |  _	 t j
 d	 t  j t j d
 g ƒ ƒ |  _ t  j d g ƒ |  _ d  S(   Ni   g      @i    i   R   R   R   R   R   g      @gUUUUUUå?gUUUUUUÕ?(   i   i    i    (   R	   R   R   R   R   R   R   R"   R#   R$   R%   R   R   R&   R'   (   R(   R   (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR*   7   s    "	!((   R+   R,   R*   (    (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR/   5   s   t   revolute_simulationc           B   sJ   e  Z d  „  Z d „  Z d „  Z d „  Z d „  Z d d „ Z d „  Z RS(   c         C   s™  g  |  _  | d k r[ xst | ƒ D]2 } t d | ƒ } d | | _ |  j  j | ƒ q" Wn0| d k r-| d k sy t ‚ t d t j d ƒ ƒ } d | _ |  j  j | ƒ | d k rê t	 d t j d ƒ ƒ } d | _ |  j  j | ƒ n  | d	 k r‹t d t j d ƒ ƒ } d | _ |  j  j | ƒ q‹n^ | d
 k r…| d k sKt ‚ t d t j d ƒ d d ƒ } d | _ |  j  j | ƒ n t
 ‚ |  j ƒ  d  S(   Nt   randomR   iÿÿÿÿt   zigzagi   R   i    i   i   t   singularR.   (   i    i    i    (   i    i    i   (   i   i    i   (   i    i    i    (   t   bodiesR   R   t   parentt   appendt   AssertionErrorR-   R	   R   R/   R!   t!   energies_and_accelerations_update(   R(   R   t   NBt   configt   ibt   B(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR*   C   s4    					c      	   C   s-  t  d |  j ƒ } d  g t |  j ƒ } g  |  j D] } | j ^ q2 } t | | | ƒ |  _ |  j ƒ  d  } d d d g } t j	 | | | | |  j
 | d t ƒ} t j	 | | | | |  j | d t ƒ} t | | ƒ sÞ t ‚ | |  _ t | | | ƒ }	 t |  j | j |	 ƒ }
 t |
 |  j ƒ s)t ‚ d  S(   NR4   i    t   f_ext_in_ff(   R   R4   t   Nonet   lenR'   R   t   e_kint   e_pot_and_f_ext_updateR   t   FDabt   f_ext_fft   Truet   f_ext_bft   FalseR
   R7   t   qddt   FDab_X0t   check_transformationst   Ttreet   e_pot(   R(   t   modelt   qR<   R'   t   taut	   grav_accnt   qdd_using_f_ext_fft   qdd_using_f_ext_bft   X0st	   e_pot_vfy(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR8   `   s     
!!	c         C   sÍ  g  |  _  d |  _ g  |  _ g  |  _ x�|  j D]„} | j j | j j | j j	 } | j
 d k rl d  } n |  j  | j
 } | | } |  j  j | ƒ t j d | j d | j d | j d | j d | ƒ } t j d | j d | j d | j d | j d | ƒ } t | | ƒ st ‚ t j d | j d | j d | j d | j d | ƒ } t j d | j d | j d | j d | j d | ƒ } |  j j | ƒ |  j j | ƒ |  j | 7_ q. W|  j |  j |  _ d  S(   Ni    iÿÿÿÿR   R$   R   R&   t   AJA_tree(   t   AJA_accuRK   RC   RE   R4   R   t   Tb0R&   t   TspR"   R5   R>   R6   R   t   potential_energyR   R$   t   potential_energy_bfR
   R7   t   potential_f_ext_fft   potential_f_ext_bfR@   t   e_tot(   R(   R<   t   AJART   t   e_pot_fft   e_pot_bft   f_ext_using_fft   f_ext_using_bf(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyRA   u   s0    				 	
	-	-	-	-c         C   s|   xk t  |  j |  j ƒ D]T \ } } | j j d | j d | d | ƒ | _ | j j d | j d | ƒ | _ q W|  j ƒ  d  S(   NR'   RG   t   delta_t(   t   zipR4   RG   R&   t   time_step_velocityR'   t   time_step_positionR8   (   R(   Rb   R<   RG   (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   dynamics_step�   s    "'%c         C   s    t  d |  j ƒ } d  g t |  j ƒ } g  |  j D] } t j d ƒ ^ q2 } g  |  j D] } t j d ƒ ^ qW } d d d g } t j | | | | |  j | ƒ S(   NR4   i    (   i    (   i    (	   R   R4   R>   R?   R	   R   R   t   IDRE   (   R(   RL   RM   R<   R'   RG   RO   (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt	   d_pot_d_q–   s    %%g�íµ ÷Æ°>c         C   sË   g  } x´ |  j  D]© } g  } | j } x[ | | g D]L } t j d t j | j d | f ƒ ƒ | _ |  j ƒ  | j |  j	 ƒ q3 W| | _ | j t j | d | d d | f ƒ ƒ q W|  j
 ƒ  | S(   NR   i    i   i   (   R4   R&   R   R%   R	   R   R   RA   R6   RK   R8   (   R(   t   epst   resultR<   t   fst   J_origt
   signed_eps(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt    d_pot_d_q_via_finite_differencesž   s    	,
	1
c         C   sR   |  j  } |  j ƒ  } |  j ƒ  } t | | ƒ s6 t ‚ t |  j  | ƒ sN t ‚ d  S(   N(   RG   Rh   Rn   R
   R7   (   R(   t   qdd_origt   anat   fin(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   check_d_pot_d_q¬   s
    	(	   R+   R,   R*   R8   RA   Rf   Rh   Rn   Rr   (    (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyR0   A   s   					c         C   s·   d  g |  j } d  g |  j } x� t |  j ƒ D] } t j |  j | | | ƒ \ } } | |  j | | | <|  j | d k r’ | | | | <q0 | | | |  j | | | <q0 W| S(   Niÿÿÿÿ(   R>   R9   R   R   t   jcalct   pitcht   XtreeR5   (   RL   RM   R'   t   Xupt   X0t   it   XJt   S(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyRH   ³   s    #!c         C   s°  g  } x• t  |  | | ƒ D]� \ } } } | j j } | | } | j d k r] | j | ƒ n | j | | | j ƒ t | d ƒ }	 t |	 | ƒ s t ‚ q Wd }
 g  } xÿ t  |  | ƒ D]î \ } } | j j	 | j j
 | j j } | j d k rd  } | j | ƒ n$ | | j } | | } | j | ƒ xv t  | j | j ƒ D]_ \ } } | j j | } | j ƒ  | } | | } t | | ƒ s�t ‚ |
 | | j ƒ  7}
 qEWqº W|
 S(   Niÿÿÿÿi    (   Rc   R&   t   TpsR5   R6   R   R
   R7   R   RV   RW   R"   R>   R   R$   t   inverse_assuming_orthogonal_rt   dot(   R4   RJ   RR   t   T0sR<   t   TtRw   t   Tjt   Tupt
   X0_from_T0RK   RU   t   T0R]   RT   t   st   wt   s_bft   s_mv1t   s_mv2(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyRI   ¿   s4    "
 
"
i    c         C   sK  | j  ƒ  t j | j g ƒ } t j | j g ƒ } xD t | ƒ D]6 } | j d | ƒ | j | j ƒ | j | j ƒ qA W| | } | j  ƒ  |  d I| j ƒ  IJ|  d It	 | ƒ It
 | ƒ IJ|  d It	 | ƒ It
 | ƒ IJ|  d It	 | ƒ It
 | ƒ IJ|  d I| d IJ|  d I| d	 IJt j | ƒ | j ƒ  } t j
 | ƒ t j	 | ƒ }	 | d k r}d }
 n
 |	 | }
 |  d
 I| IJ|  d I|	 IJ|  d I|
 IJ|  J|  j ƒ  |  t j k rGt d t d d ƒ } x< | | | g D]+ } x | D] } | | IJqW| d IJqûW| j ƒ  t d c d 7<n  |
 S(   NRb   s   energy samples:s   e_pot min, max:s   e_kin min, max:s   e_tot min, max:s   start e_tot:i    s   final e_tot:iÿÿÿÿs   ave:s   range:s   relative range:s
   tmp%02d.xyR…   t   &i   (   Rr   R   t   doubleRK   R@   R   Rf   R6   R   t   mint   maxt   sumt   flusht   syst   stdoutt   opent   plot_numbert   close(   t   outt   n_dynamics_stepsRb   t   simt   e_potst   e_kinst   i_stept   e_totst   avet   ranget   relative_ranget   ft   est   e(    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   exercise_simß   sD    


""" 	


c      	   C   sJ   |  d I| IJt  d | d | d | ƒ } t d |  d | d | d | ƒ S(	   Ns   config:R   R9   R:   R”   R•   Rb   R–   (   R0   R¡   (   R”   R   R•   Rb   R9   R:   R–   (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   exercise_revolute_sim  s    	gü©ñÒMbP?i   c         C   sí   t  j d d ƒ } t  j ƒ  } xq t | ƒ D]c } | j t d |  d | d | d | d d | g t | d ƒ d	 d
 d d g t | d ƒ ƒ ƒ q+ W|  d IJ| j ƒ  j d |  d d ƒ |  t	 j
 k	 rå t  j | ƒ d k  så t ‚ n  |  Jd  S(   Nt   seedi    R”   R   R•   Rb   R9   i   R:   R3   R2   R1   i   s   relative ranges:t   prefixs     ga2U0*©C?(   R   R   RŠ   R   R6   R¢   R‹   t   min_max_meant   showR�   R�   RŒ   R7   (   R”   t   n_trialsR•   Rb   R9   R   t   relative_rangesR)   (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   exercise_revolute  s    $
c         C   s¦   t  |  ƒ d
 k s t ‚ t  |  ƒ d k rB d } d } t ƒ  } n; t d t |  d ƒ ƒ } t d t |  d ƒ ƒ } t j } t ƒ  t d | d | d | ƒ d	 GHd  S(   Ni    i   i   i   i   R”   R§   R•   t   OK(   i    i   (	   R?   R7   R   RŒ   t   intR�   R�   R   R©   (   t   argsR§   R•   R”   (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   run"  s    	t   __main__i   ($   t
   __future__R    t   scitbx.rigid_body.protoR   R   t   scitbx.rigid_body.proto.utilsR   R   R   R   R   t   scitbx.array_familyR   t   scitbxR	   t   libtbx.test_utilsR
   t   libtbx.utilsR   R   R   R�   t   objectR   R-   R/   R0   RH   RI   R’   R¡   R¢   R©   R­   R+   t   argv(    (    (    sS   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_joint_lib.pyt   <module>   s.   "r				#		