ó
t™xTc           @   s    d  Z  d d l m Z d d l m Z m Z m Z m Z m Z y d d l	 Z	 Wn  e
 k
 rp d Z	 d „  Z n Xd d l Z	 d „  Z d e f d „  ƒ  YZ d S(	   s  Based in part on Roy Featherstone's spatial_v1 matlab code:

  http://axiom.anu.edu.au/~roy/spatial/

  Version 1: January 2008 (latest bug fix: 7 October 2008)

See also: RBDA:
  Rigid Body Dynamics Algorithms.
  Roy Featherstone,
  Springer, New York, 2007.
  ISBN-10: 0387743146
iÿÿÿÿ(   t   division(   t   matrixt   cb_as_spatial_transformt   crmt   crft   kinetic_energyNc         C   s
   |  j  ƒ  S(   N(   t   inverse(   t   m(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   generalized_inverse   s    c         C   s=   t  j t j j j d |  j ƒ  d d d d ƒ j ƒ  j ƒ  ƒ S(   NR   t   relative_epsilong�íµ ÷Æ°>t   absolute_epsilon(	   R   t   sqrt   scitbxt   linalgt   eigensystemt   real_symmetrict   as_flex_double_matrixt   generalized_inverse_as_packed_ut   matrix_packed_u_as_symmetric(   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyR      s
    t   system_modelc           B   s"  e  Z d  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z	 d „  Z
 d	 „  Z d
 „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d d „ Z d „  Z d „  Z d d „ Z d d d d „ Z d d d d „ Z d „  Z d d d d „ Z RS(   s   RBDA Tab. 4.3, p. 87c         C   s‘   | |  _  d |  _ d |  _ d |  _ x\ |  j  D]Q } | j d k rU |  j d 7_ n  |  j | j j 7_ |  j | j j 7_ q. W|  j ƒ  d  S(   Ni    iÿÿÿÿi   (   t   bodiest   number_of_treest   degrees_of_freedomt   q_packed_sizet   parentt   jointt   q_sizet   flag_positions_as_changed(   t   OR   t   body(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   __init__(   s    				 c         C   s2   d  |  _ d  |  _ d  |  _ d  |  _ |  j ƒ  d  S(   N(   t   Nonet   _system_model__aja_arrayt   _system_model__jar_arrayt   _system_model__cb_up_arrayt   _system_model__xup_arrayt   flag_velocities_as_changed(   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyR   3   s
    				c         C   s   d  |  _ d  |  _ d  S(   N(   R   t!   _system_model__spatial_velocitiest   _system_model__e_kin(   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyR$   :   s    	c         C   sI   g  } x< t  |  j ƒ D]+ \ } } | j d k r | j | ƒ q q W| S(   Niÿÿÿÿ(   t	   enumerateR   R   t   append(   R   t   resultt   ibR   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   root_indices>   s
    c         C   s~   d d l  m } | j ƒ  } | j |  j ƒ x0 |  j D]% } | j | j | j j ƒ  ƒ ƒ q6 W| j	 ƒ  |  j k sz t
 ‚ | S(   Niÿÿÿÿ(   t   flex(   t   scitbx.array_familyR,   t   doublet   reserveR   R   t   extendR   t   get_qt   sizet   AssertionError(   R   R,   R)   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   pack_qE   s    #c         C   s‘   | j  ƒ  |  j k s t ‚ d } xJ |  j D]? } | j j } | j j d | | | | !ƒ | _ | | 7} q+ W| |  j k sƒ t ‚ |  j ƒ  d  S(   Ni    t   q(   R2   R   R3   R   R   R   t   new_qR   (   R   t   q_packedt   iR   t   n(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   unpack_qN   s    #c         C   sx   d d l  m } | j ƒ  } | j |  j ƒ x* |  j D] } | j | j | j ƒ ƒ q6 W| j ƒ  |  j k st t	 ‚ | S(   Niÿÿÿÿ(   R,   (
   R-   R,   R.   R/   R   R   R0   t   qdR2   R3   (   R   R,   R)   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   pack_qdX   s    c         C   s‹   | j  ƒ  |  j k s t ‚ d } xD |  j D]9 } | j j } t j | | | | !ƒ | _ | | 7} q+ W| |  j k s} t ‚ |  j ƒ  d  S(   Ni    (	   R2   R   R3   R   R   R   t   colR;   R$   (   R   t	   qd_packedR8   R   R9   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt	   unpack_qda   s    c         C   s°   g  } d g t  |  j ƒ } x� t t  |  j ƒ d d d ƒ D]l } |  j | } | | c t | | ƒ 7<| j d k r‘ | j | | | f ƒ q< | | j c | | 7<q< W| S(   Ni    i   iÿÿÿÿ(   t   lenR   t   xranget   getattrR   R(   (   R   t   attrR)   t   accuR*   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   _accumulate_in_each_treek   s    &c         C   s   |  j  d d ƒ S(   NRC   t   number_of_sites(   RE   (   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   number_of_sites_in_each_treew   s    c         C   s   |  j  d d ƒ S(   NRC   t   sum_of_masses(   RE   (   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   sum_of_masses_in_each_treez   s    c         C   s®   | d  k r |  j ƒ  } n  t j d ƒ } d } xc | D][ \ } } |  j | } | j j d | j ƒ } | d  k rz q7 n  | | | 7} | | 7} q7 W| d k r¦ d  S| | S(   Ni    R;   (   i    i    i    (   R   RG   R   R=   R   R   t   get_linear_velocityR;   (   R   RG   t   sum_vt   sum_nR*   R9   R   t   v(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   mean_linear_velocity}   s     c         C   s’   | d  k r |  j ƒ  } n  xp | D]h \ } } |  j | } | j j d | j ƒ } | d  k re q" n  | j j d | j d | | ƒ | _ q" Wd  S(   NR;   t   value(   R   RG   R   R   RJ   R;   t   new_linear_velocity(   R   RG   RO   R*   R9   R   RM   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   subtract_from_linear_velocitiesŒ   s     c         C   sŒ   |  j  d  k r… g  |  _  xj |  j D]\ } | j j | j j | j j } | j d k rn |  j  | j | } n  |  j  j	 | ƒ q" Wn  |  j  S(   Niÿÿÿÿ(
   R    R   R   t	   alignmentt   cb_b0R   t   cb_spt   cb_0bR   R(   (   R   R   t   aja(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt	   aja_array•   s    	 c         C   sš   |  j  d  k r“ |  j ƒ  } g  |  _  xl |  j D]^ } | j j j | j j j } | j	 d k r| | | | j	 j j
 ƒ  9} n  |  j  j | ƒ q. Wn  |  j  S(   Niÿÿÿÿ(   R!   R   RW   R   R   t   cb_pst   rRR   RU   R   t	   transposeR(   (   R   t   O_ajaR   t   jar(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt	   jar_arrayŸ   s    	c         C   sE   |  j  d k r> g  |  j D] } | j j | j ^ q |  _  n  |  j  S(   s   RBDA Example 4.4, p. 80N(   R"   R   R   R   RX   t   cb_tree(   R   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   cb_up_arrayª   s    /c         C   sA   |  j  d  k r: g  |  j ƒ  D] } t | ƒ ^ q |  _  n  |  j  S(   N(   R#   R   R_   R   (   R   t   cb(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt	   xup_array°   s    +c   	      C   s8  |  j  d k r1g  |  _  |  j ƒ  } x
t t |  j ƒ ƒ D]ð } |  j | } | j j } | d k rq | j } n | | j } | j	 d k r  |  j  j
 | ƒ q: | | } |  j  | j	 j } | j t j | d  ƒ } t j | | j t j | d ƒ | j j | ƒ f ƒ j ƒ  } |  j  j
 | | ƒ q: Wn  |  j  S(   s   RBDA Example 4.4, p. 80iÿÿÿÿi   N(   R%   R   R_   RA   R@   R   R   t   motion_subspaceR;   R   R(   t   elemsRY   R   R=   t   tt   crosst   resolve_partitions(	   R   R_   R*   R   t   st   vjt   cb_upt   vpt   r_va(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   spatial_velocitiesµ   s&    	 
6c         C   sm   |  j  d k rf d } xB t |  j |  j ƒ  ƒ D]( \ } } | t d | j d | ƒ 7} q. W| |  _  n  |  j  S(   s   RBDA Eq. 2.67, p. 35i    t	   i_spatialt	   v_spatialN(   R&   R   t   zipR   Rl   R   Rm   (   R   R)   R   RM   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   e_kinÌ   s    % gê-�™—q=c         C   s~   | d k s t  ‚ | d k s$ t  ‚ |  j ƒ  } | | k rp | | d } x# |  j D] } | j | 9_ qT Wn  |  j ƒ  d  S(   Ni    g      à?(   R3   Rp   R   R;   R$   (   R   t   e_kin_targett   e_kin_epsilont   O_e_kint   factorR   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   reset_e_kinÕ   s    c         C   s1   x  |  j  D] } | j j | _ q
 W|  j ƒ  d  S(   N(   R   R   t   qd_zeroR;   R$   (   R   R   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   assign_zero_velocitiesß   s    c         C   s¢   g  |  j  D] } | j ^ q
 } |  j ƒ  } xp t t | ƒ d d d ƒ D]R } |  j  | } | j d k rH | | j c | | j ƒ  | | | | 7<qH qH W| S(   Ni   iÿÿÿÿ(   R   Rm   Ra   RA   R@   R   RZ   (   R   R   R)   Ra   R8   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   accumulated_spatial_inertiaä   s    #'c         C   sô   g  } | j  } xÞ t |  j |  j ƒ  ƒ D]Ä \ } } | j j } | j j } d g | } x� t | ƒ D]‚ }	 d | |	 <t j	 | ƒ }
 d | |	 <| d  k	 r¨ | |
 }
 n  t d | d |
 ƒ } | | k  rÖ | d ƒ qf | d | d ƒ qf Wq( W| S(   Ni    i   Rm   Rn   g      à?(   R(   Ro   R   Rx   R   Rb   R   RA   R   R=   R   R   (   R   Rr   R)   t   rapR   t   asiRg   t   j_dofR;   R8   Rh   Rp   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   qd_e_kin_scalesî   s"    	%

 c      
   C   s|  | d  k r d } n2 | d k r/ |  j ƒ  d  S| d k sA t ‚ | } d d l m } | j |  j d | ƒ ƒ } |  j d k r™ | | |  j d 9} n  | d  k r½ d d  l } | j	 } n  d } xn |  j
 D]c }	 g  }
 xB |	 j j D]4 } |
 j | | d d d | | ƒ ƒ | d 7} qæ Wt j |
 ƒ |	 _ qÍ W| |  j k sIt ‚ |  j ƒ  | d  k	 rx|  j d	 | d | ƒ n  | S(
   Ni   i    iÿÿÿÿ(   R,   Rr   g      à?t   mut   sigmaRq   (   R   Rw   R3   R-   R,   R.   R|   R   t   randomt   gaussR   R   Rv   R(   R   R=   R;   R$   Ru   (   R   Rq   Rr   t   random_gausst   work_e_kin_targetR,   R|   R   t   i_qdR   t   qd_newR;   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   assign_random_velocities  s6    	
$
c         C   sù  | d k s- t | ƒ t |  j ƒ k s- t ‚ | d k sZ t | ƒ t |  j ƒ k sZ t ‚ t |  j ƒ } |  j ƒ  } |  j ƒ  } d g | } d g | } x¤t | ƒ D]–}	 |  j |	 }
 |
 j j } | d k r|
 j	 } | d k sø | |	 d k rt
 j d d ƒ } qf| |	 } nL | |
 j	 } | d k sC| |	 d k rXt
 j d d ƒ } n | | |	 } |
 j d k rª| | |	 <| d k	 rÙ| |	 c | |	 | 7<qÙn/ | |	 | |
 j | t | |	 ƒ | | |	 <|
 j | |	 t | |	 ƒ |
 j | |	 | |	 <| d k	 r¨ | |	 d k	 r¨ | |	 c | |	 8<q¨ q¨ Wd g | } x£ t | d d d ƒ D]‹ }	 |  j |	 }
 |
 j j } | d k r¢| |	 | |	 <n | j ƒ  | |	 | |	 <|
 j d k rf| |
 j c | |	 j ƒ  | |	 7<qfqfW| S(   s  RBDA Tab. 5.1, p. 96:
Inverse Dynamics of a kinematic tree via Recursive Newton-Euler Algorithm.
qdd_array is a vector of joint acceleration variables.
The return value (tau) is a vector of joint force variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext[i] is a spatial
force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    R9   i   iÿÿÿÿi   N(   R   R@   R   R3   Ra   Rl   RA   R   Rb   R;   R   t   zerosR   R   Rm   R   RZ   (   R   t	   qdd_arrayt   f_ext_arrayt	   grav_accnt   nbRa   RM   t   at   fR*   R   Rg   Rh   t   ajt	   tau_array(    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   inverse_dynamics"  sJ    --	
/2,c   
      C   s  t  | ƒ t  |  j ƒ k s! t ‚ t  |  j ƒ } |  j ƒ  } g  | D] } | ^ qC } d g | } x£ t | d d d ƒ D]‹ } |  j | } | j j }	 |	 d k r¶ | | | | <n |	 j ƒ  | | | | <| j	 d k rz | | j	 c | | j ƒ  | | 7<qz qz W| S(   s‡   
Simplified version of Inverse Dynamics via Recursive Newton-Euler Algorithm,
with all qd, qdd zero, but non-zero external forces.
    i   iÿÿÿÿN(
   R@   R   R3   Ra   R   RA   R   Rb   RZ   R   (
   R   Rˆ   RŠ   Ra   t   eRŒ   RŽ   R*   R   Rg   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   f_ext_as_tauZ  s    ! ,c         C   s’  | d k s- t | ƒ t |  j ƒ k s- t ‚ | d k sZ t | ƒ t |  j ƒ k sZ t ‚ t |  j ƒ } |  j ƒ  } |  j ƒ  } d g | } d g | } d g | }	 xt | ƒ D]ö }
 |  j |
 } | j j } | d k rì | j	 } n | | j	 } | j
 d k r0t j d d d d d d g ƒ | |
 <n t | |
 ƒ | | |
 <| j | |
 <t | |
 ƒ | j | |
 |	 |
 <| d k	 rµ | |
 d k	 rµ |	 |
 c | |
 8<qµ qµ Wd g | } d g | } d g | } x›t | d d d ƒ D]ƒ}
 |  j |
 } | j j } | d k rB| |
 | |
 <| |
 } |	 |
 | |
 <n? | |
 | | |
 <| j ƒ  | |
 } | j ƒ  |	 |
 | |
 <| d k	 r´| |
 d k	 r´| |
 c | |
 7<n  t | ƒ | |
 <| j
 d k rí| |
 | |
 } | |
 | | |
 j ƒ  } |	 |
 | | |
 | | |
 } | | j
 c | |
 j ƒ  | | |
 7<|	 | j
 c | |
 j ƒ  | 7<qíqíWd g | } d g | } xý t | ƒ D]ï }
 |  j |
 } | j j } | j
 d k r| |
 | |
 <| d k	 r#| |
 c | |
 | 7<q#n! | |
 | | j
 | |
 | |
 <| |
 | |
 | |
 j ƒ  | |
 | |
 <| d k rr| |
 c | |
 7<q›| |
 c | | |
 7<q›W| S(   s  RBDA Tab. 7.1, p. 132:
Forward Dynamics of a kinematic tree via the Articulated-Body Algorithm.
tau_array is a vector of force variables.
The return value (qdd_array) is a vector of joint acceleration variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext_array[i] is a
spatial force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    iÿÿÿÿi    i   N(   R   R@   R   R3   Ra   Rl   RA   R   Rb   R;   R   R   R=   R   Rm   R   RZ   R   (   R   RŽ   Rˆ   R‰   RŠ   Ra   RM   t   ct   iat   paR*   R   Rg   Rh   t   ut   d_invt   u_t   dt   u_d_invt   ia_t   pa_R‹   R‡   (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   forward_dynamics_abm  st    --  (#
")(!,N(   t   __name__t
   __module__t   __doc__R   R   R$   R+   R4   R:   R<   R?   RE   RG   RI   RN   RQ   RW   R]   R_   Ra   Rl   Rp   Ru   Rw   Rx   R|   R   R…   R�   R‘   Rœ   (    (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyR   %   s<   							
			
							
						
		
8	(   RŸ   t
   __future__R    t   spatial_libR   R   R   R   R   R   t   ImportErrorR   R   t   scitbx.linalg.eigensystemt   objectR   (    (    (    sT   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/featherstone.pyt   <module>   s   (	
