ó
¡¼™\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
 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 e d „  ƒ Z d S(   iÿÿÿÿ(   t   range(   t   evalft   symbolst   pit   sint   cost   sqrtt   acost   Matrix(   t   ReferenceFramet   dynamicsymbolst   inertiat   KanesMethodt	   RigidBodyt   Pointt   dott   msubs(   t   slowt	   ON_TRAVISt   skipt   warns_deprecated_sympyc    m         s¯
  t  r t d ƒ n  t d ƒ \ }  } } } t d d ƒ \ } } } } t d ƒ \ } }	 }
 } } } t d d ƒ \ } } } } } } t d ƒ \ } } } } t d ƒ \ } } } t d ƒ \ } } } } t d ƒ \ } }  }! }" t d	 ƒ \ }# }$ }% }& t d
 ƒ \ }' }( }) }* t d ƒ \ }+ }, }- }. t d ƒ }/ |/ j d d |  |/ j g ƒ }0 |0 j d d | |0 j g ƒ }1 |1 j d d | | |1 j g ƒ }2 t d ƒ }3 |2 j d d | |2 j g ƒ }4 |2 j d d | |2 j g ƒ }5 |5 j d d | |5 j g ƒ }6 t d ƒ }7 t	 d ƒ }8 |8 j
 d | |1 j ƒ }9 |9 j
 d | |2 j ƒ }: |9 j
 d | |2 j | |2 j ƒ }; |: j
 d | |5 j | |5 j ƒ }< |: j
 d | |5 j | |5 j ƒ }= |= j
 d | t |5 j |0 j ƒ |5 j |0 j j ƒ  ƒ }> |0 j |/ | |0 j ƒ |1 j |0 |	 |1 j ƒ |3 j |2 |
 |2 j ƒ |2 j |1 | |2 j ƒ |5 j |2 | |5 j ƒ |7 j |5 | |5 j ƒ |8 j |/ d ƒ |9 j |8 |/ |3 ƒ |: j |9 |/ |2 ƒ |; j |9 |/ |2 ƒ |< j |: |/ |5 ƒ |= j |: |/ |5 ƒ |> j |= |/ |7 ƒ t |4 |" |# |$ d d |% ƒ |; f }? t |6 |& |' |( d d |) ƒ |< f }@ t |2 | | | ƒ |9 f }A t |5 |  |! |  ƒ |= f }B t d |; |2 |+ |? ƒ }C t d |< |5 |, |@ ƒ }D t d  |9 |3 |. |A ƒ }E t d! |= |7 |- |B ƒ }F | | | |	 | | | | g }G |> j |/ ƒ |0 j @|> j |/ ƒ |0 j @|> j |/ ƒ |0 j @g }H |> j |8 ƒ |0 j @g }I |; |+ |* |0 j f |< |, |* |0 j f |= |- |* |0 j f |9 |. |* |0 j f g }J |C |D |E |F g }K t |/ d" |  | | g d# | g d$ |I d% |	 |
 | g d& | | | g d' |H d( |G ƒ}L t ƒ   |L j |J |K ƒ \ }M }N Wd  QXd) }O d* }P t j t d+ t d, ƒ }Q d- }R t j |R t |Q ƒ |P t |Q ƒ ƒ }S d. }T d) }U d/ }V d/ }W d0 }X t j |T t |Q ƒ |S |P |O t |Q ƒ ƒ }Y t j |V |O |U t |Q ƒ t |Q ƒ t |Q ƒ ƒ }Z t j |U t |Q ƒ |V |O |U t |Q ƒ t |Q ƒ t |Q ƒ ƒ }[ t j |X |P ƒ }\ t j |T |W ƒ }] t j t |\ d+ |] d+ ƒ ƒ }^ t j |T t |Q ƒ |P |O t |Q ƒ ƒ }_ t j |S |^ t t d+ |Q t |\ |^ ƒ ƒ ƒ }` t j |^ t t d+ |Q t |\ |^ ƒ ƒ |_ ƒ }a t d1 ƒ }b i% |P | 6|O | 6|Q | 6|S | 6|_ | 6|Y | 6|a | 6|` | 6|Z | 6|[ | 6d2 | 6d3 | 6d4 |  6d5 |! 6d6 |& 6d7 |' 6d8 |( 6d9 |) 6d: |" 6d; |# 6d< |$ 6d= |% 6d> |, 6d? |+ 6d@ |- 6d+ |. 6dA |* 6d |  6d | 6d | 6d | 6d | 6d |	 6|b |O |
 6d | 6d | 6|b |P | 6}c |L j ƒ  d }d |L j }e t  |e |c ƒ }f t  |d |L j! ƒ  |c ƒ }g |f j ƒ  }f |g j ƒ  }g |f j" ƒ  |g }h |h j# d d+ d> dB g d d+ d@ dC g ƒ }i t$ d d dD d g d d d dD g dE dF |b d+ dG dH |b dI |b g dJ dK |b d+ dL dM |b dN |b g g ƒ }j dO ‰  xX t% dB ƒ D]J }k |j j& |b |k ƒ |i j& |b |k ƒ }l t' ‡  f dP †  |l Dƒ ƒ s]
t( ‚ q]
Wd  S(Q   Ns   Too slow for travis.s   q1 q2 q4 q5i   s   u1 u2 u3 u4 u5 u6s   WFrad WRrad htangle forkoffsets   forklength framelength forkcg1s   forkcg3 framecg1 framecg3 Iwr11s   Iwr22 Iwf11 Iwf22 Iframe11s"   Iframe22 Iframe33 Iframe31 Ifork11s   Ifork22 Ifork33 Ifork31 gs   mframe mfork mwf mwrt   Nt   Yt   Axist   Rt   Framet   WRt	   TempFramet   Forkt   TempForkt   WFt   WR_contt   WR_mct   Steert   Frame_mct   Fork_mct   WF_mct   WF_conti    t	   BodyFramet   BodyForkt   BodyWRt   BodyWFt   q_indt   q_dependentt   configuration_constraintst   u_indt   u_dependentt   velocity_constraintst   kd_eqsg333333Ó?gffffffÖ?i   i
   g{®Gáz´?gR¸…ëQð?gÍÌÌÌÌÌì?gffffffæ?t   vgÉå?¤ß®?g¸…ëQ¸¾?g–C‹lçûÁ?gìQ¸…ëÑ?g´)"Ã*®?g¸…ëQ¸®?gwJëÿ|?góÈ<÷~?gffffff"@i   gffffff@g333333Ài   iU   i   g…ëQ¸ž#@i   i   g      ð?g'ž^·Ãú"@gltg±„ì¿gîW‚÷êIâ?g>Äèî„»¿g{Ä_*'Õ¿gŸÆE_p'@g•¨ñ%Œÿ¿gø¤è>@gH·Üj@gaGúÍ­Àgê-™—q=c         3   s!   |  ] } t  | ƒ ˆ  k  Vq d  S(   N(   t   abs(   t   .0t   x(   t   eps(    sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane3.pys	   <genexpr>"  s    ()   R   R   R
   R   R	   t	   orientnewt   zR4   t   yR   t	   locatenewR   t	   normalizet   set_ang_velt   set_velt   v2pt_theoryR   R   t   velt   pos_fromR   R   t   kanes_equationsR   R   R   R   R   R   R   t	   linearizet   mass_matrix_fullR   t   kindiffdictt   invt   extractR   R    t   subst   allt   AssertionError(m   t   q1t   q2t   q4t   q5t   q1dt   q2dt   q4dt   q5dt   u1t   u2t   u3t   u4t   u5t   u6t   u1dt   u2dt   u3dt   u4dt   u5dt   u6dt   WFradt   WRradt   htanglet
   forkoffsett
   forklengtht   framelengtht   forkcg1t   forkcg3t   framecg1t   framecg3t   Iwr11t   Iwr22t   Iwf11t   Iwf22t   Iframe11t   Iframe22t   Iframe33t   Iframe31t   Ifork11t   Ifork22t   Ifork33t   Ifork31t   gt   mframet   mforkt   mwft   mwrR   R   R   R   R   R   R   R   R   R   R    R!   R"   R#   R$   R%   t   Frame_It   Fork_It   WR_It   WF_IR&   R'   R(   R)   t   kdt   conlist_speedt   conlist_coordt   FLt   BLt   KMt   frt   frstart   PaperRadReart   PaperRadFrontt   HTAt
   TrailPapert   raket   PaperWbt   PaperFrameCgXt   PaperFrameCgZt   PaperForkCgXt   PaperForkCgZt   FrameLengtht   FrameCGNormt
   FrameCGPart   tempat   tempbt   tempct
   PaperForkLt
   ForkCGNormt	   ForkCGParR1   t   val_dictt   forcing_lint   MM_fullt	   MM_full_st   forcing_lin_st   Amatt   At   Rest   it   error(    (   R5   sG   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_kane3.pyt   test_bicycle   s   !"$%$$"?		
(/5C!+33

	*#,"N(   t   sympy.core.compatibilityR    t   sympyR   R   R   R   R   R   R   R   t   sympy.physics.mechanicsR	   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_kane3.pyt   <module>   s   ::"