U
    —9%e`8  ã                   @   sœ   d dl mZ 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mZ d dlmZmZmZ ed	d
„ ƒZdS )é    )Úevalf)Úpi)Úsymbols)Úsqrt)ÚacosÚsinÚcos)ÚMatrix)ÚReferenceFrameÚdynamicsymbolsÚKanesMethodÚinertiaÚmsubsÚPointÚ	RigidBodyÚdot)ÚslowÚON_CIÚskipc            m   K      s”  t rtdƒ 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|| |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 ¡}9|9 
d||2j ¡}:|9 
d| |2j ||2j  ¡};|: 
d| |5j ||5j  ¡}<|: 
d||5j ||5j  ¡}=|= 
d|t|5j|0jƒ|5j |0j  ¡  ¡}>|0 |/||0j ¡ |1 |0|	|1j ¡ |3 |2|
|2j ¡ |2 |1||2j ¡ |5 |2||5j ¡ |7 |5||5j ¡ |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|| ||	 || || g}G|> |/¡|0j@ |> |/¡|0j@ |> |/¡|0j@ g}H|> |8¡|0j@ g}I|;|+ |* |0j f|<|, |* |0j f|=|- |* |0j f|9|. |* |0j fg}J|C|D|E|Fg}Kt|/| ||g|g|I|	|
|g|||g|H|Gd"}L|L |K|J¡\}M}Nd#}Od$}Pt td% td&  ¡}Qd'}Rt |Rt|Qƒ |Pt|Qƒ   ¡}Sd(}Td#}Ud)}Vd)}Wd*}Xt |Tt|Qƒ |S|P|O t|Qƒ   ¡}Yt |V|O |Ut|Qƒ t|Qƒ  t|Qƒ ¡}Zt |Ut|Qƒ |V|O |Ut|Qƒ t|Qƒ  t|Qƒ  ¡}[t |X|P ¡}\t |T|W ¡}]t t|\d% |]d%  ƒ¡}^t |Tt|Qƒ |P|O t|Qƒ  ¡}_t |S|^ttd% |Q t|\|^ ƒ ƒ  ¡}`t |^ttd% |Q t|\|^ ƒ ƒ |_ ¡}atd+ƒ}b||P||O||Q||S||_||Y||a||`||Z||[|d,|d-| d.|!d/|&d0|'d1|(d2|)d3|"d4|#d5|$d6|%d7|,d8|+d9|-d:|.d%|*d;| d|d|d|d|d|	d|
|b|O |d|d||b|P i%}c|L ¡ d }d|Lj}et|e|cƒ}ft|d|L  ¡ |cƒ}g|f ¡ }f|g ¡ }g|f !¡ |g }h|h "dd%d8d<gdd%d:d=g¡}it#ddd>dgdddd>gd?d@|bd%  dA dB|b dC|b gdDdE|bd%  dF dG|b dH|b ggƒ}jdI‰ t$d<ƒD ]:}k|j %|b|k¡|i %|b|k¡ }lt&‡ fdJdK„|lD ƒƒsTt'‚qTd S )LNzToo slow for CI.z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ÚYZ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)Zq_indZq_dependentZconfiguration_constraintsZu_indZu_dependentZvelocity_constraintsZkd_eqsg333333Ó?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¸ž#@é   é   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)Úabs)Ú.0Úx©Zeps© úg/var/www/html/Darija-Ai-API/env/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_kane3.pyÚ	<genexpr>%  s     ztest_bicycle.<locals>.<genexpr>)(r   r   r   r   r
   Z	orientnewÚzr5   Úyr   Z	locatenewr   Ú	normalizeZset_ang_velZset_velZv2pt_theoryr   r   ZvelZpos_fromr   Zkanes_equationsr   r   r   r   r   r   r   Z	linearizeZmass_matrix_fullr   ZkindiffdictÚinvÚextractr	   ÚrangeÚsubsÚallÚAssertionError)mÚq1Úq2Zq4Zq5Zq1dZq2dZq4dZq5dÚu1Úu2Úu3Zu4Zu5Zu6Zu1dZu2dZu3dZu4dZu5dZu6dZWFradZWRradZhtangleZ
forkoffsetZ
forklengthZframelengthZforkcg1Zforkcg3Zframecg1Zframecg3ZIwr11ZIwr22ZIwf11ZIwf22ZIframe11ZIframe22ZIframe33ZIframe31ZIfork11ZIfork22ZIfork33ZIfork31ÚgZmframeZmforkZmwfZmwrr   r   r   r   r   r   r   r   r   r   r    r!   r"   r#   r$   r%   ZFrame_IZFork_IZWR_IZWF_Ir&   r'   r(   r)   ÚkdZconlist_speedZconlist_coordZFLZBLZKMÚfrZfrstarZPaperRadRearZPaperRadFrontZHTAZ
TrailPaperZrakeZPaperWbZPaperFrameCgXZPaperFrameCgZZPaperForkCgXZPaperForkCgZZFrameLengthZFrameCGNormZ
FrameCGParZtempaZtempbZtempcZ
PaperForkLZ
ForkCGNormZ	ForkCGParr,   Zval_dictZforcing_linZMM_fullZ	MM_full_sZforcing_lin_sZAmatÚAZResÚiÚerrorr7   r6   r8   Útest_bicycle   sr   ÿÿÿ.	ý  ü &*6"**                                    Ü.

ýrN   N)Zsympy.core.evalfr   Zsympy.core.numbersr   Zsympy.core.symbolr   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   r   r   Zsympy.matrices.denser	   Zsympy.physics.mechanicsr
   r   r   r   r   r   r   r   Zsympy.testing.pytestr   r   r   rN   r7   r7   r7   r8   Ú<module>   s   (