a
    kh'                     @   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   Y/var/www/auris/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_lagrange.py<lambda>       z*test_invalid_coordinates.<locals>.<lambda>)r   r   r   set_velr   Z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s2J 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#   thetaZydZthetadr)   r*   Rr(   alphar   r-   r0   r1   Ir2   r   r   r3   r8   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sJ d S )Nzq uzq u r,   r   r   r-   r.   r   r   r   Pa)r   r   r   r5   r6   set_ang_velr   r!   	locatenewr"   v2pt_theoryr   r   r&   r	   r   r7   r8   r   )r   uZqdZudr(   r)   r*   r   r-   r   r   r>   r   lmRHSr   r   r   test_simp_penJ   s"    	
rE   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sBJ 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   ZP1pPr/   )r3   Z	forcelistframe)r   r   g3@   r   )Zsol_type)r   r   r   r   r!   r@   r"   r#   r$   dtr   r   r	   r   r7   Zlam_vecr
   eomr   Zsolve_multipliers)q1q2q1dq2dr   r)   tr*   r   ZpNr   rG   Zf_cLagZLMZlam1Zeom_solZlam_solr   r   r   test_nonminimal_pendulums   s.    

&FrR   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sJ |j||gksJ d S )Nzq1 q2r,   r/   zu1 u2r   r   r-   r.   r0   r   r   r:   r   ParPParRr   )r   r   r   r5   r6   r?   r   r@   r"   r!   rA   r   r   r&   r	   r   r7   r   r   rK   r   )rL   rM   rN   rO   Zq1ddZq2ddu1u2Zu1dZu2dr(   r)   r*   r   r-   r0   r   r   r:   rS   rT   r   rC   r   r   r   test_dub_pen   sf    	,.

*
rW   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   DmcrI   r/   BodyDrL   rM   q3rP   r4         i   
   )r   r   r   r5   r6   r"   r#   r   r!   r@   rA   r   r   r   r&   r	   r   r   r7   r8   r   Zmass_matrixr   r
   r   )rL   rM   r\   rN   rO   Zq3drr)   r*   r   rX   r   r:   rY   rZ   r<   r[   rQ   r   r(   rD   rP   r   r   r   test_rolling_disc   sR    2


.
4
0rc   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=   rE   rR   rW   rc   r   r   r   r   <module>   s   (0)1