
    sg'                         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	 Zd
 Zd Zd Zd Zd Zy)    )dynamicsymbolsReferenceFramePoint	RigidBodyLagrangesMethodParticleinertia
Lagrangian)
DerivativeFunction)pi)symbols)cossintan)Matrix)simplify)raisesc                     t        d      \  } }}t        d      t        d      t        d      }}|j                  |d       t	        dt        d      |      j
                  j                  || t              |j                  z  t              |j                  z  z
  z         ||z  j
                  j                  |      j                  |j                        z  _        t        |      t        t         fd       y )Nl m gqNOr   Pc                  "    t         g      S )Nbodies)r   )Lr   r   s   ^/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_lagrange.py<lambda>z*test_invalid_coordinates.<locals>.<lambda>   s    q1#a@     )r   r   r   set_velr   pointset_posr   xr   ypos_fromdotpotential_energyr
   r   
ValueError)lmgr   r   r   r   r   s        @@@r   test_invalid_coordinatesr.      s    gGAq!A#c
qAIIaOeCj!$AGGOOAqCFQSSL3q6ACC<789Q!1!1!!4!8!8!==A1aA
:@Ar!   c                     t        d      \  } }t        dd      \  }}t        d      \  }}}}}t        d      }	|	j                  ddt        dz  |z
  |	j
                  g      }
|
j                  dd| |
j
                  g      }t        d	      }|j                  |	||
j                  z         ||dz  z  dz  |j
                  z  |j
                  z  }t        d
|||||f      }||z  || z
  z  t        |      z  |_        t        |	|      }| |g}| ||z  z
  g}t        |||      }|j                          |j                         }|j!                          |d   d|z  t        |      z  dz  k(  sJ y )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   r1   r4   r5   Ir6   r   r   r7   r<   s                      r   test_disc_on_an_incline_planerC      sr    i(HAu	1-JB0Aq!Q 	sA	C"Q$,!45A	C5&!##/A
 
tBJJq"qss(	AqD
QSS133A#r1a!R)A
 Q!a%3u:5A1aA 
E
Aa%i-J14A 
%%'CLLNq6QqSU^A%%%%r!   c                     t        d      \  } }t        dd      \  }}t        d      \  }}}t        d      }|j                  dd| |j                  g      }|j                  |||j                  z         t        d      }	|	j                  |d	       |	j                  d
||j                  z        }
|
j                  |	||       t        d|
|      }| |z  |z  t        |       z  |_        t        ||      }t        || g      }|j!                          |j#                         }|d   | t%        |       z  |z  k(  sJ y )Nzq uzq u r0   r   r   r1   r2   r   r   r   Pa)r   r   r   r9   r:   set_ang_velr   r"   	locatenewr%   v2pt_theoryr   r   r)   r
   r   r;   r<   r   )r   uqdudr+   r,   r-   r   r1   r   r   rE   r   lmRHSs                  r   test_simp_penrN   J   s,    % DAqFA&FBgGAq!
 	sA	C!QSS*AMM!R!##X
 	c
AIIaO	CQSS!AMM!Q 
$1	B#'A+A.B1bA 
QC	 B!
&&(Cq6aRAYq[   r!   c            	         t        d      \  } }t        dd      \  }}t        d      \  }}}d}t        d      }t        d      }	|	j	                  |d       |	j                  d	| |j                  z  ||j                  z  z         }
|
j	                  ||
j                  |	      j                  |             t        d
|
|      }t        | dz  |dz  z   |dz  z
  g      }t        ||      }t        || |g||
||z  |j                  z  fg|      }|j                          |j                  d   }t        |t!        | ||      z  d|z  z
  d|z  | z  z   g|t!        |||      z  d|z  |z  z   gg      }|j"                  |k(  sJ t        d| z  d|dz  z  z   d|dz  z  z   d| dz  z  |z  d|dz  z  |z  z   z  g      }t%        |j'                  d            t%        |      k(  sJ y )Nzq1:3r0   )levelzL, m, tg#@r   zN*r   P1pPr3   )r7   	forcelistframe)r   r   g3@   r   )sol_type)r   r   r   r   r"   rG   r%   r&   r'   dtr   r   r
   r   r;   lam_vecr   eomr   solve_multipliers)q1q2q1dq2dr   r,   tr-   r   pNr   rR   f_cLagLMlam1eom_sollam_sols                     r   test_nonminimal_pendulumrg   s   s   F#FBfA.HCi GAq!AsA	tBJJq!
T2acc6BqssF?+AIIaB""1%&	$1	B
"a%"a%-!Q$&'
(C
Q
C	r2h31Q3qss7|nA
/B!::dDqB1--A5$r	ABB1--$r	9:< =G66WtBw36)Ac1fH4qQwqy1RU7197LMNOGB((((;<@QQQQr!   c                     t        d      \  } }t        dd      \  }}t        dd      \  }}t        d      \  }}t        dd      \  }}	t        d      \  }
}}t        d      }|j                  dd| |j                  g      }|j                  d	d||j                  g      }|j                  |||j                  z         |j                  |||j                  z         t        d
      }|j                  d|
|j                  z        }|j                  d|
|j                  z        }|j                  |d       |j                  |||       |j                  |||       t        d||      }t        d||      }| |z  |
z  t        |       z  |_        | |z  |
z  t        |       z  ||z  |
z  t        |      z  z
  |_        t        |||      }t        || |g||g      }|j!                          t#        |
|z  d|z  t%        |       z  |
t%        |       z  t%        |      z  |z  z   |
t%        |       z  t        |      z  |dz  z  z   |
t%        |      z  t        |       z  |dz  z  z
  |
t        |       z  t        |      z  |z  z   d|
z  |z  z   z  |j&                  d   z
        dk(  sJ t#        |
|z  |t%        |      z  |
t%        |       z  t%        |      z  |z  z   |
t%        |       z  t        |      z  |dz  z  z
  |
t%        |      z  t        |       z  |dz  z  z   |
t        |       z  t        |      z  |z  z   |
|z  z   z  |j&                  d   z
        dk(  sJ |j(                  ||gk(  sJ y )Nzq1 q2r0   r3   zu1 u2r   r   r1   r2   r4   r   r   r@   r   ParPParRr   )r   r   r   r9   r:   rF   r   rG   r%   r"   rH   r   r   r)   r
   r   r;   r   r   rY   r   )r[   r\   r]   r^   q1ddq2ddu1u2u1du2dr+   r,   r-   r   r1   r4   r   r   r@   ri   rj   r   rL   s                          r   test_dub_penrq      sz    G$FBgq)HC+JD$G$FBgq)HCgGAq!sA	C"acc+A	C"acc+AMM!S133YMM!S133Yc
A	CQSS!A	CQSS!AIIaOMM!QMM!QFAq!DFAq!DC!GaK#b'1DC!GaK#b'1AEAIB4GGD1dD!A	RHdD\	:B!AaC1SWqRyR'8'==
CG)CG
CF
"#%&s2wYs2w%6sAv%=>
CG)CG
D
 !#$Q3t8, -/1vvay9 :=>? ? ? AaC3r7Qs2wYs2w%6t%;;
CG)CG
CF
"#%&s2wYs2w%6sAv%=>
CG)CG
D
 !#$T6* +-/VVAY7 8;<= = = 99t$$$r!   c            
      
   t        d      \  } }}t        dd      \  }}}t        d      \  }}}t        d      }	|	j                  dd| |	j                  g      }
|
j                  dd||
j
                  g      }|j                  dd||j                  g      }t        d	      }|j                  |	d
       |j                  d||j                  z        }|j                  ||	|       t        ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z        }t        d|||||f      }| |z  |z  t        |      z  |_        t        |	|      }| ||g}t!        d      } t!        d      }t!        d      }t#        ||      }|j%                          |j'                         }|j)                          t        d      }|j*                  dd d
d|z  |dz  z  dz  d
gk(  sJ |d   j)                         d|z  t-         ||            z  |dt-        d ||      z        z  t/         | |      |      z  dt         ||            z  t/         ||      |      z  z   z  t/         | |      |      z  z   d|z  z  k(  sJ |d   dt         ||            z  t/         | |      |      z  dt1         ||            z  t/         ||      |      z  z   dt/         | |      |      z  t         ||            z  z   t/         ||      |      z  k(  sJ y )Nzq1 q2 q3r0   zr m gr   Yr2   r   r@   Cr   DmcrU   r3   BodyDr[   r\   q3r_   r8         i   
   )r   r   r   r9   r:   r%   r&   r   r"   rG   rH   r	   r   r   r)   r
   r   r   r;   r<   r   mass_matrixr   r   r   )r[   r\   rw   r]   r^   q3drr,   r-   r   rs   r   r@   rt   ru   rB   rv   rb   r   r+   rM   r_   s                         r   test_rolling_discr      s     
+JBB":q1MCcgGAq! 	sA	C"acc+A	C"acc+A	C"acc+A 	c
AIIaO
++eQW
%COOAq! 	1Q3A:qsQTz1Q3A:6AgsAq1c(3E  !S1Wq[3r72E
Q
C	RA	$B	$B	$BQA 
%%'CLLNAMM!A1ac!Q$hqj!"4454q6??	Ac"Q%j1aAbeGnZ1q-AA
3r!u:jA**+ ,,6r!ua,@A 
ACEa4	IJ J J q6bRUmJr!ua$881SA >
 <

RUA
< !":beQ#7!7BqE
!BC
RUA
   r!   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.   rC   rN   rg   rq   r    r!   r   <module>r      sQ   9 9 9 7 ! % D D ' , '
B-&`&!RR<.%b5r!   