ó
¡¼™\c           @   sÝ   d  d l  m Z d  d l m Z m Z m Z m Z m Z m Z m	 Z	 m
 Z
 m Z d  d l m Z d  d l m Z m Z m Z m Z m Z m Z m Z m Z d „  Z d „  Z d „  Z d „  Z d	 „  Z d
 „  Z d „  Z d S(   iÿÿÿÿ(   t   warns_deprecated_sympy(	   t   cost   expandt   Matrixt   sint   symbolst   tant   sqrtt   St   zeros(   t   simplify(   t   dynamicsymbolst   ReferenceFramet   Pointt	   RigidBodyt   KanesMethodt   inertiat   Particlet   dotc          C   sÈ  t  d ƒ \ }  } t  d d ƒ \ } } t d ƒ \ } } } t d ƒ } t d ƒ } | j | | | j ƒ | | g }	 | | |  | | | j f g }
 t d | | ƒ } | g } t | |  g | g |	 ƒ } t ƒ   | j	 |
 | ƒ Wd  QX| j
 } | j } | j ƒ  | } t | d ƒ t |  | | | | ƒ k sFt ‚ t | j ƒ  | j j | j ƒ ƒ t d d ƒ k s€t ‚ | j d	 t ƒ d t d d g | | | | g g ƒ k sÄt ‚ d  S(
   Ns   q ui   s   m c kt   Nt   Pt   pai    i   t   A_and_B(   R   R   R   R   t   set_velt   xR   R   R    t   kanes_equationst   mass_matrixt   forcingt   invR   t   AssertionErrorR
   t   rhst   mass_matrix_fullt   LUsolvet   forcing_fullR	   t	   linearizet   TrueR   (   t   qt   ut   qdt   udt   mt   ct   kR   R   t   kdt   FLR   t   BLt   KMt   MMR   R   (    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   test_one_dof   s(    #	
		3.c          C   so  t  d ƒ \ }  } } } t  d d ƒ \ } } } } t d ƒ \ } }	 }
 } } t d ƒ } t d ƒ } t d ƒ } | j | | | j ƒ | j | | | | j ƒ | | | | g } | | |  |	 | | | |
 | | j f | | | |
 | | j f g } t d | | ƒ } t d | | ƒ } | | g } t | d	 |  | g d
 | | g d | ƒ} t ƒ   | j	 | | ƒ Wd  QX| j
 } | j } | j ƒ  | } t | d ƒ t | |  |	 | | | |
 | | ƒ k sçt ‚ t | d ƒ t | |  |	 | d | | d |
 | | ƒ k s1t ‚ t | j ƒ  | j j | j ƒ ƒ t d d ƒ k skt ‚ d  S(   Ns   q1 q2 u1 u2i   s   m c1 c2 k1 k2R   t   P1t   P2t   pa1t   pa2t   q_indt   u_indt   kd_eqsi    i   i   (   R   R   R   R   R   R   R   R   R    R   R   R   R   R   R   R
   R   R   R    R!   R	   (   t   q1t   q2t   u1t   u2t   q1dt   q2dt   u1dt   u2dR(   t   c1t   c2t   k1t   k2R   R1   R2   R+   R,   R3   R4   R-   R.   R/   R   R   (    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   test_two_dof+   s2    4*
		C+c          C   s«  t  d ƒ \ }  } t  d d ƒ \ } } t d ƒ \ } } } t d ƒ } t d ƒ } | j | | | t |  ƒ | j | | t |  ƒ | j ƒ | | g }	 | | | | j f g }
 t	 d | | ƒ } | g } t
 | |  g | g |	 ƒ } t ƒ   | j |
 | ƒ Wd  QX| j } | j } | j ƒ  | } | j ƒ  t | d ƒ t | | t |  ƒ ƒ k smt ‚ t | j ƒ  | j j | j ƒ ƒ t d d ƒ k s§t ‚ d  S(	   Ns   q ui   s   m l gR   R   R   i    i   (   R   R   R   R   R   R   R   R   t   yR   R   R    R   R   R   R   R
   R   R   R   R   R    R!   R	   (   R$   R%   R&   R'   R(   t   lt   gR   R   R+   R,   R   R-   R.   R/   R   R   (    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt	   test_pendU   s(    ?	
		
1c    $   
   C   s  t  d ƒ \ }  } } } } } t  d d ƒ \ } } } }	 }
 } t d ƒ \ } } } t d ƒ } | j d d |  | j g ƒ } | j d d | | j g ƒ } | j d d | | j g ƒ } | j | ƒ } | j | | | j | | j | | j ƒ t	 d	 ƒ } | j
 | d
 ƒ | j d | | j ƒ } | j | | | ƒ t | | d | d | d | d | d | d ƒ } g  | D]" } t | j | ƒ | | ƒ ^ q~} | | | | j f g } t d | | | | | f ƒ } | g } t | d |  | | g d | | | g d | ƒ} t ƒ   | j | | ƒ Wd  QX| j } | j } | j ƒ  | } | j ƒ  }  | j |  ƒ } | j ƒ  | j ƒ  t d | | | | d | t | ƒ d | t | ƒ d | d | | d | d | | t | ƒ g ƒ j ƒ  k s
t ‚ t | j ƒ  | j j  | j! ƒ ƒ t" d d ƒ k sDt ‚ | j# d t$ ƒ d
 }! |! j i d | 6d | 6d | 6ƒ j i d
 |  6d
 | 6d
 | 6d
 | 6d
 | 6ƒ }" d d  l% }# |# j& |" j i d t' d ƒ | 6ƒ ƒ j( ƒ  i d t) d
 ƒ 6k sþt ‚ d  S(   Ns   q1 q2 q3 u1 u2 u3i   s   r m gR   t   Yt   Axist   Lt   Rt   Ci    t   Dmci   i   t   BodyDR5   R6   R7   i   i   iþÿÿÿi   R   iÿÿÿÿ(*   R   R   R   t	   orientnewt   zR   RE   t
   ang_vel_int   set_ang_velR   R   t	   locatenewt   v2pt_theoryR   R   R   R   R    R   R   R   R   t   kindiffdictt   subsR
   R   R   R   R   R   R   R   R    R!   R	   R"   R#   t   sympyt   sympifyR   t	   eigenvalsR   ($   R8   R9   t   q3R:   R;   t   u3R<   R=   t   q3dR>   R?   t   u3dt   rR(   RG   R   RI   RK   RL   t   w_R_N_qdRM   RN   t   It   uvR+   t	   ForceListRO   t   BodyListR.   R/   R   R   t   kddt   At	   A_uprightRX   (    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   test_rolling_discn   sF    !-9/	0
		
1Z.Pc    (      C   so  t  d ƒ \ }  } } } } } t  d d ƒ \ } } } }	 }
 } t  d ƒ \ } } } } t  d d ƒ \ } } t d ƒ \ } } } t d ƒ } | j d d |  | j g ƒ } | j d	 d | | j g ƒ } | j d
 d | | j g ƒ } | j | ƒ } | j | | | j | | j | | j ƒ t	 d ƒ } | j
 | | | j | | j | j Aƒ | j d | | j ƒ } | j | | | ƒ | j | | | ƒ t | | d | d | d | d | d | d ƒ } g  | D]" } t | j | ƒ | | ƒ ^ q×} | | | | j f | | | j | | j | j Af g } t d | | | | | f ƒ }  |  g }! t | d |  | | g d | | | | | g d | ƒ}" t ƒ   |" j | |! ƒ \ }# }$ Wd  QX|# j i d | 6d | 6ƒ j i d | 6d | 6ƒ }# |$ j i d | 6d | 6ƒ j i d | 6d | 6ƒ }$ t | d |  | | g d | | | g d | d | | g ƒ}% t ƒ   |% j | |! ƒ \ }& }' Wd  QX|& j i d | 6d | 6ƒ j i d | 6d | 6ƒ }& |' j i d | 6d | 6ƒ j i d | 6d | 6ƒ }' |$ j ƒ  |' j ƒ  |# |& j ƒ  t d d d d d g ƒ k s:t ‚ |$ |' j ƒ  t d d d d d g ƒ k skt ‚ d  S(   Ns   q1 q2 q3 u1 u2 u3i   s   u4, u5, f1, f2s   u4, u5s   r m gR   RI   RJ   RK   RL   RM   RN   i   i   RO   R5   R6   R7   i    t   u_auxiliary(   R   R   R   RP   RQ   R   RE   RR   RS   R   R   RT   RU   t   a2pt_theoryR   R   R   R   R    R   RW   R
   R   R   R   ((   R8   R9   R[   R:   R;   R\   R<   R=   R]   R>   R?   R^   t   u4t   u5t   f1t   f2t   u4dt   u5dR_   R(   RG   R   RI   RK   RL   R`   RM   RN   Ra   Rb   R+   Rc   RO   Rd   R.   t   frt   frstart   KM2t   fr2t   frstar2(    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   test_aux¹   sJ    !-)9/=	-	
44-
44

1c        
   C   s‹  t  d ƒ }  t  d ƒ \ } } t  d ƒ \ } } } t d ƒ } t  d ƒ \ } } }	 t d ƒ \ }
 } t d d ƒ \ } } t d ƒ \ } } t d d ƒ \ } } t d	 ƒ } t d
 ƒ } | j | d | | j g ƒ | j | | | j ƒ t d ƒ } | j d |
 | j ƒ } | j d | | j	 ƒ } | j
 | d ƒ | j
 | | | j ƒ | j | | | ƒ t d | | ƒ } t d | | | t | | | |	 ƒ | f ƒ } | | | | g } | | g } | | j	 |  | f | | j	 |  | f | | j | |
 | f | | j | f g } t | |
 | g | | g | ƒ } t ƒ   | j | | ƒ \ } } Wd  QX| j } | d |	 k s‡t ‚ d  S(   NRG   s   k lss   a mA mCt   Fs   Ix Iy Izs   q1 q2i   s   u1 u2R   Rf   RJ   t   ORM   t   Aoi    t   Cartt   Pendulumi   (   i   i   (   R   R   R   t   orientRQ   RS   R   RT   R   RE   R   RU   R   R   R   R   R    R   R   R   (    t   gravityR*   t   lst   at   mAt   mCRw   t   Ixt   Iyt   IzR8   R9   R<   R=   R:   R;   R>   R?   R   Rf   Rx   RM   Ry   Rz   R{   t   kindiffst   bodyListt	   forceListt   kmRq   Rr   t   mm(    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   test_parallel_axisï   s@    -!
	c    !         sË  t  d ƒ \ }  } t  d d ƒ \ } } t d ƒ \ } } } t d ƒ } t d ƒ } | j | | | j ƒ | | g }	 | | |  | | | j f g }
 t d | | ƒ } | g } t | |  g | g |	 ƒ ‰  ˆ  j | ƒ d t	 d g ƒ k sù t
 ‚ ˆ  j d | d	 d  ƒ d t	 d g ƒ k s*t
 ‚ ˆ  j | d	 d  ƒd t	 d g ƒ k sXt
 ‚ ˆ  j | ƒ d t	 d g ƒ k s€t
 ‚ d
 d l m } | t ‡  f d †  ƒ t  d ƒ \ } } } } t  d d ƒ \ } } } } t d ƒ \ } } } } } t d ƒ } t d ƒ } t d ƒ } | j | | | j ƒ | j | | | | j ƒ | | | | g }	 | | | | | | | | | | j f | | | | | | j f f }
 t d | | ƒ } t d | | ƒ } | | f } t | d | | g d | | g d |	 ƒ‰  ˆ  j | |
 ƒ ˆ  j } ˆ  j } | j ƒ  | }  t |  d ƒ t | | | | | | | | | ƒ k s}t
 ‚ t |  d ƒ t | | | | d | | d | | | ƒ k sÇt
 ‚ d  S(   Ns   q ui   s   m c kR   R   R   i    t   bodiest   loadsiÿÿÿÿ(   t   raisesc              s   ˆ  j  d ƒ S(   Ns	   bad input(   t   _form_fr(    (   R.   (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   <lambda>A  t    s   q1 q2 u1 u2s   m c1 c2 k1 k2R1   R2   R3   R4   R5   R6   R7   i   (   R   R   R   R   R   R   R   R   R   R   R   t   Nonet   sympy.utilities.pytestR   t
   ValueErrorR   R   R   R   (!   R$   R%   R&   R'   R(   R)   R*   R   R   R+   R,   R   R-   R   R8   R9   R:   R;   R<   R=   R>   R?   R@   RA   RB   RC   R1   R2   R3   R4   R/   R   R   (    (   R.   sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   test_input_format(  sN    #	(1.(4*		C+N(   R’   R    t   sympy.core.backendR   R   R   R   R   R   R   R   R	   RX   R
   t   sympy.physics.mechanicsR   R   R   R   R   R   R   R   R0   RD   RH   Rh   Rv   RŠ   R”   (    (    (    sF   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane.pyt   <module>   s   @:	 	*		K	6	9