o
    GZŽ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         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+ƒ}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|O “¥|d|d||b|P i¥}c|Ljd<i | ¡ d“|	 ¡ d“|
 ¡ d“| ¡ d“| ¡ d“| ¡ d“|d“|	d“|
|b|O “|d“|d“||b|P “| d“|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%  dC dD|b dE|b gdFdG|bd%  dH dI|b dJ|b ggƒ}idK‰ tdLƒD ]}j|i  |b|j¡|d  |b|j¡ }kt!‡ fdMdN„|kD ƒƒs	J ‚qíd S )ONz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)r   r&   r,   é   )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   s    | ]	}t |ƒˆ k V  qd S )N)Úabs)Ú.0Úx©Zeps© úW/var/www/auris/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane3.pyÚ	<genexpr>;  s   € ztest_bicycle.<locals>.<genexpr>)"r
   r   r	   Z	orientnewÚzr2   Ú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)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   r3   r5   Útest_bicycle   s°  ÿÿÿÿ.	ý
û$(4 ((ÿþýüûúùø	÷
öõôóòñðïîíìëêéèçæåäãâá à!ß"Ü.üûúùø	÷
öõôóòñðïîíé

ý þrL   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   rL   r4   r4   r4   r5   Ú<module>   s    $