ó
¡¼™\c           @   sì   d  d l  m Z m Z m Z m Z m Z m Z m Z d  d l 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 m Z m Z d  d l m Z m Z e d „  ƒ Z d „  Z d „  Z d „  Z d	 „  Z d
 „  Z d S(   iÿÿÿÿ(   t   symbolst   Matrixt   cost   sint   atant   sqrtt   S(   t   solvet   simplify(   t   dynamicsymbolst   ReferenceFramet   Pointt   dott   crosst   inertiat   KanesMethodt   Particlet	   RigidBodyt
   Lagrangiant   LagrangesMethod(   t   slowt   warns_deprecated_sympyc    D      C   sG	  t  d ƒ \ }  } } } } t d ƒ \ } } } } }	 }
 } g  | D] } | j |  ƒ ^ qD \ } } } } } } } t d ƒ } t d ƒ \ } } } } } } } g  | D] } | j |  ƒ ^ qª \ } } } } }  }! t d ƒ }" t d ƒ }# |" j d d | |" j g ƒ }$ |$ j d	 d | |$ j g ƒ }% |% j d
 d | |% j g ƒ }& |# j	 d | |" j |	 |" j |
 |" j ƒ }' |& j
 |" ƒ }( |% j
 |" ƒ }) |& j |" | |% j | |% j | |% j ƒ |' j |# ƒ j |" ƒ }* |' j |" | |& j | |& j | |& j ƒ |' j	 d | |% j ƒ }+ |+ j |' |" |& ƒ t |
 t |' j |+ ƒ |" j ƒ g ƒ }, t g  |& D] }- t |+ j |" ƒ |- ƒ ^ qfƒ }. t g  |% D]" }- t |( |& j
 |" ƒ |- ƒ ^ q—g  |" D]" }- t |* |' j |" ƒ |- ƒ ^ qÃƒ }/ t |/ | ƒ }0 |% j |" |) j |0 ƒ ƒ |& j |" |& j
 |" ƒ j |% ƒ t |% j
 |" ƒ |& j
 |" ƒ ƒ ƒ | | |$ j }1 | | d d }2 | | d d }3 t |& |2 |3 |2 ƒ }4 t d |' |& | |4 |' f ƒ }5 |5 g }6 |' |1 f g }7 t |" | | | | |	 g | | | g d |/ d |
 g d |, d | | | g d |. ƒ}8 t ƒ   |8 j |7 |6 ƒ \ }9 }: Wd  QX|8 j ƒ  }; |; j |, k sut ‚ |; j |. k sŠt ‚ |; j  |. j |  ƒ k s¨t ‚ t |; j! |; j" | ƒ }< x( | D]  } |< | |0 | k sÈt ‚ qÈWt# |; j$ |; j% |9 |: ƒ t d d d g ƒ k s%t ‚ i | t& | ƒ |
 6}= i d | 6t' | ƒ | | | 6t& | ƒ | | 6| t' | ƒ | | t& | ƒ | 6d | 6| t' | ƒ | | t' | ƒ | 6}> i d | 6| t' | ƒ | | t& | ƒ | 6| t' | ƒ | | t' | ƒ | 6d | 6}? i d | t' | ƒ d | t' d | ƒ | d d d t& | ƒ | | d | 6d | 6d | 6| t' | ƒ t' | ƒ | | t' | ƒ | d | 6| d | t' | ƒ d | t' d | ƒ | d d d t& | ƒ | | d |  6| t' | ƒ t& | ƒ | | t& | ƒ | d |! 6}@ |; j( d |= |> |? |@ g d t) d t) ƒ \ }$ }% i d | 6d | 6d | 6d | 6d | 6}A t d d d d d d d d g d d d d d d d d g d d d d d d d d g t' | ƒ | d d d d t' | ƒ t& | ƒ d g t& | ƒ | d d d d t& | ƒ t' | ƒ d g d t* d ƒ d d d d d d d | d g d d d d d d d d g d d d d d d | d d g g ƒ }B t g  ƒ }C |$ j |A ƒ |B k sêt ‚ |% j |A ƒ |C k s	t ‚ |$ j |A ƒ j | d t+ d ƒ ƒ j, ƒ  i d d 6k sC	t ‚ d  S(   Ns	   t r m g vs   q1:7s   u:6s   u1:7t   Nt   NOt   At   Axist   Bt   Ct   COt   Pi   i   t   Disct   kd_eqst   q_dependentt   configuration_constraintst   u_dependentt   velocity_constraintsi    i   i   t   op_pointt   A_and_BR   i   iþÿÿÿi   i   (-   R    R	   t   diffR
   R   t	   orientnewt   zt   xt   yt	   locatenewt
   ang_vel_int   set_ang_velt   pos_fromt   dtt   set_velt   v2pt_theoryR   R   t   velR   t   subst   set_ang_accR   R   R   R   R   t   kanes_equationst   to_linearizert   f_ct   AssertionErrort   f_vt   f_at   f_0t   f_1R   t   f_2t   f_3R   R   t	   linearizet   TrueR   R   t	   eigenvals(D   t   tt   rt   mt   gt   vt   q1t   q2t   q3t   q4t   q5t   q6t   qt   qit   q1dt   q2dt   q3dt   q4dt   q5dt   q6dt   qdt   ut   u1t   u2t   u3t   u4t   u5t   u6t   uit   u1dt   u2dt   u3dt   u4dt   u5dt   u6dR   R   R   R   R   R   t   w_c_n_qdt   w_b_n_qdt	   v_co_n_qdR   R7   t   uvR9   t   kindiffst   qdotst   F_COt   It   Jt   I_C_COR   t   BLt   FLt   KMt   frt   fr_start
   linearizert   solt   q_opt   u_opt   qd_opt   ud_opt   upright_nominalt   A_solt   B_sol(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt    test_linearize_rolling_disc_kane	   sš    "8"4/--(1/3A	-
9
$'
$$
T5U90)33-(c          C   sÎ  t  d ƒ }  t  d ƒ } t  d d ƒ } t d ƒ \ } } } d } t d ƒ } t d ƒ } | j | d ƒ | j d	 d
 |  | j g ƒ }	 |	 j | | | j ƒ | j d | |	 j	 ƒ }
 |
 j
 | | |	 ƒ t d |
 | ƒ } t | | g ƒ } | | | j	 } t | d |  g d | g d | ƒ} t ƒ  ) | j |
 | f g | g ƒ \ } } Wd  QX| j d t d t ƒ \ }	 } } |	 t d d g d t |  ƒ | d g g ƒ k s²t ‚ | t g  ƒ k sÊt ‚ d  S(   NRG   RW   i   s   L, m, tgš™™™™™#@R   s   N*i    R   t   axisR   t   pPt   q_indt   u_indR   R%   R   gš™™™™™#À(   R	   R    R
   R   R0   R'   R(   R-   R+   R)   R1   R   R   R   R   R5   R?   R@   R   R8   (   RG   RW   RO   t   LRD   RB   RE   R   t   pNR   R   R~   t   kdet   RRp   Rq   t   frstarR   t   inp_vec(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt$   test_linearize_pendulum_kane_minimal„   s*    $
*!8c           C   s  t  d ƒ \ }  } t  d d d ƒ\ } } t  d ƒ \ } } t  d d d ƒ\ } } t d ƒ \ } }	 }
 d } t d ƒ } t d ƒ } | j | d	 ƒ t | |  ƒ } | j d
 d | | j g ƒ } | j d |  | j	 | | j
 ƒ } t d | |	 ƒ } t | | | | g ƒ } t | | | g ƒ } | j | | j | ƒ j | ƒ j | ƒ ƒ t | j | ƒ j ƒ  | g ƒ } t | j | ƒ j | ƒ j | j	 ƒ g ƒ } | j ƒ  | j |
 ƒ } | j ƒ  |	 | | j	 } t | d | g d | g d |  g d | g d | d | d | d | ƒ} t ƒ  ) | j | | f g | g ƒ \ } } Wd  QXi | |  6d	 | 6} i d	 | 6d	 | 6} i d	 | 6d	 | 6} | j d | | | g d t d t ƒ \ } } } | j ƒ  t d	 d g d | d	 g g ƒ k sôt ‚ | t g  ƒ k st ‚ d  S(   Ns   q1:3t   leveli   s   u1:3s   L, m, tgš™™™™™#@R   s   N*i    R   R}   t   P1R~   R   R€   R    R"   R!   R#   t   acceleration_constraintsR   R$   R%   R   gš™™™™™#À(   R	   R    R
   R   R0   R   R'   R(   R+   R)   R*   R   R   R   R.   R/   R3   t	   magnitudeR2   t   expressR   R   R&   R   R   R5   R?   R@   t   expandR8   (    RG   RH   RO   RP   RW   RX   R^   R_   R   RD   RB   RE   R   R‚   t   theta1R   R   R~   Rƒ   t   dq_dictR7   R9   R:   R„   Rp   Rq   R…   Ru   Rv   Rx   R   R†   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt'   test_linearize_pendulum_kane_nonminimalª   sF    $
+"-

$
*4c          C   s—  t  d ƒ }  t  d d ƒ } t d ƒ \ } } } d } t d ƒ } t d ƒ } | j | d ƒ | j d d	 |  | j g ƒ } | j | | | j ƒ | j d
 | | j	 ƒ }	 |	 j
 | | | ƒ t d |	 | ƒ }
 t | |
 ƒ } t | |  g d |	 | | | j	 f g d | ƒ} | j ƒ  | j |  g | g d t ƒ\ } } } | t d d g d t |  ƒ | d g g ƒ k s{t ‚ | t g  ƒ k s“t ‚ d  S(   NRG   i   s   L, m, tgš™™™™™#@R   s   N*i    R   R}   R   R~   t	   forcelistt   frameR%   gš™™™™™#À(   R	   R    R
   R   R0   R'   R(   R-   R+   R)   R1   R   R   R   t   form_lagranges_equationsR?   R@   R   R   R8   (   RG   RO   R   RD   RB   RE   R   R‚   R   R   R~   t   Lagt   LMR   R†   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt(   test_linearize_pendulum_lagrange_minimalë   s$    2
'8c       	   C   sN  t  d ƒ \ }  } t  d d d ƒ\ } } t d ƒ \ } } } d } t d ƒ } t d ƒ }	 |	 j | d ƒ t | |  ƒ }
 | j d	 d
 |
 | j g ƒ } |	 j d |  | j	 | | j
 ƒ } | j | | j |	 ƒ j | ƒ ƒ t d | | ƒ } t |  d | d | d g ƒ } t | | ƒ } t | |  | g d | d | | | | j	 f g d | ƒ} | j ƒ  i | |  6d | 6d | 6d | 6d | j | ƒ 6d | j | ƒ 6} | j d | ƒ } | j | ƒ | j | g | g |  g | g d | d t ƒ\ } } } | t d d g d | d g g ƒ k s2t ‚ | t g  ƒ k sJt ‚ d  S(   Ns   q1:3Rˆ   i   s   L, m, tgš™™™™™#@R   s   N*i    R   R}   R‰   R~   i   t
   hol_coneqsR‘   R’   R$   R%   gš™™™™™#À(   R	   R    R
   R   R0   R   R'   R(   R+   R)   R*   R.   R/   R   R   R   R   R“   R&   t   solve_multiplierst   updateR?   R@   R8   (   RG   RH   RO   RP   R   RD   RB   RE   R   R‚   RŽ   R   R   R~   R7   R”   R•   R$   t   lam_opR   R†   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt+   test_linearize_pendulum_lagrange_nonminimal
  s.    $"#;
B!.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 ƒ } t d	 ƒ } | j | d
 ƒ | j	 d | | j ƒ } | j
 | | | ƒ t | |	 d | d |	 d | d |	 d | d ƒ } t d | | |	 | | f ƒ } |	 |
 | t | ƒ | _ t | | ƒ } t | | ƒ } | j ƒ  i d
 |  6d
 | 6d
 | 6d
 | 6d
 | 6d
 | j ƒ  6d
 | j ƒ  6d
 | j ƒ  6} | j d | d | d | d t ƒ d
 } t d
 d
 d
 d d
 d
 g d
 d
 d
 d
 d d
 g d
 d
 d
 d
 d
 d g d
 d
 d
 d
 d | d
 g d
 d |
 d | d
 d | d d
 d
 g d
 d
 d
 d
 d
 d
 g g ƒ } | | k s¿t ‚ d  S(   Ns   q1 q2 q3i   s   r m gR   t   YR   R   R„   R   i    t   Dmci   i   t   BodyDR   t   qd_indR$   R%   iúÿÿÿiüÿÿÿi   i   (   R	   R    R
   R'   R(   R)   R*   R   R0   R+   R1   R   R   R   t   potential_energyR   R   R“   R&   R?   R@   R   R8   (   RG   RH   RI   RM   RO   RP   RQ   RU   RC   RD   RE   R   Rœ   R   R„   R   R   Rk   Rž   R”   t   lR$   R   Rt   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt$   test_linearize_rolling_disc_lagrange+  s8    9
*()N(    t   sympy.core.backendR    R   R   R   R   R   R   t   sympyR   R   t   sympy.physics.mechanicsR	   R
   R   R   R   R   R   R   R   R   R   t   sympy.utilities.pytestR   R   R|   R‡   R   R–   R›   R¢   (    (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_linearize.pyt   <module>   s   4L{	&	A		!