Ë
    7^(ho:  ã                   óˆ   — d dl mZ d dlmZ d dlmZ d dl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 d dlmZ ed„ «       Zy	)
é    )Úpi)Úsymbols)Úsqrt)ÚacosÚsinÚcos)ÚMatrix)ÚReferenceFrameÚdynamicsymbolsÚKanesMethodÚinertiaÚPointÚ	RigidBodyÚdot)Úslowc                  óü  ‡l— 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|0j                  dd||0j
                  g«      }1|1j                  dd||z   |1j                  g«      }2t        d«      }3|2j                  dd| |2j                  g«      }4|2j                  dd||2j
                  g«      }5|5j                  dd| |5j                  g«      }6t        d«      }7t        d«      }8|8j                  d||1j                  z  «      }9|9j                  d||2j                  z  «      }:|9j                  d| |2j
                  z  ||2j                  z  z   «      };|:j                  d| |5j
                  z  ||5j                  z  z   «      }<|:j                  d||5j
                  z  ||5j                  z  z   «      }=|=j                  d|t        |5j                  |0j                  «      |5j                  z  |0j                  z
  j                  «       z  «      }>|0j                  |/||0j                  z  «       |1j                  |0|	|1j
                  z  «       |3j                  |2|
|2j                  z  «       |2j                  |1||2j                  z  «       |5j                  |2||5j
                  z  «       |7j                  |5||5j                  z  «       |8j                  |/d«       |9j                  |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|||«      |9f}At        |5| |!| «      |=f}Bt        d|;|2|+|?«      }Ct        d|<|5|,|@«      }Dt        d|9|3|.|A«      }Et        d |=|7|-|B«      }F||z
  ||	z
  ||z
  ||z
  g}G|>j!                  |/«      |0j
                  z  |>j!                  |/«      |0j                  z  |>j!                  |/«      |0j                  z  g}H|>j#                  |8«      |0j                  z  g}I|;|+ |*z  |0j                  z  f|<|, |*z  |0j                  z  f|=|- |*z  |0j                  z  f|9|. |*z  |0j                  z  fg}J|C|D|E|Fg}Kt%        |/| ||g|g|I|	|
|g|||g|H|Gd!¬"«	      }L|Lj'                  |K|J«      \  }M}Nd#}Od$}Pt(        d%z  t(        d&z  z
  j+                  «       }Qd'}R|Rt-        |Q«      z  |Pt/        |Q«      z  z
   j+                  «       }Sd(}Td#}Ud)}Vd)}Wd*}X|Tt-        |Q«      z  |S|P|Oz
  t/        |Q«      z  z
  z
  j+                  «       }Y|V|Oz
  |Ut-        |Q«      z  t/        |Q«      z  z
  t-        |Q«      z  j+                  «       }Z|Ut-        |Q«      z  |V|Oz
  |Ut-        |Q«      z  t/        |Q«      z  z
  t/        |Q«      z  z   j+                  «       }[|X|Pz
  }\|T|Wz
  }]t1        |\d%z  |]d%z  z   «      j+                  «       }^|Tt/        |Q«      z  |P|Oz
  t-        |Q«      z  z
  j+                  «       }_|S|^t-        t(        d%z  |Qz
  t3        |\|^z  «      z
  «      z  z   j+                  «       }`|^t/        t(        d%z  |Qz
  t3        |\|^z  «      z
  «      z  |_z
  j+                  «       }at        d+«      }bi ||P“||O“||Q“||S“||_“||Y“||a“||`“||Z“||[“|d,“|d-“| d.“|!d/“|&d0“|'d1“|(d2“i |)d3“|"d4“|#d5“|$d6“|%d7“|,d8“|+d9“|-d:“|.d%“|*d;“| d“|d“|d“|d“|d“|	d“|
|b|Oz  “¥|d|d||b|Pz  i¥}c|Lj5                  d<i |j7                  «       d“|	j7                  «       d“|
j7                  «       d“|j7                  «       d“|j7                  «       d“|j7                  «       d“|d“|	d“|
|b|Oz  “|d“|d“||b|Pz  “| d“|d“|d“|d“d!¬=«      \  }d}e}f|dj9                  |c«      }g|ej9                  |c«      }h|gj+                  «       }g|hj+                  «       }h|gj;                  g d>¢g d>¢«      }dt=        g d?¢g d@¢dAdB|bd%z  z  dCz
  dD|bz  dE|bz  gdFdG|bd%z  z  dHz   dI|bz  dJ|bz  gg«      }idKŠlt?        dL«      D ]=  }jijA                  b|j«      djA                  |b|j«      z
  }ktC        ˆlfdM„|kD «       «      rŒ=J ‚ y )NNzq1 q2 q4 q5é   zu1 u2 u3 u4 u5 u6zWFrad WRrad htangle forkoffsetzforklength framelength forkcg1zforkcg3 framecg1 framecg3 Iwr11zIwr22 Iwf11 Iwf22 Iframe11z"Iframe22 Iframe33 Iframe31 Ifork11zIfork22 Ifork33 Ifork31 gzmframe mfork mwf mwrÚNÚYÚAxisÚRÚFrameÚWRÚ	TempFrameÚForkÚTempForkÚWFÚWR_contÚWR_mcÚSteerÚFrame_mcÚFork_mcÚWF_mcÚWF_contr   Ú	BodyFrameÚBodyForkÚBodyWRÚBodyWFÚCRAMER)Úq_indÚq_dependentÚconfiguration_constraintsÚu_indÚu_dependentÚvelocity_constraintsÚkd_eqsÚconstraint_solverg333333Ó?gffffffÖ?é   é
   g{®Gáz´?gR¸…ëQð?gÍÌÌÌÌÌì?gffffffæ?ÚvgÉå?¤ß®?g¸…ëQ¸¾?g–C‹lçûÁ?gìQ¸…ëÑ?g´)"Ã*®?g¸…ëQ¸®?gwJëÿ|?góÈ<÷~?gffffff"@é   gffffff@g333333Àé   éU   é   g…ëQ¸ž#@T)ÚA_and_BÚop_pointÚlinear_solver)r   r2   r8   é   )r   r   ç      ð?r   )r   r   r   r=   g'ž^·Ãú"@gltg±„ì¿gîW‚÷êIâ?g>Äèî„»¿g{Ä_*'Õ¿gŸÆE_p'@g•¨ñ%Œÿ¿gø¤è>@gH·Üj@gaGúÍ­Àgê-�™—q=é   c              3   ó:   •K  — | ]  }t        |«      ‰k  –— Œ y ­w)N)Úabs)Ú.0ÚxÚepss     €úf/var/www/skyplay_api_hub/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane3.pyú	<genexpr>ztest_bicycle.<locals>.<genexpr>;  s   øè ø€ Ò/ A”3�q“6˜C•<Ñ/ùs   ƒ)"r   r   r
   Ú	orientnewÚzrB   Úyr   Ú	locatenewr   Ú	normalizeÚset_ang_velÚset_velÚv2pt_theoryr   r   ÚvelÚpos_fromr   Úkanes_equationsr   Úevalfr   r   r   r   Ú	linearizeÚdiffÚxreplaceÚextractr	   ÚrangeÚsubsÚall)mÚq1Úq2Úq4Úq5Úq1dÚq2dÚq4dÚq5dÚu1Úu2Úu3Úu4Úu5Úu6Úu1dÚu2dÚu3dÚu4dÚu5dÚu6dÚWFradÚWRradÚhtangleÚ
forkoffsetÚ
forklengthÚframelengthÚforkcg1Úforkcg3Úframecg1Úframecg3ÚIwr11ÚIwr22ÚIwf11ÚIwf22ÚIframe11ÚIframe22ÚIframe33ÚIframe31ÚIfork11ÚIfork22ÚIfork33ÚIfork31ÚgÚmframeÚmforkÚmwfÚmwrr   r   r   r   r   r   r   r   r   r   r   r    r!   r"   r#   r$   ÚFrame_IÚFork_IÚWR_IÚWF_Ir%   r&   r'   r(   ÚkdÚconlist_speedÚconlist_coordÚFLÚBLÚKMÚfrÚfrstarÚPaperRadRearÚPaperRadFrontÚHTAÚ
TrailPaperÚrakeÚPaperWbÚPaperFrameCgXÚPaperFrameCgZÚPaperForkCgXÚPaperForkCgZÚFrameLengthÚFrameCGNormÚ
FrameCGParÚtempaÚtempbÚtempcÚ
PaperForkLÚ
ForkCGNormÚ	ForkCGParr4   Úval_dictÚAÚBÚ_ÚA_sÚB_sÚResÚiÚerrorrC   sm                                                                                                               @rD   Útest_bicycler°      s±  ø€ ô, $ MÓ2�N€BˆˆB�Ü'¨°qÓ9Ñ€Cˆˆc�3Ü+Ð,?Ó@Ñ€BˆˆB��B˜Ü#1Ð2EÀqÓ#IÑ €Cˆˆc�3˜˜Sô )0Ð0PÓ(QÑ%€Eˆ5�'˜:Ü'.Ð/OÓ'PÑ$€J�˜WÜ)0Ð1RÓ)SÑ&€GˆX�x Ü$+Ð,HÓ$IÑ!€Eˆ5�%˜Ü,3Ð4XÓ,YÑ)€Hˆh˜ 'Ü#*Ð+FÓ#GÑ €GˆW�g˜qÜ%Ð&<Ó=Ñ€FˆE�3˜ô 	�sÓ€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ�K‰K˜ ¨"¨w©,¸¿¹Ð)<Ó=€EÜ	˜Ó	€BØ—‘ ¨V°w°hÀÇÁÐ5HÓI€IØ�?‰?˜6 6¨B°·±¨=Ó9€DØ�~‰~˜j¨&°G°8¸T¿V¹VÐ2DÓE€HÜ	˜Ó	€Bô �IÓ€GØ×Ñ˜g u¨q¯s©s¡{Ó3€EØ�O‰O˜G [°5·7±7Ñ%:Ó;€EØ�‰˜z¨X¨:¸¿¹Ñ+?Ø-5¸¿¹Ñ-?ñ,@ó A€Hà�o‰o˜i¨7¨°T·V±VÑ);Ø+2°T·V±VÑ+;ñ*<ó =€Gà�O‰O˜G Z°$·&±&Ñ%8¸:ÈÏÉÑ;NÑ%NÓO€EØ�o‰o˜i¨´#°d·f±f¸a¿c¹cÓ2BÀTÇVÁVÑ2KØ23·#±#ñ36ß7@±y³{ñ*Có D€Gð ‡M�M�!�R˜!Ÿ#™#‘XÔØ‡M�M�!�R˜!Ÿ#™#‘XÔØ‡N�N�5˜"˜uŸw™w™,Ô'Ø	×Ñ�a˜˜eŸg™g™Ô&Ø×Ñ�U˜B §¡™KÔ(Ø‡N�N�4˜˜dŸf™f™Ô%ð
 ‡O�O�A�qÔØ	×Ñ�g˜q "Ô%Ø	×Ñ�e˜Q Ô&Ø×Ñ˜  5Ô)Ø×Ñ˜˜q $Ô'Ø	×Ñ�e˜Q Ô%Ø×Ñ˜˜q "Ô%ô �y (¨H°hÀÀ1ÀhÓOÐQYÐZ€GÜ�h ¨°'¸1¸aÀÓIÈ7ÐS€FÜ�E˜5 %¨Ó/°Ð7€DÜ�D˜% ¨Ó.°Ð6€Dô ˜+ x°¸ÀÓH€IÜ˜ W¨d°E¸6ÓB€HÜ�x ¨¨C°Ó6€FÜ�x ¨¨C°Ó6€Fð �‰(�C˜"‘H˜c B™h¨¨b©Ð	1€Bð —[‘[ “^ a§c¡cÑ)¨7¯;©;°q«>¸A¿C¹CÑ+?ÀÇÁÈQÃÐRS×RUÑRUÑAUÐV€Mð ×%Ñ% gÓ.°·±Ñ4Ð5€Mð �f�W˜q‘[ 1§3¡3Ñ&Ð
'Ø	�5�&˜1‘*˜qŸs™sÑ"Ð#Ø	���q‘˜1Ÿ3™3‘ÐØ	���q‘˜1Ÿ3™3‘Ðð
!€Bð �X˜v vÐ	.€Bô 
�Q˜r 2 r˜lØ˜¸Ø�r˜2�,Ø˜R ˜¸=ØØ&ô
(€Bð ×%Ñ% b¨"Ó-�L€Rˆð (+€LØ'+€MÜ(*¨Q©´°b±Ñ(8×'?Ñ'?Ó'A€CØ'+€JØ*4´S¸³XÑ*=¸}ÌSÐQTËXÑ?UÑ*VÐ(W×'^Ñ'^Ó'`€DØ'+€GØ'*€MØ'*€MØ'*€LØ'*€LØ(/´°C³Ñ(8¸$ÀÈlÑ@ZÔ\_Ð`cÓ\dÑ?dÑ:dÑ(e×'lÑ'lÓ'n€KØ)6¸Ñ)EÀ}ÔUXÐY\ÓU]ÑG]Ô_bÐcfÓ_gÑFgÑ)gÔilÐmpÓiqÑ(q×'xÑ'xÓ'z€KØ(5¼¸C»Ñ(@ÀMÐT`ÑD`ÐcpÔsvÐwzÓs{Ñc{ô  Bð  CFó  Gñ  dGñ  EGô  KNð  ORó  KSñ  DSñ  )S÷  (Zñ  (Zó  (\€JØ(4°}Ñ(D€EØ(/°Ñ(<€EÜ(,¨U°A©X°e¸Q±hÑ->Ó(?×'FÑ'FÓ'H€EØ(/´°C³Ñ(8¸-ÈÑ:TÔVYÐZ]ÓV^Ñ9^Ñ(^×'eÑ'eÓ'g€JØ(,¨e´c¼"¸Q¹$¸s¹(Ä4ÈÈeÉÓCTÑ:TÓ6UÑ.UÑ(V×']Ñ']Ó'_€JØ(-´´R¸±T¸#±X¼tÀEÈ%ÁKÓ?PÑ4PÓ0QÑ(QÐR\Ñ(\×'cÑ'cÓ'e€Iô 	�‹€Að$'��}ð $'Ø�|ð$'à˜ð$'ð ˜Dð$'ð ˜Jð	$'ð
 ˜[ð$'ð ˜ð$'ð ˜ð$'ð ˜+ð$'ð ˜*ð$'ð �vð$'ð �tð$'ð �vð$'ð �tð$'ð ˜ð$'ð ˜ð$'ð  ˜ñ!$'ð" ˜ð#$'ð$ ˜#ð%$'ð& ˜"ð'$'ð( ˜#ð)$'ð* ˜$ð+$'ð, �qð-$'ð. ˜ð/$'ð0 �Qð1$'ð2 �Qð3$'ð4 �4ð5$'ð6 �Að7$'ð8 �Að9$'ð: �Að;$'ð< �Að=$'ð> �Að?$'ð@ �AðA$'ðB �A˜Ñ$ñC$'ðD �AØ�AØ�A˜Ñ%ñI$'€Hð\ �l‰lØð
ð �G‰G‹I�qð	
ð
 �G‰G‹I�qð
ð �G‰G‹I�qð
ð �G‰G‹I�qð
ð �G‰G‹I�qð
ð �G‰G‹I�qð
ð �ð
ð �ð
ð ��LÑ ð
ð �ð
ð �ð
ð ��MÑ!ð
ð  �ð!
ð" �ð#
ð$ �ð%
ð& �ð'
ð* ð/ ó �G€A€qˆ!ð: �*‰*�XÓ
€CØ
�*‰*�XÓ
€Cà
�)‰)‹+€CØ
�)‰)‹+€Cð 	�‰’L¢,Ó/€Aô Ò}Ú}Ø#Ð%7¸¸1¹Ñ%<Ð?PÑ%PÐRdÐefÑRfÐhzÐ{|Ñh|Ð}Ø#Ð'8¸¸A¹Ñ'=Ð@PÑ'PÐTdÐefÑTfÐizÐ{|Ñi|Ð}ðó @€Cð €CÜ�1‹Xò 0ˆØ—‘˜˜A“ §¡¨¨1£Ñ-ˆÜÓ/¨Ô/Õ/Ð/Ð/ñ0ó    N)Úsympy.core.numbersr   Úsympy.core.symbolr   Ú(sympy.functions.elementary.miscellaneousr   Ú(sympy.functions.elementary.trigonometricr   r   r   Úsympy.matrices.denser	   Úsympy.physics.mechanicsr
   r   r   r   r   r   r   Úsympy.testing.pytestr   r°   © r±   rD   ú<module>rº      sA   ðÝ !Ý %Ý 9ß CÑ CÝ '÷*÷ *ñ *õ &ð ñn0ó ñn0r±   