U
    ZÊ¹j‡'  ã                   @   s¼   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 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 d dlmZ d	d
„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd„ ZdS )é    )ÚdynamicsymbolsÚReferenceFrameÚPointÚ	RigidBodyÚLagrangesMethodÚParticleÚinertiaÚ
Lagrangian)Ú
DerivativeÚFunction)Úpi)Úsymbols)ÚcosÚsinÚtan)ÚMatrix)Úsimplify)Úraisesc                     s°   t dƒ\} }}t dƒ‰tdƒtdƒ }}| |d¡ tdtdƒ|ƒ‰ˆj || tˆƒ|j t	ˆƒ|j
   ¡ || ˆj |¡ |j
¡ ˆ_t|ˆƒ‰ tt‡ ‡‡fdd„ƒ d S )	Núl m gÚqÚNÚOr   ÚPc                      s   t ˆ ˆgˆd�S )N©Úbodies)r   © ©ÚLr   r   r   új/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_lagrange.pyÚ<lambda>   ó    z*test_invalid_coordinates.<locals>.<lambda>)r   r   r   Úset_velr   ÚpointZset_posr   Úxr   ÚyÚpos_fromÚdotÚpotential_energyr	   r   Ú
ValueError)ÚlÚmÚgr   r   r   r   r   Útest_invalid_coordinates   s    *
r,   c                  C   s6  t dƒ\} }t ddƒ\}}tdƒ\}}}}}tdƒ}	|	 ddtd | |	jg¡}
|
 dd| |
jg¡}td	ƒ}| |	||
j ¡ ||d  d |j |jB }t	d
|||||fƒ}|| ||   t
|ƒ |_t|	|ƒ}| |g}| ||  g}t|||d�}| ¡  | ¡ }| ¡  |d d| t
|ƒ d k�s2t‚d S )Nzy thetaé   zm g R l alphar   ÚAÚAxisé   ÚBÚDoÚD)Ú
hol_coneqsé   )r   r   r   Ú	orientnewr   Úzr   r!   r#   r   r   r'   r	   r   Úform_lagranges_equationsÚrhsr   ÚAssertionError)r$   ÚthetaZydZthetadr*   r+   ÚRr)   Úalphar   r.   r1   r2   ÚIr3   r   r   r4   r9   r   r   r   Útest_disc_on_an_incline_plane   s&    
	r?   c                  C   sö   t dƒ\} }t ddƒ\}}tdƒ\}}}tdƒ}| dd| |jg¡}| |||j ¡ tdƒ}	|	 |d	¡ |	 d
||j	 ¡}
|
 
|	||¡ td|
|ƒ}| | | t| ƒ |_t||ƒ}t|| gƒ}| ¡  | ¡ }|d | t| ƒ | ksòt‚d S )Nzq uzq u r-   r   r   r.   r/   r   r   r   ÚPa)r   r   r   r6   r7   Úset_ang_velr   r!   Ú	locatenewr#   Úv2pt_theoryr   r   r'   r	   r   r8   r9   r   r:   )r   ÚuZqdZudr)   r*   r+   r   r.   r   r   r@   r   ÚlmÚRHSr   r   r   Útest_simp_penJ   s"    	
rG   c                  C   sª  t dƒ\} }t ddd�\}}tdƒ\}}}d}tdƒ}tdƒ}	|	 |d¡ |	 d	| |j ||j  ¡}
|
 ||
 |	¡ 	|¡¡ t
d
|
|ƒ}t| d |d  |d  gƒ}t||ƒ}t|| |g||
|| |j fg|d�}| ¡  |jd }t|t| ||ƒ d|  d| |   g|t|||ƒ d| |  ggƒ}|j|k�sBt‚td|  d|d   d|d   d| d  | d|d  |   gƒ}t|jdd�ƒt|ƒk�s¦t‚d S )Nzq1:3r-   )ÚlevelzL, m, tgš™™™™™#@r   zN*r   ZP1ÚpPr0   )r4   Z	forcelistÚframe)r   r   gš™™™™™3@é   r   )Zsol_type)r   r   r   r   r!   rB   r#   r$   r%   Údtr   r   r	   r   r8   Zlam_vecr
   Úeomr:   r   Zsolve_multipliers)Úq1Úq2Úq1dÚq2dr   r*   Útr+   r   ZpNr   rI   Zf_cÚLagZLMZlam1Zeom_solZlam_solr   r   r   Útest_nonminimal_pendulums   s0    
 ÿ
&ÿFrT   c                  C   sÖ  t dƒ\} }t ddƒ\}}t ddƒ\}}t dƒ\}}t ddƒ\}}	tdƒ\}
}}tdƒ}| dd| |jg¡}| d	d||jg¡}| |||j ¡ | |||j ¡ td
ƒ}| d|
|j ¡}| d|
|j ¡}| 	|d¡ | 
|||¡ | 
|||¡ td||ƒ}td||ƒ}| | |
 t| ƒ |_| | |
 t| ƒ || |
 t|ƒ  |_t|||ƒ}t|| |g||gd�}| ¡  t|
| d| t| ƒ |
t| ƒ t|ƒ |  |
t| ƒ t|ƒ |d   |
t|ƒ t| ƒ |d   |
t| ƒ t|ƒ |  d|
 |   |jd  ƒdk�s"t‚t|
| |t|ƒ |
t| ƒ t|ƒ |  |
t| ƒ t|ƒ |d   |
t|ƒ t| ƒ |d   |
t| ƒ t|ƒ |  |
|   |jd  ƒdk�s¾t‚|j||gk�sÒt‚d S )Nzq1 q2r-   r0   zu1 u2r   r   r.   r/   r1   r   r   r<   r   ÚParPÚParRr   )r   r   r   r6   r7   rA   r   rB   r#   r!   rC   r   r   r'   r	   r   r8   r   r   rM   r:   r   )rN   rO   rP   rQ   Zq1ddZq2ddÚu1Úu2Zu1dZu2dr)   r*   r+   r   r.   r1   r   r   r<   rU   rV   r   rE   r   r   r   Útest_dub_pen‘   sf    	,.ÿÿþ
þþþ
*ÿÿþþþþ
rY   c                  C   s€  t dƒ\} }}t ddƒ\}}}tdƒ\}}}tdƒ}	|	 dd| |	jg¡}
|
 dd||
jg¡}| dd||jg¡}td	ƒ}| |	d
¡ | 	d||j ¡}| 
||	|¡ t||d |d  |d |d  |d |d  ƒ}td|||||fƒ}| | | t|ƒ |_t|	|ƒ}| ||g}tdƒ} tdƒ}tdƒ}t||ƒ}| ¡  | ¡ }| ¡  tdƒ}|jdd… d
d| |d  d d
gk�s†t‚|d  ¡ d| t||ƒƒ |dtd||ƒ ƒ t| |ƒ|ƒ dt||ƒƒ t||ƒ|ƒ   t| |ƒ|ƒ  d|  k�st‚|d dt||ƒƒ t| |ƒ|ƒ dt||ƒƒ t||ƒ|ƒ  dt| |ƒ|ƒ t||ƒƒ  t||ƒ|ƒ k�s|t‚d S )Nzq1 q2 q3r-   zr m gr   ÚYr/   r   r<   ÚCr   ÚDmcrK   r0   ÚBodyDrN   rO   Úq3rR   r5   é   é   iøÿÿÿé   é
   éûÿÿÿ)r   r   r   r6   r7   r#   r$   r   r!   rB   rC   r   r   r   r'   r	   r   r   r8   r9   r   Zmass_matrixr:   r   r
   r   )rN   rO   r^   rP   rQ   Zq3dÚrr*   r+   r   rZ   r   r<   r[   r\   r>   r]   rS   r   r)   rF   rR   r   r   r   Útest_rolling_discÂ   sR    2


.
4ÿÿÿÿ
0ÿÿþre   N) Zsympy.physics.mechanicsr   r   r   r   r   r   r   r	   Zsympy.core.functionr
   r   Zsympy.core.numbersr   Zsympy.core.symbolr   Z(sympy.functions.elementary.trigonometricr   r   r   Zsympy.matrices.denser   Zsympy.simplify.simplifyr   Zsympy.testing.pytestr   r,   r?   rG   rT   rY   re   r   r   r   r   Ú<module>   s   (0)1