U
    9%e'                     @   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/Darija-Ai-API/env/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st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std S )Nzq1:3r-   )levelzL, m, tg#@r   zN*r   ZP1pPr0   )r4   Z	forcelistframe)r   r   g3@   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st|j||gkst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st|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