a
    kº”hÏ(  ã                   @   sB  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mZ d dl	mZ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 d dlmZmZ edƒ\ZZZ Z!Z"edƒZ#e# $dd	ee#j%g¡Z&e& $d
d	ee&j'g¡Z(e( $dd	e e(j)g¡Z*dd„ Z+dd„ Z,dd„ Z-dd„ Z.dd„ Z/dd„ Z0dd„ Z1dd„ Z2dd„ Z3dd„ Z4d d!„ Z5d"S )#é    )ÚsinÚcosÚtanÚpiÚsymbolsÚMatrixÚSÚFunction)ÚParticleÚPointÚReferenceFrameÚ	RigidBody)	Úangular_momentumÚdynamicsymbolsÚkinetic_energyÚlinear_momentumÚouterÚpotential_energyÚmsubsÚfind_dynamicsymbolsÚ
Lagrangian)Úcenter_of_massÚ_validate_coordinatesÚ_parse_linear_solver)ÚraisesÚwarns_deprecated_sympyzq1 q2 q3 q4 q5ÚNÚAZAxisÚBÚCc                     s¼   t dƒ‰tdƒ} |  ˆdˆj ¡ tˆjˆjƒ}td| ˆd|| fƒ‰ tdƒ}td|dƒ‰ˆj ˆd	ˆj ¡ t	t
‡ ‡fd
d„ƒ t	t
‡‡fdd„ƒ tˆˆ ˆƒd	ˆj dˆj  ks¸J ‚d S )Nr   ÚAcé   r   é   ÚPÚPaé   é
   c                      s   t ˆ ˆ ˆƒS ©N©r   © ©r   r$   r)   úZ/var/www/auris/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_functions.pyÚ<lambda>   ó    z&test_linear_momentum.<locals>.<lambda>c                      s   t ˆ ˆ ˆƒS r'   r(   r)   ©r   r$   r)   r+   r,      r-   iô  )r   r   Úset_velÚyr   Úxr   r
   Úpointr   Ú	TypeErrorr   )r    ÚIr#   r)   ©r   r   r$   r+   Útest_linear_momentum   s    r6   c            	   	      s†  t dƒ\} }}}tdƒ}tdƒ‰tdƒ}tdƒ‰ˆ d|ˆj ¡}| d|ˆj ¡}ˆ ˆdˆj ¡ | ˆ|ˆj ¡ | 	ˆˆ|¡ | 	ˆˆ|¡ t
d	|| ƒ‰td
||||tˆjˆjƒ |fƒ‰ d|  | | ˆj || | ˆj  }tˆˆ ˆƒ|ksþJ ‚tt‡ ‡‡fdd„ƒ tt‡ ‡‡fdd„ƒ tt‡‡‡fdd„ƒ |||d   d|  |d   | ˆj }tˆˆˆ ˆƒ|ks‚J ‚dS )a   A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N.z
m, M, l, IÚomegar   ÚaÚOr    r#   r   r$   r   é   c                      s   t ˆˆˆ ˆƒS r'   ©r   r)   r5   r)   r+   r,   7   r-   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>c                      s   t ˆˆˆ ˆƒS r'   r;   r)   )r   r9   r$   r)   r+   r,   8   r-   c                      s   t ˆˆ ˆˆƒS r'   r;   r)   )r   r9   r$   r)   r+   r,   9   r-   é   N)r   r   r   r   Ú	locatenewr1   r/   Úset_ang_velÚzÚv2pt_theoryr
   r   r   r0   r   r   r3   r   )	ÚmÚMÚlr4   r7   r8   r    r#   Úexpectedr)   )r   r   r9   r$   r+   Ú)test_angular_momentum_and_linear_momentum#   s(    "(*rE   c            	         sB  t dƒ\} }}tdƒ}tdƒ‰tdƒ}| ˆdˆj ¡ | d|ˆj ¡}| d|ˆj ¡}tdƒ}| ˆ|ˆj ¡ | 	|ˆ|¡ | 	|ˆ|¡ t
d	|| ƒ‰tˆjˆjƒ}td
|||||fƒ‰ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ dtˆˆˆ ƒ||d  |d  d d|d  |  |d   |d d    ¡ ks>J ‚d S )Nzm M l1r7   r   r9   r   r    r#   r8   r$   r   c                      s   t ˆˆˆ ƒS r'   ©r   r)   r*   r)   r+   r,   M   r-   z%test_kinetic_energy.<locals>.<lambda>c                      s   t ˆˆˆ ƒS r'   rF   r)   )r   r   r)   r+   r,   N   r-   r:   )r   r   r   r   r/   r1   r=   r>   r?   r@   r
   r   r   r   r3   r   Úexpand)	rA   rB   Úl1r7   r9   r    r#   r8   r4   r)   r5   r+   Útest_kinetic_energy>   s*    "ÿ
ÿrI   c                  C   s  t dƒ\} }}}}}tdƒ}tdƒ}tdƒ}| |d|j ¡ | d||j ¡}	|	 d||j ¡}
tdƒ}| |||j ¡ |	 	|||¡ |
 	|||¡ t
d	|
| ƒ}t|j|jƒ}td
|	||||	fƒ}| | | |_|| | |_t||ƒ| | | || |  ks
J ‚d S )Nzm M l1 g h Hr7   r   r9   r   r    r#   r8   r$   r   )r   r   r   r   r/   r1   r=   r>   r?   r@   r
   r   r   r   )rA   rB   rH   ÚgÚhÚHr7   r   r9   r    r#   r8   r$   r4   r   r)   r)   r+   Útest_potential_energyS   s"    rM   c            	         s
  t dƒ\} }}}tdƒ‰tdƒ}| ˆdˆj ¡ | ddˆj ¡}| ˆdˆj ¡ td|dƒ‰| d	d
ˆj ¡}| ˆdˆj ¡ tdƒ}| ˆdˆj	 ¡ t
ˆj	ˆj	ƒ}td||d||fƒ‰ || | ˆ_| | | ˆ _tt‡ ‡fdd„ƒ tt‡‡fdd„ƒ d S )NzM m g hr   r9   r   r#   r%   r&   r$   r    r:   é   r8   r   r"   c                      s   t ˆ ˆ ˆƒS r'   ©r   r)   r*   r)   r+   r,   w   r-   z!test_Lagrangian.<locals>.<lambda>c                      s   t ˆ ˆ ˆƒS r'   rO   r)   r.   r)   r+   r,   x   r-   )r   r   r   r/   r1   r=   r
   r0   r>   r?   r   r   r   r   r3   )	rB   rA   rJ   rK   r9   r#   r    r8   r4   r)   r5   r+   Útest_Lagrangiang   s"    rP   c                  C   sˆ  t dƒ\} }tdƒ\}}}t| | | || ¡  | g| ¡  ¡ |t| ¡ ƒ ggƒ}t| | |g| ¡  ¡ dggƒ}|d|d| ¡ d| ¡ di}t||ƒ|ks¤J ‚t|| ƒt|| ƒ || ¡   }|d|td | ¡ di}t||dd|d ksúJ ‚t	dƒ}||j
 ||j  }	||j
|j
B  ||j|jB   }
d|j }d|j|jB  }|d|di}t|	|ƒ|kspJ ‚t|
|ƒ|ks„J ‚d S )	Núa, búx, y, zr%   r   r:   T)Zsmartr   )r   r   r   Údiffr   r   r   r   r   r   r1   r0   )r8   Úbr1   r0   r?   ÚexprÚsolÚsdr   ÚvÚdZv_solZd_solr)   r)   r+   Ú
test_msubs{   s*    ÿÿ$ 
rZ   c                     s(  t dƒ\} }tdƒ\}}}t| | | || ¡  | g| ¡  ¡ |t| ¡ ƒ ggƒ}|| ¡ || ¡  ¡ || ¡ h}t|ƒ|ks†J ‚|||g}| ¡ | ¡  ¡ | ¡ h}t||d|ks¾J ‚tdƒ\}}	}
tdƒ}||j |	|j  |
|j	  ‰ ||	|
h}tˆ |d|ksJ ‚t
t‡ fdd„ƒ d S )	NrQ   rR   )Úexcludezd, e, fr   )Zreference_framec                      s   t ˆ ƒS r'   )r   r)   ©rX   r)   r+   r,   ¦   r-   z*test_find_dynamicsymbols.<locals>.<lambda>)r   r   r   rS   r   r   r   r1   r0   r?   r   Ú
ValueError)r8   rT   r1   r0   r?   rU   rV   Zexclude_listrY   ÚeÚfr   r)   r\   r+   Útest_find_dynamicsymbols“   s     ÿ 

r`   c               
   C   sv  t dƒ} tddd}tdtdƒtjƒ}tdtdƒtd	ƒƒ}td
tdƒtdƒƒ}tdtdƒ|ƒ}t dƒ}tdƒ}tdƒ}td|||t|j|jƒ|fƒ}	|j	 
|j	| j¡ |j	 
|j	| j| j ¡ |j	 
|j	| j¡ |	j 
|j	| j| j ¡ tdƒ}
|
 
|j	t|j	|||||	ƒ¡ d|| d  | j || d || d  | j  ||| d  | j  }|
 |j	¡| dksrJ ‚d S )Nr8   rA   T)ÚrealÚp1Zp1_ptÚp2Zp2_ptr:   Úp3Zp3_pté   Úp4Zp4_ptÚb_fÚb_cmÚmbrT   ÚorN   é   r   )r   r   r
   r   r   ZOner   r   r1   r2   Zset_posr0   Ú
masscenterr?   r   Zpos_from)r8   rA   rb   rc   rd   rf   rg   rh   ri   rT   Zpoint_orU   r)   r)   r+   Útest_center_of_mass¬   s$    Jrm   c                
      sÌ  t dƒ\	‰‰‰‰
‰‰‰‰‰tdƒ\‰‰‰tˆˆˆgˆ
ˆˆgˆˆˆgd tˆˆgƒ tˆˆgˆ
gƒ tˆ
ˆgd tˆˆˆgˆ
ˆˆgdd tt‡‡‡
‡‡fdd„ƒ tˆˆˆgˆ
ˆˆgdd tt‡‡‡‡
‡fd	d„ƒ tt‡‡‡‡‡fd
d„ƒ tˆˆˆgˆ
ˆˆgdˆ
ˆˆgd tt‡‡‡‡
‡‡‡‡fdd„ƒ tt‡‡‡‡
‡‡‡‡fdd„ƒ tt‡‡‡‡
‡‡‡‡fdd„ƒ tˆˆ ˆgdd tt‡‡‡fdd„ƒ tˆˆˆgdˆ
ˆgdd tt‡‡‡‡
‡fdd„ƒ tˆˆ ˆ ˆgdˆ
gdd tt‡‡‡‡‡
fdd„ƒ tˆˆgdd tt‡‡fdd„ƒ t j‰	tdƒ‰ tdtd\‰‰tˆˆ ƒˆˆ ƒgdd tt‡ ‡‡fdd„ƒ tt‡ ‡‡fdd„ƒ ˆ t _tˆˆ ƒˆˆ ƒgƒ tt‡‡‡	fdd„ƒ ˆ	t _d S )Nzq1:4 u1:4 ua1:4zs1:4©Úu_auxiliary©ZspeedsF©Úcheck_duplicatesc                      s   t ˆ ˆˆgˆˆˆgƒS r'   ©r   r)   )Úq1Úq2Úu1Úu2Úu3r)   r+   r,   Í   s   ÿz+test_validate_coordinates.<locals>.<lambda>c                      s   t ˆ ˆˆgˆˆˆgddS ©NTrq   rs   r)   )rt   ru   Úq3rv   rw   r)   r+   r,   Ð   s   ÿc                      s   t ˆ ˆˆgˆ ˆˆgddS ry   rs   r)   )rt   ru   rz   rw   rx   r)   r+   r,   Ò   s   ÿ)rr   ro   c                      s    t ˆ ˆˆgˆˆˆgˆˆˆgdS ©Nrn   rs   r)   ©rt   ru   rz   rv   rw   rx   Úua2Úua3r)   r+   r,   Ö   s   ÿc                      s    t ˆ ˆˆgˆˆˆgˆ ˆˆgdS r{   rs   r)   r|   r)   r+   r,   Ø   s   ÿc                      s    t ˆ ˆˆgˆˆˆgˆˆˆgdS r{   rs   r)   )rt   ru   rz   rv   rw   rx   Úua1r}   r)   r+   r,   Ú   s   ÿ©Úis_dynamicsymbolsc                      s   t ˆ ˆ ˆgƒS r'   rs   r)   )rt   ru   rz   r)   r+   r,   Þ   r-   r   c                      s   t ˆˆ ˆgdˆˆgddS ©Nr   Tr€   rs   r)   )rt   ru   Ús1rv   rw   r)   r+   r,   à   s   ÿc                      s   t ˆˆ ˆ ˆ gdˆgddS r‚   rs   r)   )rt   rƒ   Ús2Ús3rv   r)   r+   r,   ã   s   ÿ)ro   r   c                      s   t ˆ ˆgdS r{   rs   r)   )rƒ   r   r)   r+   r,   æ   r-   r8   zf1:3)Úclsc                      s   t ˆˆ ƒˆˆ ƒgƒS r'   rs   r)   ©r8   Úf1Úf2r)   r+   r,   ì   r-   c                      s   t ˆˆ ƒˆˆ ƒgdS )Nrp   rs   r)   r‡   r)   r+   r,   í   r-   c                      s   t ˆ ˆƒˆˆƒgƒS r'   rs   r)   )rˆ   r‰   Útr)   r+   r,   ð   r-   )r   r   r   r   r]   Z_tr	   r)   r)   )r8   rˆ   r‰   rt   ru   rz   rƒ   r„   r…   rŠ   rv   rw   rx   r   r}   r~   r+   Útest_validate_coordinatesÁ   sJ    ÿÿ   r‹   c                  C   sX   t ddtdƒƒt ddtdƒƒ } }tt jƒt jks6J ‚tdƒ| |ƒt  | |¡ksTJ ‚d S )Nre   za:9r:   zb:6ZLU)r   r   r   ZLUsolve)r   rT   r)   r)   r+   Útest_parse_linear_solverô   s    "rŒ   c                  C   sZ  ddl m} m}m} tdƒ}tƒ H | |ddddƒ|j|jB |j|jB  |j|jB  ks\J ‚W d   ƒ n1 sp0    Y  tƒ h |d|j|j |ƒ|j|jB |j|jB  d|j|jB   |j|jB  |j|jB  ksÚJ ‚W d   ƒ n1 sî0    Y  t	dƒ}tƒ > |d|j |ƒ|j
d|j |j fgks6J ‚W d   ƒ n1 sL0    Y  d S )Nr   )ÚinertiaÚinertia_of_point_massÚgravityr   r%   r:   r#   éþÿÿÿ)Ú!sympy.physics.mechanics.functionsr   rŽ   r   r   r   r1   r0   r?   r
   rl   Zmass)r   rŽ   r   r   Úpr)   r)   r+   Útest_deprecated_moved_functionsú   s    V&
ÿ
ÿÿ&r“   N)6Zsympyr   r   r   r   r   r   r   r	   Zsympy.physics.mechanicsr
   r   r   r   r   r   r   r   r   r   r   r   r   r‘   r   r   r   Zsympy.testing.pytestr   r   rt   ru   rz   Zq4Zq5r   Z	orientnewr?   r   r1   r   r0   r   r6   rE   rI   rM   rP   rZ   r`   rm   r‹   rŒ   r“   r)   r)   r)   r+   Ú<module>   s(   (,3