
    \h'                         S SK JrJrJrJrJrJrJrJr  S SK	J
r
Jr  S SKJr  S SKJr  S SKJrJrJr  S SKJr  S SKJr  S SKJr  S	 rS
 rS rS rS rS rg)    )dynamicsymbolsReferenceFramePoint	RigidBodyLagrangesMethodParticleinertia
Lagrangian)
DerivativeFunction)pi)symbols)cossintan)Matrix)simplify)raisesc                    ^^^ [        S5      u  pn[        S5      m[        S5      [        S5      pCUR                  US5        [	        S[        S5      U5      mTR
                  R                  X@[        T5      UR                  -  [        T5      UR                  -  -
  -  5        X-  TR
                  R                  U5      R                  UR                  5      -  Tl        [        UT5      m[        [         UUU4S j5        g )Nl m gqNOr   Pc                     > [        T T/TS9$ )Nbodies)r   )Lr   r   s   c/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_lagrange.py<lambda>*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GA!A#c
qIIaOeCj!$AGGOOACFQSSL3q6ACC<789!1!1!!4!8!8!==A1aA
:@Ar"   c                     [        S5      u  p[        SS5      u  p#[        S5      u  pEpgn[        S5      n	U	R                  SS[        S-  U-
  U	R
                  /5      n
U
R                  SSU* U
R
                  /5      n[        S	5      nUR                  XU
R                  -  5        XFS-  -  S-  UR
                  -  UR
                  -  n[        S
XXMU45      nXE-  Xp-
  -  [        U5      -  Ul        [        X5      nX/nXU-  -
  /n[        UUUS9nUR                  5         UR                  5       nUR!                  5         US   SU-  [        U5      -  S-  :X  d   eg )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   r2   r5   r6   Ir7   r   r   r8   r=   s                      r   test_disc_on_an_incline_planerD      sX    i(HA	1-JB0A! 	sA	C"Q$,!45A	C5&!##/A
 
tBJJqqss(	qD
QSS133A#raR)A
 !%3u:5A1A 

A%i-J14A 
%%'CLLNq6QqSU^A%%%%r"   c                  n   [        S5      u  p[        SS5      u  p#[        S5      u  pEn[        S5      nUR                  SSXR                  /5      nUR                  XrUR                  -  5        [        S5      n	U	R                  US	5        U	R                  S
XHR                  -  5      n
U
R                  XU5        [        SX5      nU* U-  U-  [        U 5      -  Ul        [        X{5      n[        X/5      nUR!                  5         UR#                  5       nUS   U* [%        U 5      -  U-  :X  d   eg )Nzq uzq u r1   r   r   r2   r3   r   r   r   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   r2   r   r   rF   r   lmRHSs                  r   test_simp_penrO   J   s    % DAFA&FBgGA!
 	sA	C!SS*AMM!!##X
 	c
AIIaO	CSS!AMM! 
$	B#'A+A.B1A 
C	 B!
&&(Cq6aRAYq[   r"   c            	         [        S5      u  p[        SSS9u  p#[        S5      u  pEnSn[        S5      n[        S5      n	U	R	                  US5        U	R                  S	XR                  -  XR                  -  -   5      n
U
R	                  XR                  U	5      R                  U5      5        [        S
X5      n[        U S-  US-  -   US-  -
  /5      n[        X5      n[        XU/UXU-  UR                  -  4/US9nUR                  5         UR                  S   n[        U[!        XU5      -  SU-  -
  SU-  U -  -   /U[!        XU5      -  SU-  U-  -   //5      nUR"                  U:X  d   e[        SU -  SUS-  -  -   SUS-  -  -   SU S-  -  U-  SUS-  -  U-  -   -  /5      n[%        UR'                  SS95      [%        U5      :X  d   eg )Nzq1:3r1   )levelzL, m, tg#@r   zN*r   P1pPr4   )r8   	forcelistframe)r   r   g3@   r   )sol_type)r   r   r   r   r#   rH   r&   r'   r(   dtr   r   r
   r   r<   lam_vecr   eomr   solve_multipliers)q1q2q1dq2dr   r-   tr.   r   pNr   rS   f_cLagLMlam1eom_sollam_sols                     r   test_nonminimal_pendulumrh   s   s   F#FBfA.HCi GA!AsA	tBJJq!
T2cc6BssF?+AIIaB""1%&	$	B
"a%"a%-!Q$&'
(C
Q
C	2h3Q3qss7|nA
/B!::dDqB1--A5$r	ABB1--$r	9:< =G66WtBw36)Ac1fH4qQwqy1RU7197LMNOGB((((;<@QQQQr"   c                     [        S5      u  p[        SS5      u  p#[        SS5      u  pE[        S5      u  pg[        SS5      u  p[        S5      u  pn[        S5      nUR                  SSXR                  /5      nUR                  S	SXR                  /5      nUR                  XUR                  -  5        UR                  XUR                  -  5        [        S
5      nUR                  SXR                  -  5      nUR                  SXR                  -  5      nUR                  US5        UR                  UX5        UR                  UX5        [        SUU5      n[        SUU5      nU* U-  U
-  [        U 5      -  Ul        U* U-  U
-  [        U 5      -  X-  U
-  [        U5      -  -
  Ul        [        UUU5      n[        UX/UU/S9nUR!                  5         [#        X-  SU-  [%        U 5      -  U
[%        U 5      -  [%        U5      -  U-  -   U
[%        U 5      -  [        U5      -  US-  -  -   U
[%        U5      -  [        U 5      -  US-  -  -
  U
[        U 5      -  [        U5      -  U-  -   SU
-  U-  -   -  UR&                  S   -
  5      S:X  d   e[#        X-  U[%        U5      -  U
[%        U 5      -  [%        U5      -  U-  -   U
[%        U 5      -  [        U5      -  US-  -  -
  U
[%        U5      -  [        U 5      -  US-  -  -   U
[        U 5      -  [        U5      -  U-  -   X-  -   -  UR&                  S   -
  5      S:X  d   eUR(                  UU/:X  d   eg )Nzq1 q2r1   r4   zu1 u2r   r   r2   r3   r5   r   r   rA   r   ParPParRr   )r   r   r   r:   r;   rG   r   rH   r&   r#   rI   r   r   r*   r
   r   r<   r   r   rZ   r   )r\   r]   r^   r_   q1ddq2ddu1u2u1du2dr,   r-   r.   r   r2   r5   r   r   rA   rj   rk   r   rM   s                          r   test_dub_penrr      sT    G$FBgq)HC+JDG$FBgq)HCgGA!sA	C"cc+A	C"cc+AMM!133YMM!133Yc
A	CSS!A	CSS!AIIaOMM!QMM!QFAq!DFAq!DC!GaK#b'1DC!GaK#b'1AEAIB4GGD1dD!A	RHdD\	:B!AC1SWqRyR'8'==
CG)CG
CF
"#%&s2wYs2w%6sAv%=>
CG)CG
D
 !#$Q3t8, -/1vvay9 :=>? ? ? AC3r7Qs2wYs2w%6t%;;
CG)CG
CF
"#%&s2wYs2w%6sAv%=>
CG)CG
D
 !#$6* +-/VVAY7 8;<= = = 99t$$$r"   c            
         [        S5      u  pn[        SS5      u  p4n[        S5      u  pgn[        S5      n	U	R                  SSX	R                  /5      n
U
R                  SSXR
                  /5      nUR                  SSX+R                  /5      n[        S	5      nUR                  U	S
5        UR                  SXkR                  -  5      nUR                  XU5        [        XS-  US-  -  US-  US-  -  US-  US-  -  5      n[        SXXU45      nU* U-  U-  [        U5      -  Ul        [        U	U5      nXU/n[!        S5      n [!        S5      n[!        S5      n[#        UU5      nUR%                  5         UR'                  5       nUR)                  5         [        S5      nUR*                  SS S
SU-  US-  -  S-  S
/:X  d   eUS   R)                  5       SU-  [-        U" U5      5      -  US[-        SU" U5      -  5      -  [/        U " U5      U5      -  S[        U" U5      5      -  [/        U" U5      U5      -  -   -  [/        U " U5      U5      -  -   SU-  -  :X  d   eUS   S[        U" U5      5      -  [/        U " U5      U5      -  S[1        U" U5      5      -  [/        U" U5      U5      -  -   S[/        U " U5      U5      -  [        U" U5      5      -  -   [/        U" U5      U5      -  :X  d   eg )Nzq1 q2 q3r1   zr m gr   Yr3   r   rA   Cr   DmcrV   r4   BodyDr\   r]   q3r`   r9         i   
   )r   r   r   r:   r;   r&   r'   r   r#   rH   rI   r	   r   r   r*   r
   r   r   r<   r=   r   mass_matrixr   r   r   )r\   r]   rx   r^   r_   q3drr-   r.   r   rt   r   rA   ru   rv   rC   rw   rc   r   r,   rN   r`   s                         r   test_rolling_discr      s     
+JBB":q1MCcgGA! 	sA	C"cc+A	C"cc+A	C"cc+A 	c
AIIaO
++eQW
%COOA! 	Q3A:qsQTz1Q3A:6Agsqc(3E  !S1Wq[3r72E
Q
C	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/   rD   rO   rh   rr   r    r"   r   <module>r      sQ   9 9 9 7 ! % D D ' , '
B-&`&!RR<.%b5r"   