U
    0e'                     @   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   O/tmp/pip-unpacked-wheel-_6tpq7m6/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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-   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st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   r9   )r   uZqdZudr(   r)   r*   r   r-   r   r   r?   r   lmRHSr   r   r   test_simp_penJ   s"    	
rF   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pPr/   )r3   Z	forcelistframe)r   r   g3@   r   )Zsol_type)r   r   r   r   r!   rA   r"   r#   r$   dtr   r   r	   r   r7   Zlam_vecr
   eomr9   r   Zsolve_multipliers)q1q2q1dq2dr   r)   tr*   r   ZpNr   rH   Zf_cLagZLMZlam1Zeom_solZlam_solr   r   r   test_nonminimal_pendulums   s0    
 
&FrS   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,   r/   zu1 u2r   r   r-   r.   r0   r   r   r;   r   ParPParRr   )r   r   r   r5   r6   r@   r   rA   r"   r!   rB   r   r   r&   r	   r   r7   r   r   rL   r9   r   )rM   rN   rO   rP   Zq1ddZq2ddu1u2Zu1dZu2dr(   r)   r*   r   r-   r0   r   r   r;   rT   rU   r   rD   r   r   r   test_dub_pen   sf    	,.

*
rX   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   DmcrJ   r/   BodyDrM   rN   q3rQ   r4         i   
   )r   r   r   r5   r6   r"   r#   r   r!   rA   rB   r   r   r   r&   r	   r   r   r7   r8   r   Zmass_matrixr9   r   r
   r   )rM   rN   r]   rO   rP   Zq3drr)   r*   r   rY   r   r;   rZ   r[   r=   r\   rR   r   r(   rE   rQ   r   r   r   test_rolling_disc   sR    2


.
4
0rd   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>   rF   rS   rX   rd   r   r   r   r   <module>   s   (0)1