ó
¡¼™\c           @   sØ   d  d l  m Z d  d l m Z m Z m Z m Z m Z m Z m	 Z	 d  d l
 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  d l m Z d „  Z d „  Z d „  Z d	 „  Z d
 S(   iÿÿÿÿ(   t   range(   t   cost   Matrixt   sint   zerost   tant   pit   symbols(   t   trigsimpt   simplifyt   solve(	   t   crosst   dott   dynamicsymbolst   KanesMethodt   inertiat   inertia_of_point_masst   Pointt   ReferenceFramet	   RigidBody(   t   warns_deprecated_sympyc    J      C   si
  t  d ƒ \ }  } } } } } t  d ƒ \ } } } t d ƒ }	 g  |	 D] }
 |
 j |  ƒ ^ qF } t d ƒ } g  | D] } | j |  ƒ ^ qt } t t | d g t | ƒ ƒ ƒ } t d ƒ } t t | d g t | ƒ ƒ ƒ } t d ƒ } | j d d	 |	 d
 | j g ƒ } | j d d	 |	 d | j	 g ƒ } | j d d	 |	 d | j
 g ƒ } | j | | d
 | j	 | d | j
 | d | j ƒ | j | | j | ƒ j |  | ƒ t | j | ƒ | j | ƒ ƒ ƒ t d ƒ } | j | | d
 | j	 | d | j
 | d | j ƒ | j d |	 d | j | t |	 d ƒ | j
 ƒ } | j | | d | j	 | d | j
 | d | j ƒ | j | | j | ƒ j |  | ƒ t | j | ƒ | j | ƒ ƒ ƒ | d
 | j | d | j	 | d | j
 } | j | ƒ j |  | ƒ t | j | ƒ | j | ƒ ƒ } t g  | D]" } t | | j | ƒ | ƒ ^ q9t | | j | ƒ | j ƒ g ƒ } t | | ƒ } t | j i d
 | d 6d
 | d 6ƒ | ƒ } | j i d
 | d 6d
 | d 6ƒ g  | | D]! } | j | ƒ j | | ƒ ^ qï} g  | | D]! } | j | ƒ j | | ƒ ^ q!} g  | | D]! } | j | ƒ j | | ƒ ^ qS}  t t | | j | j ƒ |	 d g ƒ }! t g  | D]P }" t | j | ƒ | j | ƒ t | j | ƒ | j | ƒ ƒ |" ƒ j ƒ  ^ q¯ƒ }# t | j | ƒ | j | ƒ ƒ }$ |$ j |  | ƒ t | j | ƒ |$ ƒ }% t g  | D]" }" t | j | ƒ |% |" ƒ ^ q[ƒ }& t d d ƒ }' xR t d ƒ D]D }( x; t | | ƒ D]) \ }) } |# |( j | ƒ |' |( |) f <q¹Wq¢W|' d  d  … d  d … f }* |' d  d  … d d … f }+ |' d  d  … d d  … f }, |* j  |, ƒ }- |+ j! ƒ  |- }. |. d  d  … d  d … f t | d  ƒ }/ t t | d |/ ƒ ƒ }0 | | | j }1 | | j	 | | j
 | | j }2 t g  t | |  ƒ D]( \ }3 }4 t |1 |3 ƒ t |2 |4 ƒ ^ qèƒ }5 | | j | ƒ }6 t" | | | | ƒ }7 t |7 | j# | ƒ ƒ t | j | ƒ t |7 | j | ƒ ƒ ƒ }8 t g  t | | ƒ D]( \ }9 }: t |6 |9 ƒ t |8 |: ƒ ^ q–ƒ }; |5 d  d … d  d  … f j$ |5 d d  … d  d  … f ƒ |. j% |5 d d … d  d  … f }< |; d  d … d  d  … f j$ |; d d  … d  d  … f ƒ |. j% |; d d … d  d  … f }= |= j | ƒ j |0 ƒ j | ƒ j i | t& |	 d ƒ |	 d 6ƒ j ƒ  }> |7 | f }? t' d | | | |? ƒ }@ |@ g }A | |1 f }B | |2 f }C |B |C g }D t( | d |	 d  d | d  d | d |	 d d |! d | d d |# d | ƒ}E t) ƒ   |E j* |D |A ƒ \ }F }G Wd  QX|G j | ƒ j |0 ƒ j | ƒ j i | t& |	 d ƒ |	 d 6ƒ j ƒ  }H |E j+ ƒ  }I t |< ƒ j ƒ  |F j ƒ  k s
t, ‚ t |= j |I ƒ ƒ j ƒ  |G j ƒ  k s5
t, ‚ t- t |> ƒ j ƒ  ƒ t- |H j ƒ  ƒ k se
t, ‚ d  S(   Ns   t r m g I Js   Fx Fy Fzs   q:4s   u:6g        s   ua:3t   Nt   At   Axisi    t   Bi   t   Ci   t   Pt   Oi   i   i   i	   i   t   disct   q_indt   u_indt   kd_eqst   q_dependentt   configuration_constraintst   u_dependentt   velocity_constraintst   u_auxiliary(.   R   R   t   difft   dictt   zipt   lenR   t	   orientnewt   zt   xt   yt   set_ang_velt   set_ang_acct
   ang_vel_inR   R   t   set_velt	   locatenewR   t   set_acct   velt   pos_fromR   R   R
   t   subst   updatet   expandt   accR   R    t	   enumeratet   row_joint   invR   t
   ang_acc_int   col_joint   TR   R   R   R   t   kanes_equationst   kindiffdictt   AssertionErrorR	   (J   t   tt   rt   mt   gt   It   Jt   Fxt   Fyt   Fzt   qt   qit   qdt   ut   uit   udt   ud_zerot   uat   ua_zeroR   R   R   R   R   R   t   w_c_n_qdt   v_o_n_qdt   uvt   kindiffst   qd_kdt   steady_conditionst   partial_w_Ct   partial_v_Ot   partial_v_Pt   f_ct   ait   f_vt   v_o_nt   a_o_nt   f_at   M_vt   it   jt   M_v_it   M_v_dt   M_v_auxt	   M_v_i_auxt   A_rst   u_dept
   u_dep_dictt   F_Ot   F_Pt   pv_ot   pv_pt   Fr_ut   R_star_Ot   I_C_Ot   T_star_Ct   pvt   pavt	   Fr_star_ut   Fr_ct	   Fr_star_ct   Fr_star_steadyt
   iner_tupleR   t   bodyListt   F_ot   F_pt	   forceListt   kanet   frt   frstart   frstar_steadyt   kdd(    (    sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane2.pyt   test_aux_dep
   s¦    """""""9!#969D/:/&.#222+]!(5	%*#>,>\8$2	 	
!)$-c    C      C   sÇ  t  d ƒ \ }  } } t  d d d ƒ\ } } } t  d ƒ \ } } } }	 }
 t d ƒ \ } } } } } } } t d ƒ \ } } } } } } } } t d ƒ \ } } } t d ƒ \ } } } t d	 ƒ }  |  j d
 d | |  j g ƒ }! |! j d d |  |! j g ƒ }" |" j |  | |" j | |" j ƒ |" j d d | |" j g ƒ }# |" j d d | |" j g ƒ }$ |# j |" |	 |" j ƒ |$ j |" |
 |" j ƒ t d ƒ }% |% j	 |" d ƒ |% j	 |  | |" j ƒ |% j
 d | |" j ƒ }& |% j
 d | |" j | |" j ƒ }' x' |& |' g D] }( |( j |% |  |" ƒ qW|% j
 d | |" j ƒ }) |% j
 d | |" j ƒ }* |% j
 d | |" j ƒ }+ x* |) |* |+ g D] }( |( j |% |  |" ƒ q…W|* j
 d | |" j ƒ }, |+ j
 d | |" j ƒ }- |, j |* |  |# ƒ |- j |+ |  |$ ƒ | | | |	 | |
 g }. g  |, |- g D]! }( t |( j |  ƒ |" j ƒ ^ q$}/ t |" | | | d | d ƒ }0 t |# | | | ƒ }1 t |$ | | | ƒ }2 t d |) |" | |0 |) f ƒ }3 t d |* |# | |1 |* f ƒ }4 t d |+ |$ | |2 |+ f ƒ }5 t |  d |  | | g d | | g d |. d |	 |
 g d |/ d  | g ƒ}6 |& | | |  j f |' | |" j | |" j | |" j f g }7 |3 |4 |5 g }8 t ƒ   |6 j |7 |8 ƒ \ }9 }: Wd  QXt |/ |	 |
 g ƒ }; t | d! | | d! | d! d! | | | d! d! | | d! | j | ƒ | | | | | d! | d! | | d! | j | ƒ | | | d! d g ƒ }< t |: j |; ƒ j i d | 6ƒ ƒ j ƒ  j ƒ  } |< | j ƒ  t d" d ƒ k s½t ‚ g  }= x… t |3 |4 |5 g |0 |1 |2 g ƒ D]b \ }> }? |? t |> j |> j j  |% ƒ |> j! ƒ }@ |= j" t d# |> j |> j! |> j |@ |% f ƒ ƒ qåWt ƒ   |6 j |7 |= ƒ \ }A }B Wd  QXt |B j |; ƒ j i d | 6ƒ ƒ j ƒ  } |< | j ƒ  t d" d ƒ k sÃt ‚ d  S($   Ns   q1:4t   leveli   s   u1:6s   u' R, M, g, e, f, thetas   a b mA mB IA J K ts	   Q1, Q2 Q3s   IA22 IA23 IA33t   FR   t   axisR   R   R   t   Di    s   S*t   Qs   A*s   B*s   C*s   B^s   C^t   rbAt   rbBt   rbCR   R   R   R"   R#   R$   i   i   t    (#   R   R   R   R)   R,   R+   R-   R*   R   R0   R1   t   v2pt_theoryR   R3   R   R   R   R   R?   R
   R   R%   R   R5   t   doitR7   R   RA   R'   R   t   masst
   masscenterR4   t   framet   append(C   t   q1t   q2t   q3t   q1dt   q2dt   q3dt   u1t   u2t   u3t   u4t   u5t   u_primet   Rt   MRE   t   et   ft   thetat   at   bt   mAt   mBt   IARG   t   KRB   t   Q1t   Q2t   Q3t   IA22t   IA23t   IA33R‡   R   R   R   R   t   pDt   pS_start   pQt   pt   pA_start   pB_start   pC_start   pB_hatt   pC_hatt   kdet   vct	   inertia_At	   inertia_Bt	   inertia_CR‹   RŒ   R   t   kmt   forcest   bodiesR   t   fr_start   vc_mapt   fr_star_expectedt   bodies2t   rbt   I_starRF   t   fr2t   fr_star2(    (    sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane2.pyt   test_non_central_inertiaÁ   s€    !$"$4*A
]91%.
+c    =      C   só  t  d ƒ \ }  } } t  d d d ƒ\ } } } t  d ƒ \ } } } t d ƒ \ }	 }
 } } } } } t d ƒ \ } } } } } } } } t d ƒ \ } } } t d ƒ \ } } } t d	 ƒ } | j d
 d | | j g ƒ } | j d d |  | j g ƒ }  |  j | | |  j | |  j ƒ |  j d d | |  j g ƒ }! |  j d d | |  j g ƒ }" t d ƒ }# |# j	 |  d ƒ |# j	 | | |  j ƒ |# j
 d | |  j ƒ }$ |# j
 d | |  j |
 |  j ƒ }% |# j
 d | |  j ƒ }& |# j
 d | |  j ƒ }' |# j
 d | |  j ƒ }( x0 |$ |% |& |' |( g D] }) |) j |# | |  ƒ q-W|' j
 d |
 |  j ƒ }* |( j
 d |
 |  j ƒ }+ |* j |' | |! ƒ |+ j |( | |" ƒ g  |* |+ g D]! }) t |) j | ƒ |  j ƒ ^ q±}, |, | | g 7}, t |, | | | g ƒ }- x< t |- j ƒ  ƒ D]( \ }. }/ |/ j | ƒ |- |. j | ƒ <qWt |  | | | d | d ƒ }0 t |! | | | ƒ }1 t |" | | | ƒ }2 t d |& |  | |0 |& f ƒ }3 t d |' |! | |1 |' f ƒ }4 t d |( |" | |2 |( f ƒ }5 t | |  | | g | | g d |, d | g ƒ}6 |$ | | | j f |% | |  j | |  j | |  j f g }7 |3 |4 |5 g }8 t | | | | | t | ƒ t |  ƒ | | | t | ƒ t |  ƒ | | | t | ƒ | | | |
 g ƒ }9 t | d | | d |
 d d | | | d d | | d | j | ƒ | | | | | d | d | |
 d | j | ƒ | | | d d g ƒ }: t ƒ   |6 j |7 |8 ƒ \ }; }< Wd  QX|; j ƒ  |9 j ƒ  k sÄt ‚ |: t |< ƒ j ƒ  t d d ƒ k sït ‚ d  S(   Ns   q1:4R†   i   s   u1:4s   u' R, M, g, e, f, thetas   a b mA mB IA J K ts   IA22 IA23 IA33s   Q1 Q2 Q3R‡   R   Rˆ   R   R   R   R‰   i    s   S*RŠ   s   A*s   B*s   C*s   B^s   C^R‹   RŒ   R   R   R$   i   i   (   R   R   R   R)   R,   R+   R-   R*   R   R0   R1   R   R   R3   R
   t   listt   itemsR%   R   R   R   R   R   R   R   R?   R7   RA   R   R   (=   R•   R–   R—   R˜   R™   Rš   R›   Rœ   R   R    R¡   R¢   RE   R£   R¤   R¥   R¦   R§   R¨   R©   Rª   RG   R«   RB   R¯   R°   R±   R¬   R­   R®   R‡   R   R   R   R   R²   R³   R´   R¶   R·   R¸   Rµ   R¹   Rº   R»   t   kde_mapt   kt   vR½   R¾   R¿   R‹   RŒ   R   RÀ   RÁ   RÂ   t   fr_expectedRÅ   R   RÃ   (    (    sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane2.pyt   test_sub_qdot&  sj    !$"$4 0A'.]9
c    $      C   s[  t  d ƒ \ }  } } } } } } t d ƒ } t d d d ƒ} t d ƒ }	 t d ƒ }
 |
 j d d | d	 |
 j g ƒ } | j d
 d t d | d | j g ƒ } | j d d | d | j g ƒ } t d ƒ } | j |
 d	 ƒ | j	 d | d |
 j | d |
 j
 ƒ } | j |
 | j | ƒ j | |
 ƒ ƒ | j | d	 ƒ | j	 d d	 ƒ } | j | d	 ƒ | j	 d | | j
 ƒ } | j | d	 ƒ | j | d	 ƒ | j | |
 | ƒ | j | |
 | ƒ | |
 j | |
 j
 | |
 j } | |  |
 j } | | f | | f g } g  | D] } | j |
 ƒ | @^ q} | | d 7} g  t |	 | ƒ D] \ } } | | ^ qT} t |
 | |	 | ƒ } t ƒ   | j | g  ƒ \ } } Wd  QX|	 d  } t t |	 ƒ t | ƒ ƒ } g  |
 j |
 j
 g D] } | j |
 ƒ | @^ qæ} t |
 | | | d | d | ƒ}  t ƒ   |  j | g  ƒ \ }! } Wd  QXt | |  | t | d ƒ | | t | d	 ƒ | t | d	 ƒ t | d ƒ | | t | d	 ƒ | t | d	 ƒ | | g ƒ }" t | |  | t | d ƒ d	 d	 g ƒ }# t | j ƒ  ƒ t |" j ƒ  ƒ k s-t ‚ t |! j ƒ  ƒ t |# j ƒ  ƒ k sWt ‚ d  S(   Ns   g m Px Py Pz R ts   q:5R†   i   s   u:5R   t   B_primeR   i    R   i   R   R   R¡   i   i   s   C^s   C*R"   R#   (   R   R   R   R)   R*   R   R+   R   R0   R1   R,   R4   R%   R   R/   R'   R   R   R?   RÌ   t   setR3   R   R   R   R   R   R7   RA   ($   RE   RD   t   Pxt   Pyt   PzR¡   RB   RK   RM   RN   R   RÓ   R   R   t   pOt   pRRº   t   pCst   R_C_hatt   R_CsRÁ   RV   t   u_exprRO   R£   R»   t   km1t   fr1t   _t   u_indepRk   R¼   t   km2RÉ   t   fr1_expectedt   fr2_expected(    (    sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane2.pyt   test_sub_qdot2  s`    !"*",%#&,

2
6'*N(   t   sympy.core.compatibilityR    t   sympy.core.backendR   R   R   R   R   R   R   t   sympyR   R	   R
   t   sympy.physics.mechanicsR   R   R   R   R   R   R   R   R   t   sympy.utilities.pytestR   R…   RË   RÒ   Rå   (    (    (    sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane2.pyt   <module>   s   4@	·	e	Y