o
    GZŽhe  ã                   @   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ÚAZAxisÚBÚOr   ÚPÚQÚApÚBpzsigma, delta)Zkd_eqsé   é   )r   r   r   Z	orientnewÚzr   Zset_velZ	locatenewÚxZv2pt_theoryr
   r	   Zkanes_equationsr   r   r   Zmass_matrixÚexpandZforcing)"Úq1Úq2Zqd1Zqd2Úu1Úu2ÚlÚmr   r   r   r   r   r   r   r   ÚsigÚdeltaZTaZforcesZkde1Zkde2ZKM1Zfr1Zfstar1ZKM2Zfr2Zfstar2Zforcing_matrix_expectedZmass_matrix_expectedZfr1_expectedZfr2_expectedZfr1_cubic_expectedZfr2_cubic_expected© r"   úW/var/www/auris/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane4.pyÚtest_replace_qdots_in_force   s€   $
ÿ$
ÿþ$ÿÿÿr$   N)Zsympyr   r   r   r   Zsympy.physics.mechanicsr   r   r   r	   r
   r$   r"   r"   r"   r#   Ú<module>   s    