U
    L?ho:  ã                   @   s„   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d	„ ƒZd
S )é    )Úpi)Úsymbols)Úsqrt)ÚacosÚsinÚcos)ÚMatrix)ÚReferenceFrameÚdynamicsymbolsÚKanesMethodÚinertiaÚPointÚ	RigidBodyÚdot)Úslowc            l   K      s¾  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!d"	}L|L |K|J¡\}M}Nd#}Od$}Ptd% td&   ¡ }Qd'}R|Rt|Qƒ |Pt|Qƒ    ¡ }Sd(}Td#}Ud)}Vd)}Wd*}X|Tt|Qƒ |S|P|O t|Qƒ    ¡ }Y|V|O |Ut|Qƒ t|Qƒ  t|Qƒ  ¡ }Z|Ut|Qƒ |V|O |Ut|Qƒ t|Qƒ  t|Qƒ   ¡ }[|X|P }\|T|W }]t|\d% |]d%  ƒ ¡ }^|Tt|Qƒ |P|O t|Qƒ   ¡ }_|S|^ttd% |Q 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|Ljd<| ¡ d|	 ¡ d|
 ¡ d| ¡ d| ¡ d| ¡ d|d|	d|
|b|O |d|d||b|P | d|d|d|did!d=\}d}e}f|d |c¡}g|e |c¡}h|g ¡ }g|h ¡ }h|g dd%d:d>gdd%d:d>g¡}dtddd?dgdddd?gd@dA|bd%  dB dC|b dD|b gdEdF|bd%  dG dH|b dI|b ggƒ}idJ‰ tdKƒD ]:}j|i  |b|j¡|d  |b|j¡ }kt!‡ fdLdM„|kD ƒƒs~t"‚q~d 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Ú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ÚBodyWFZCRAMER)Zq_indZq_dependentZconfiguration_constraintsZu_indZu_dependentZvelocity_constraintsZkd_eqsZ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)ZA_and_BZop_pointZlinear_solveré   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©Úeps© úZ/var/www/html/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_kane3.pyÚ	<genexpr>;  s     ztest_bicycle.<locals>.<genexpr>)#r
   r   r	   Z	orientnewÚzr1   Úyr   Z	locatenewr   Ú	normalizeZset_ang_velZset_velZv2pt_theoryr   r   ZvelZpos_fromr   Zkanes_equationsr   Zevalfr   r   r   r   Z	linearizeÚdiffZxreplaceÚextractr   ÚrangeÚsubsÚallÚAssertionError)lÚ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_dictÚAÚBÚ_ZA_sZB_sZResÚiÚerrorr4   r2   r5   Útest_bicycle   s´   ÿÿÿ.	ý  û$(4 ((                                    Ü.                íé


ýrM   N)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   Zsympy.testing.pytestr   rM   r4   r4   r4   r5   Ú<module>   s   $