o
    …º¥ge  ã                   @   s@   d dl mZmZmZmZ d dlmZmZmZm	Z	m
Z
 dd„ ZdS )é    )ÚcosÚsinÚMatrixÚsymbols)ÚdynamicsymbolsÚReferenceFrameÚPointÚKanesMethodÚParticlec            "      C   s¶  t dƒ\} }t ddd\}}t dƒ\}}tdƒ\}}tdƒ}| dd| |jf¡}	|	 d	d||jf¡}
td
ƒ}| |d¡ | d||	j ¡}| 	|||	¡ | d||
j ¡}| 	|||
¡ t
d||ƒ}t
d||ƒ}tdƒ\}}|| ||  |j }|	|f|
| fg}|| || g}|| |||  g}t|| |g||g|d}| ||g|¡\}}t|| |g||g|d}| ||g|¡\}}t||d  t|ƒ |d  ||  |||   g||d  t|ƒ |d   ||  |||   ggƒ}td| |d  ||d  t|ƒ g||d  t|ƒ ||d  ggƒ}|j ¡ | ¡ ks%J ‚|j ¡ | ¡ ks1J ‚td|| ||   gƒ}| ¡ | ¡ ksIJ ‚t|| |||   | | |||   gƒ}| ¡ | ¡ kskJ ‚|| ||  |j }|	|f|
| fg}t|| |g||g|d}| ||g|¡\}}| ¡ | ¡ ks J ‚|| |||   |j }|	|f|
| fg}t|| |g||g|d}| ||g|¡\}}| ¡ | ¡ ks×J ‚|| ||d   |j }|	|f|
| fg}t|| |g||g|d}| ||g|¡\}}td|| ||d    gƒ} | ¡ |  ¡ ksJ ‚t|| |g||g|d}| ||g|¡\}}t|| ||| d   | | ||| d   gƒ}!| ¡ |! ¡ ksYJ ‚d S )Nzq1, q2é   )Úlevelzu1, u2zl, mÚNÚAÚAxisÚBÚOr   ÚPÚQÚApÚBpzsigma, delta)Úkd_eqsé   é   )r   r   r   Ú	orientnewÚzr   Úset_velÚ	locatenewÚxÚv2pt_theoryr
   r	   Úkanes_equationsr   r   r   Úmass_matrixÚexpandÚforcing)"Úq1Úq2Úqd1Úqd2Úu1Úu2ÚlÚmr   r   r   r   r   r   r   r   ÚsigÚdeltaÚTaÚforcesÚkde1Úkde2ÚKM1Úfr1Úfstar1ÚKM2Úfr2Úfstar2Úforcing_matrix_expectedÚmass_matrix_expectedÚfr1_expectedÚfr2_expectedÚfr1_cubic_expectedÚfr2_cubic_expected© r=   ú_/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane4.pyÚtest_replace_qdots_in_force   s€   $
ÿ$
ÿþ$ÿÿÿr?   N)Úsympyr   r   r   r   Úsympy.physics.mechanicsr   r   r   r	   r
   r?   r=   r=   r=   r>   Ú<module>   s    