§
    OŠtjo:  ã                   ó�   — 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d	S )
é    )Ú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¦  «        }/|/                     dd| |/j        g¦  «        }0|0                     dd||0j        g¦  «        }1|1                     dd||z   |1j        g¦  «        }2t          d¦  «        }3|2                     dd| |2j        g¦  «        }4|2                     dd||2j        g¦  «        }5|5                     dd| |5j        g¦  «        }6t          d¦  «        }7t          d¦  «        }8|8                     d||1j        z  ¦  «        }9|9                     d||2j        z  ¦  «        }:|9                     d| |2j        z  ||2j        z  z   ¦  «        };|:                     d| |5j        z  ||5j        z  z   ¦  «        }<|:                     d||5j        z  ||5j        z  z   ¦  «        }=|=                     d|t          |5j        |0j        ¦  «        |5j        z  |0j        z
   
                    ¦   «         z  ¦  «        }>|0                     |/||0j        z  ¦  «         |1                     |0|	|1j        z  ¦  «         |3                     |2|
|2j        z  ¦  «         |2                     |1||2j        z  ¦  «         |5                     |2||5j        z  ¦  «         |7                     |5||5j        z  ¦  «         |8                     |/d¦  «         |9                     |8|/|3¦  «         |:                     |9|/|2¦  «         |;                     |9|/|2¦  «         |<                     |:|/|5¦  «         |=                     |:|/|5¦  «         |>                     |=|/|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|>                     |/¦  «        |0j        z  |>                     |/¦  «        |0j        z  |>                     |/¦  «        |0j        z  g}H|>                     |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|L                     |K|J¦  «        \  }M}Nd#}Od$}Pt(          d%z  t(          d&z  z
                       ¦   «         }Qd'}R|Rt-          |Q¦  «        z  |Pt/          |Q¦  «        z  z
                        ¦   «         }Sd(}Td#}Ud)}Vd)}Wd*}X|Tt-          |Q¦  «        z  |S|P|Oz
  t/          |Q¦  «        z  z
  z
                       ¦   «         }Y|V|Oz
  |Ut-          |Q¦  «        z  t/          |Q¦  «        z  z
  t-          |Q¦  «        z                       ¦   «         }Z|Ut-          |Q¦  «        z  |V|Oz
  |Ut-          |Q¦  «        z  t/          |Q¦  «        z  z
  t/          |Q¦  «        z  z                        ¦   «         }[|X|Pz
  }\|T|Wz
  }]t1          |\d%z  |]d%z  z   ¦  «                             ¦   «         }^|Tt/          |Q¦  «        z  |P|Oz
  t-          |Q¦  «        z  z
                       ¦   «         }_|S|^t-          t(          d%z  |Qz
  t3          |\|^z  ¦  «        z
  ¦  «        z  z                        ¦   «         }`|^t/          t(          d%z  |Qz
  t3          |\|^z  ¦  «        z
  ¦  «        z  |_z
                       ¦   «         }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|L                     d<i |                     ¦   «         d“|	                     ¦   «         d“|
                     ¦   «         d“|                     ¦   «         d“|                     ¦   «         d“|                     ¦   «         d“|d“|	d“|
|b|Oz  “|d“|d“||b|Pz  “| d“|d“|d“|d“d!¬=¦  «        \  }d}e}f|d                     |c¦  «        }g|e                     |c¦  «        }h|g                     ¦   «         }g|h                     ¦   «         }h|g                     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 ]L}j|i                      |b|j¦  «        |d                      |b|j¦  «        z
  }ktC          ˆlfdM„|kD ¦   «         ¦  «        sJ ‚ŒMd S )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     V — Œd S )N)Úabs)Ú.0ÚxÚepss     €úf/var/www/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane3.pyú	<genexpr>ztest_bicycle.<locals>.<genexpr>;  s-   øè è € Ð/Ð/ A•3�q‘6”6˜C’<Ð/Ð/Ð/Ð/Ð/Ð/ó    )"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Ô2�N€BˆˆB�Ý'¨°qÑ9Ô9Ñ€Cˆˆc�3Ý+Ð,?Ñ@Ô@Ñ€BˆˆB��B˜Ý#1Ð2EÀqÑ#IÔ#IÑ €Cˆˆc�3˜˜Sõ )0Ð0PÑ(QÔ(QÑ%€Eˆ5�'˜:Ý'.Ð/OÑ'PÔ'PÑ$€J�˜WÝ)0Ð1RÑ)SÔ)SÑ&€GˆX�x Ý$+Ð,HÑ$IÔ$IÑ!€Eˆ5�%˜Ý,3Ð4XÑ,YÔ,YÑ)€Hˆh˜ 'Ý#*Ð+FÑ#GÔ#GÑ €GˆW�g˜qÝ%Ð&<Ñ=Ô=Ñ€FˆE�3˜õ 	�sÑÔ€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ�KŠK˜ ¨"¨w©,¸¼Ð)<Ñ=Ô=€EÝ	˜Ñ	Ô	€BØ—’ ¨V°w°hÀÄÐ5HÑIÔI€IØ�?Š?˜6 6¨B°´¨=Ñ9Ô9€DØ�~Š~˜j¨&°G°8¸T¼VÐ2DÑEÔE€HÝ	˜Ñ	Ô	€Bõ �IÑÔ€GØ×Ò˜g u¨q¬s¡{Ñ3Ô3€EØ�OŠO˜G [°5´7Ñ%:Ñ;Ô;€EØ�Š˜z¨X¨:¸¼Ñ+?Ø-5¸¼Ñ-?ñ,@ñ Aô A€Hà�oŠo˜i¨7¨°T´VÑ);Ø+2°T´VÑ+;ñ*<ñ =ô =€Gà�OŠO˜G Z°$´&Ñ%8¸:ÈÌÑ;NÑ%NÑOÔO€EØ�oŠo˜i¨µ#°d´f¸a¼cÑ2BÔ2BÀTÄVÑ2KØ23´#ñ36ß7@²y±{´{ñ*Cñ Dô D€Gð ‡M‚M�!�R˜!œ#‘XÑÔÐØ‡M‚M�!�R˜!œ#‘XÑÔÐØ‡N‚N�5˜"˜uœw™,Ñ'Ô'Ð'Ø	×Ò�a˜˜eœg™Ñ&Ô&Ð&Ø×Ò�U˜B ¤™KÑ(Ô(Ð(Ø‡N‚N�4˜˜dœf™Ñ%Ô%Ð%ð
 ‡O‚O�A�qÑÔÐØ	×Ò�g˜q "Ñ%Ô%Ð%Ø	×Ò�e˜Q Ñ&Ô&Ð&Ø×Ò˜  5Ñ)Ô)Ð)Ø×Ò˜˜q $Ñ'Ô'Ð'Ø	×Ò�e˜Q Ñ%Ô%Ð%Ø×Ò˜˜q "Ñ%Ô%Ð%õ �y (¨H°hÀÀ1ÀhÑOÔOÐQYÐZ€GÝ�h ¨°'¸1¸aÀÑIÔIÈ7ÐS€FÝ�E˜5 %¨Ñ/Ô/°Ð7€DÝ�D˜% ¨Ñ.Ô.°Ð6€Dõ ˜+ x°¸ÀÑHÔH€IÝ˜ W¨d°E¸6ÑBÔB€HÝ�x ¨¨C°Ñ6Ô6€FÝ�x ¨¨C°Ñ6Ô6€Fð �‰(�C˜"‘H˜c B™h¨¨b©Ð	1€Bð —[’[ ‘^”^ a¤cÑ)¨7¯;ª;°q©>¬>¸A¼CÑ+?ÀÇÂÈQÁÄÐRSÔRUÑAUÐV€Mð ×%Ò% gÑ.Ô.°´Ñ4Ð5€Mð �f�W˜q‘[ 1¤3Ñ&Ð
'Ø	�5�&˜1‘*˜qœsÑ"Ð#Ø	���q‘˜1œ3‘ÐØ	���q‘˜1œ3‘Ðð
!€Bð �X˜v vÐ	.€Bõ 
�Q˜r 2 r˜lØ˜¸Ø�r˜2�,Ø˜R ˜¸=ØØ&ð
(ñ 
(ô 
(€Bð ×%Ò% b¨"Ñ-Ô-�L€Rˆð (+€LØ'+€MÝ(*¨Q©µ°b±Ñ(8×'?Ò'?Ñ'AÔ'A€CØ'+€JØ*4µS¸±X´XÑ*=¸}ÍSÐQTÉXÌXÑ?UÑ*VÐ(W×'^Ò'^Ñ'`Ô'`€DØ'+€GØ'*€MØ'*€MØ'*€LØ'*€LØ(/µ°C±´Ñ(8¸$ÀÈlÑ@ZÕ\_Ð`cÑ\dÔ\dÑ?dÑ:dÑ(e×'lÒ'lÑ'nÔ'n€KØ)6¸Ñ)EÀ}ÕUXÐY\ÑU]ÔU]ÑG]Õ_bÐcfÑ_gÔ_gÑFgÑ)gÕilÐmpÑiqÔiqÑ(q×'xÒ'xÑ'zÔ'z€KØ(5½¸C¹¼Ñ(@ÀMÐT`ÑD`ÐcpÕsvÐwzÑs{Ôs{Ñc{õ  Bð  CFñ  Gô  Gñ  dGñ  EGõ  KNð  ORñ  KSô  KSñ  DSñ  )S÷  (Zò  (Zñ  (\ô  (\€JØ(4°}Ñ(D€EØ(/°Ñ(<€EÝ(,¨U°A©X°e¸Q±hÑ->Ñ(?Ô(?×'FÒ'FÑ'HÔ'H€EØ(/µ°C±´Ñ(8¸-ÈÑ:TÕVYÐZ]ÑV^ÔV^Ñ9^Ñ(^×'eÒ'eÑ'gÔ'g€JØ(,¨eµc½"¸Q¹$¸s¹(Å4ÈÈeÉÑCTÔCTÑ:TÑ6UÔ6UÑ.UÑ(V×']Ò']Ñ'_Ô'_€JØ(-µµR¸±T¸#±X½tÀEÈ%ÁKÑ?PÔ?PÑ4PÑ0QÔ0QÑ(QÐR\Ñ(\×'cÒ'cÑ'eÔ'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ŒI�qð	
ð
 �GŠG‰IŒI�qð
ð �GŠG‰IŒI�qð
ð �GŠG‰IŒI�qð
ð �GŠG‰IŒI�qð
ð �GŠG‰IŒI�qð
ð �ð
ð �ð
ð ��LÑ ð
ð �ð
ð �ð
ð ��MÑ!ð
ð  �ð!
ð" �ð#
ð$ �ð%
ð& �ð'
ð* ð/ ñ ô �G€A€qˆ!ð: �*Š*�XÑ
Ô
€CØ
�*Š*�XÑ
Ô
€Cà
�)Š)‰+Œ+€CØ
�)Š)‰+Œ+€Cð 	�Š�L�L�L , , ,Ñ/Ô/€Aõ Ð}Ð}Ð}Ø}Ð}Ð}Ø#Ð%7¸¸1¹Ñ%<Ð?PÑ%PÐRdÐefÑRfÐhzÐ{|Ñh|Ð}Ø#Ð'8¸¸A¹Ñ'=Ð@PÑ'PÐTdÐefÑTfÐizÐ{|Ñi|Ð}ðñ @ô @€Cð €CÝ�1‰XŒXð 0ð 0ˆØ—’˜˜A‘” §¢¨¨1¡¤Ñ-ˆÝÐ/Ð/Ð/Ð/¨Ð/Ñ/Ô/Ñ/Ô/Ð/Ð/Ð/Ð/ð0ð 0rF   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±   © rF   rD   ú<module>rº      s  ðØ !Ð !Ð !Ð !Ð !Ð !Ø %Ð %Ð %Ð %Ð %Ð %Ø 9Ð 9Ð 9Ð 9Ð 9Ð 9Ø CÐ CÐ CÐ CÐ CÐ CÐ CÐ CÐ CÐ CØ 'Ð 'Ð 'Ð 'Ð 'Ð 'ð*ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð &Ð %Ð %Ð %Ð %Ð %ð ðn0ð n0ñ „ðn0ð n0ð n0rF   