o
    g'                     @   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   b/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_lagrange.py<lambda>   s    z*test_invalid_coordinates.<locals>.<lambda>)r   r   r   set_velr   point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   s4  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sJ 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   )r$   thetayd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syJ 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   uqdudr)   r*   r+   r   r.   r   r   rA   r   lmRHSr   r   r   test_simp_penJ   s"   	
"rJ   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sJ td|  d|d   d|d   d| d  | d|d  |   g}t|jddt|ksJ d S )Nzq1:3r-   )levelzL, m, tg#@r   zN*r   P1pPr0   )r4   	forcelistframe)r   r   g3@   r   )sol_type)r   r   r   r   r    rC   r#   r$   r%   dtr   r   r	   r   r8   lam_vecr
   eomr   solve_multipliers)q1q2q1dq2dr   r*   tr+   r   pNr   rM   f_cLagLMlam1eom_sollam_solr   r   r   test_nonminimal_pendulums   s.   

&F rb   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J t|
| |t| |
t|  t| |  |
t|  t| |d   |
t| t|  |d   |
t|  t| |  |
|   |jd  dks_J |j||gksiJ d S )Nzq1 q2r-   r0   zu1 u2r   r   r.   r/   r1   r   r   r=   r   ParPParRr   )r   r   r   r6   r7   rB   r   rC   r#   r    rD   r   r   r'   r	   r   r8   r   r   rT   r   )rV   rW   rX   rY   q1ddq2ddu1u2u1du2dr)   r*   r+   r   r.   r1   r   r   r=   rc   rd   r   rH   r   r   r   test_dub_pen   sf   	,.

*
rk   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sJ |d  d| t|| |dtd||  t| || dt|| t|||   t| ||  d|  ksJ |d dt|| t| || dt|| t|||  dt| || t||  t||| ks=J d S )Nzq1 q2 q3r-   zr m gr   Yr/   r   r=   Cr   DmcrP   r0   BodyDrV   rW   q3rZ   r5         i   
   )r   r   r   r6   r7   r#   r$   r   r    rC   rD   r   r   r   r'   r	   r   r   r8   r9   r   mass_matrixr   r
   r   )rV   rW   rp   rX   rY   q3drr*   r+   r   rl   r   r=   rm   rn   r?   ro   r]   r   r)   rI   rZ   r   r   r   test_rolling_disc   sR   2


,
4
0ry   N) sympy.physics.mechanicsr   r   r   r   r   r   r   r	   sympy.core.functionr
   r   sympy.core.numbersr   sympy.core.symbolr   (sympy.functions.elementary.trigonometricr   r   r   sympy.matrices.denser   sympy.simplify.simplifyr   sympy.testing.pytestr   r,   r@   rJ   rb   rk   ry   r   r   r   r   <module>   s   ( 0)1