o
    *ήcy%                     @   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	 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)simplifyc                  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 alphaNAAxis   BDoD)
hol_coneqs   )r   r   r   	orientnewr   zr   set_velxr   r   potential_energyr	   r   form_lagranges_equationsrhsr   )ythetaydthetadmgRlalphar   r   r   r   Ir   Lqr   r#    r0   R/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/mechanics/tests/test_lagrange.pytest_disc_on_an_incline_plane   s&   
	$r2   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   l m gr   r   r   Or   PPa)r   r   r   r   r   set_ang_velr   r   	locatenewr    v2pt_theoryr   r   r!   r	   r   r"   r#   r   )r/   uqdudr+   r(   r)   r   r   r4   r5   r6   r.   lmRHSr0   r0   r1   test_simp_pen<   s"   	
"r?   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pPr   )r   	forcelistframe)r   r   g3@   r   )sol_type)r   r   r   r   r   r8   r    r$   pos_fromdtr   r   r	   r   r"   lam_vecr
   eomr   solve_multipliers)q1q2q1dq2dr.   r(   tr)   r   pNr5   rB   f_cLagLMlam1eom_sollam_solr0   r0   r1   test_nonminimal_pendulume   s.   

&F rX   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   r   zu1 u2r3   r   r   r   r   r4   r5   r*   r   ParPParR)bodies)r   r   r   r   r   r7   r   r8   r    r   r9   r   r   r!   r	   r   r"   r   r   rJ   r[   )rL   rM   rN   rO   q1ddq2ddu1u2u1du2dr+   r(   r)   r   r   r   r4   r5   r*   rY   rZ   r.   r=   r0   r0   r1   test_dub_pen   sf   	,.

*
rb   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   DmcrE   r   BodyDrL   rM   q3rP   r         i   
   )r   r   r   r   r   r    r$   r   r   r8   r9   r   r   r   r!   r	   r   r   r"   r#   r   mass_matrixr   r
   r   )rL   rM   rg   rN   rO   q3drr(   r)   r   rc   r.   r*   rd   re   r-   rf   rS   r/   r+   r>   rP   r0   r0   r1   test_rolling_disc   sR   2


,
4
0rp   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   r2   r?   rX   rb   rp   r0   r0   r0   r1   <module>   s   ( 0)1