ó
¡¼™\c           @   s«   d  d l  m Z m Z m Z m Z m Z m Z m Z m Z d  d l	 m
 Z
 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 S(	   iÿÿÿÿ(   t   dynamicsymbolst   ReferenceFramet   Pointt	   RigidBodyt   LagrangesMethodt   Particlet   inertiat
   Lagrangian(	   t   symbolst   pit   sint   cost   tant   simplifyt   Functiont
   Derivativet   Matrixc          C   s   t  d ƒ \ }  } t  d d ƒ \ } } t d ƒ \ } } } } } t d ƒ }	 |	 j d d t d | |	 j g ƒ }
 |
 j d d | |
 j g ƒ } t d	 ƒ } | j |	 | |
 j ƒ | | d d | j | j B} t	 d
 | | | | | f ƒ } | | | |  t
 | ƒ | _ t |	 | ƒ } |  | g } |  | | g } t | | d | ƒ} | j ƒ  | j ƒ  } | j ƒ  | d d | t
 | ƒ d k sœt ‚ d  S(   Ns   y thetai   s   m g R l alphat   Nt   At   Axisi   t   Bt   Dot   Dt
   hol_coneqsi   (   R    R   R   t	   orientnewR	   t   zR   t   set_velt   xR   R
   t   potential_energyR   R   t   form_lagranges_equationst   rhsR   t   AssertionError(   t   yt   thetat   ydt   thetadt   mt   gt   Rt   lt   alphaR   R   R   R   t   IR   t   Lt   qR   R   (    (    sJ   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyt   test_disc_on_an_incline_plane   s&    & 	

c          C   sS  t  d ƒ \ }  } t  d d ƒ \ } } t d ƒ \ } } } t d ƒ } | j d d |  | j g ƒ } | j | | | j ƒ t d ƒ }	 |	 j | d	 ƒ |	 j d
 | | j	 ƒ }
 |
 j
 |	 | | ƒ t d |
 | ƒ } | | | t |  ƒ | _ t | | ƒ } t | |  g ƒ } | j ƒ  | j ƒ  } | d | t |  ƒ | k sOt ‚ d  S(   Ns   q us   q u i   s   l m gR   R   R   t   Oi    t   Pt   Pa(   R    R   R   R   R   t   set_ang_velR   R   t	   locatenewR   t   v2pt_theoryR   R   R   R   R   R   R   R
   R   (   R+   t   ut   qdt   udR'   R$   R%   R   R   R-   R.   R/   R*   t   lmt   RHS(    (    sJ   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyt   test_simp_pen8   s"    	
c       	   C   s#  t  d ƒ \ }  } t  d d d ƒ\ } } t d ƒ \ } } } d } t d ƒ } t d ƒ }	 |	 j | d ƒ |	 j d	 |  | j | | j ƒ }
 |
 j | |
 j |	 ƒ j	 | ƒ ƒ t
 d
 |
 | ƒ } t |  d | d | d g ƒ } t | | ƒ } t | |  | g d | d |
 | | | j f g d | ƒ} | j ƒ  | j d } t | t |  | | ƒ d | d | |  g | t | | | ƒ d | | g g ƒ } | j | k s¶t ‚ t d |  d | d d | d d |  d | d | d | g ƒ } | j d d ƒ | k st ‚ d  S(   Ns   q1:3t   leveli   s   L, m, tgš™™™™™#@R   s   N*i    t   P1t   pPi   R   t	   forcelistt   framegš™™™™™3@i   t   sol_typeR   (   i    i    (   R    R   R   R   R   R1   R   R    t   pos_fromt   dtR   R   R   R   R   t   lam_vecR   t   eomR   t   solve_multipliers(   t   q1t   q2t   q1dt   q2dR*   R$   t   tR%   R   t   pNR.   R;   t   f_ct   Lagt   LMt   lam1t   eom_solt   lam_sol(    (    sJ   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyt   test_nonminimal_penduluma   s*    $"##
-+Kc          C   s¢  t  d ƒ \ }  } t  d d ƒ \ } } t  d d ƒ \ } } t  d ƒ \ } } t  d d ƒ \ } }	 t d ƒ \ }
 } } t d ƒ } | j d d |  | j g ƒ } | j d	 d | | j g ƒ } | j | | | j ƒ | j | | | j ƒ t d
 ƒ } | j d |
 | j ƒ } | j d |
 | j ƒ } | j	 | d ƒ | j
 | | | ƒ | j
 | | | ƒ t d | | ƒ } t d | | ƒ } | | |
 t |  ƒ | _ | | |
 t |  ƒ | | |
 t | ƒ | _ t | | | ƒ } t | |  | g d | | g ƒ} | j ƒ  t |
 | d | t |  ƒ |
 t |  ƒ t | ƒ | |
 t |  ƒ t | ƒ | d |
 t | ƒ t |  ƒ | d |
 t |  ƒ t | ƒ | d |
 | | j d ƒ d k sÎt ‚ t |
 | | t | ƒ |
 t |  ƒ t | ƒ | |
 t |  ƒ t | ƒ | d |
 t | ƒ t |  ƒ | d |
 t |  ƒ t | ƒ | |
 | | j d ƒ d k sƒt ‚ | j | | g k sžt ‚ d  S(   Ns   q1 q2i   i   s   u1 u2s   l m gR   R   R   R   R-   R.   R&   i    t   ParPt   ParRt   bodies(   R    R   R   R   R   R0   R   R1   R   R   R2   R   R   R   R   R   R   R   R
   RB   R   RS   (   RD   RE   RF   RG   t   q1ddt   q2ddt   u1t   u2t   u1dt   u2dR'   R$   R%   R   R   R   R-   R.   R&   RQ   RR   R*   R6   (    (    sJ   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyt   test_dub_pen   s:    	2!

³
«c          C   sT  t  d ƒ \ }  } } t  d d ƒ \ } } } t d ƒ \ } } } t d ƒ }	 |	 j d d |  |	 j g ƒ }
 |
 j d d | |
 j g ƒ } | j d d | | j g ƒ } t d	 ƒ } | j |	 d
 ƒ | j	 d | | j ƒ } | j
 | |	 | ƒ t | | d | d | d | d | d | d ƒ } t d | | | | | f ƒ } | | | t | ƒ | _ t |	 | ƒ } |  | | g } t d ƒ }  t d ƒ } t d ƒ } t | | ƒ } | j ƒ  | j ƒ  } | j ƒ  t d ƒ } | j d d !d
 d | | d d d
 g k st ‚ | d j ƒ  d | t | | ƒ ƒ | d t d | | ƒ ƒ t |  | ƒ | ƒ d t | | ƒ ƒ t | | ƒ | ƒ t |  | ƒ | ƒ d | k s¶t ‚ | d d t | | ƒ ƒ t |  | ƒ | ƒ d t | | ƒ ƒ t | | ƒ | ƒ d t |  | ƒ | ƒ t | | ƒ ƒ t | | ƒ | ƒ k sPt ‚ d  S(   Ns   q1 q2 q3i   s   r m gR   t   YR   R*   R&   t   Ci    t   Dmci   i   t   BodyDRD   RE   t   q3RH   i   i   i   iøÿÿÿi   i
   iûÿÿÿ(   R    R   R   R   R   R   R    R   R   R1   R2   R   R   R   R   R   R   R   R   R   R   t   mass_matrixR   R
   R   R   (   RD   RE   R_   RF   RG   t   q3dt   rR$   R%   R   R[   R*   R&   R\   R]   R)   R^   RK   R+   R'   R7   RH   (    (    sJ   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyt   test_rolling_disc°   s>    9

5}tN(   t   sympy.physics.mechanicsR    R   R   R   R   R   R   R   t   sympyR   R	   R
   R   R   R   R   R   R   R,   R8   RP   RZ   Rc   (    (    (    sJ   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyt   <module>   s   :@	0	)		1