
    \h3                         S SK JrJrJrJrJrJrJr  S SKJ	r	  S SK
Jr  S SKJr  S SKJrJrJrJrJrJrJrJrJrJrJr  S SKJr  \S 5       rS rS	 rS
 rS r S r!g)    )symbolsMatrixcossinatansqrtRational)sympify)simplify)solve)dynamicsymbolsReferenceFramePointdotcrossinertiaKanesMethodParticle	RigidBody
LagrangianLagrangesMethod)slowc                     [        S5      u  pp#n[        S5      =u  pVpxpnU Vs/ s H  oR                  U 5      PM     sn=u  pnnnnn[        S5      n[        S5      =u  nnnnnnnU Vs/ s H  nUR                  U 5      PM     snu  nnnnn n![        S5      n"[	        S5      n#U"R                  SSUU"R                  /5      n$U$R                  S	SUU$R                  /5      n%U%R                  S
SUU%R                  /5      n&U#R                  SUU"R                  -  U	U"R                  -  -   U
U"R                  -  -   5      n'U&R                  U"5      n(U%R                  U"5      n)U&R                  U"UU%R                  -  UU%R                  -  -   UU%R                  -  -   5        U'R                  U#5      R                  U"5      n*U'R                  U"UU&R                  -  UU&R                  -  -   UU&R                  -  -   5        U'R                  SUU%R                  -  5      n+U+R                  U'U"U&5        [!        U
[#        U'R                  U+5      U"R                  5      -
  /5      n,[!        U& V-s/ s H  n-[#        U+R%                  U"5      U-5      PM      sn-5      n.[!        U% V-s/ s H!  n-[#        U(U&R                  U"5      -
  U-5      PM#     sn-U" V-s/ s H!  n-[#        U*U'R%                  U"5      -
  U-5      PM#     sn--   5      n/['        U/U5      n0U%R                  U"U)R)                  U05      5        U&R+                  U"U&R                  U"5      R                  U%5      [-        U%R                  U"5      U&R                  U"5      5      -   5        X#-  U$R                  -  n1X!S-  -  S-  n2X!S-  -  S-  n3[/        U&U2U3U25      n4[1        SU'U&UU4U'45      n5U5/n6U'U14/n7[3        U"XVXxU	/UUU/U/U
/U,UUU/U.S9n8U8R5                  U6U75      u  n9n:U8R7                  5       n;U;R8                  U,:X  d   eU;R:                  U.:X  d   eU;R<                  U.R                  U 5      R)                  U8R?                  5       5      :X  d   e['        U;R@                  U;RB                  -   U5      n<U0RE                  5        H  nU<U   U0U   :X  a  M   e   [G        U;RH                  U;RJ                  -   U9-
  U:-
  5      [!        / SQ5      :X  d   eX* [M        U5      -  0n=USU[O        U5      U-  U-   U[M        U5      U-  UU* [O        U5      U-  U-   -  [M        U5      -  USUU* [O        U5      U-  U-   -  [O        U5      -  0n>USUU* [O        U5      U-  U-   -  [M        U5      -  UU* [O        U5      U-  U-   -  [O        U5      -  US0n?USU-  [O        U5      -  SU-  -  [O        SU-  5      US-  -  S-  -   S[M        U5      -  U-  U-  S-  -   USUSUU[O        U5      [O        U5      -  U-  U-  [O        U5      US-  -  -   -  U USU-  [O        U5      -  SU-  -  [O        SU-  5      US-  -  S-  -   S[M        U5      -  U-  U-  S-  -   -  U!U* [O        U5      [M        U5      -  U-  U-  [M        U5      US-  -  -   -  0n@U;RQ                  U=U>U?U@/SSS9u  n$n%USUSUSUSUS0nA[!        / SQ/ SQ/ SQ[O        U5      U-  SSSS[O        U5      * [M        U5      * S/[M        U5      * U-  SSSS[M        U5      [O        U5      * S/S[S        SS5      SSSSSSU-  S-  // SQSSSSSSU-  SS//5      nB[!        / 5      nCU$R)                  UA5      UB:X  d   eU%R)                  WA5      WC:X  d   e[U        U$R)                  WA5      R)                  US[W        S5      -  5      5      RY                  5       SS0:X  d   eU8R7                  SS 9n;U;RQ                  U=U>U?W@/SSS9u  n$n%U$R)                  WA5      WB:X  d   eU%R)                  WA5      WC:X  d   eg s  snf s  snf s  sn-f s  sn-f s  sn-f )!Nz	t r m g vzq1:7zu:6zu1:7NNOAAxisBCCOP      Disc)kd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraints)r   r   r   r         Top_pointA_and_Br      )r   r   r   r   r   r   r   r/   )r   r   r   r   r   r/   r   r   )r   r   r   r   r   r   r/   r   )r   r   r   r   r   r   r   r         GJ)linear_solver)-r   r   diffr   r   	orientnewzxy	locatenew
ang_vel_inset_ang_velpos_fromdtset_velv2pt_theoryr   r   velr   subsset_ang_accr   r   r   r   kanes_equationsto_linearizerf_cf_vf_akindiffdictf_0f_1keysr   f_2f_3r   r   	linearizer	   r
   r   	eigenvals)Dtrmgvq1q2q3q4q5q6qqiq1dq2dq3dq4dq5dq6dqduu1u2u3u4u5u6uiu1du2du3du4du5du6dr   r   r   r   r   r    w_c_n_qdw_b_n_qd	v_co_n_qdr!   rF   uvrG   kindiffsqdotsF_COIJI_C_COr$   BLFLKMfrfr_star
linearizersolq_opu_opqd_opud_opupright_nominalA_solB_solsD                                                                       d/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_linearize.py test_linearize_rolling_disc_kaner      sy    K(MA! "0!77BBBQ>?(@aa(@@ Cc3S2 	uA!/!77BBBQ9:#;2BGGAJ#; Cc3S 	sA	tB	C"acc+A	C"acc+A	C"acc+A	dBqssFRVObf4	5B ||AH||AH MM!RVbf_r!##v-. B""1%I JJq"QSS&2acc6/BqssF*+ 	S!ACC% AMM"a "s2;;q>133//0
1C a0a#aeeAh#a0
1C QGQrs8all1o5r:QGBCD!BY2B7!DE FH(BE MM!X]]5)*MM!Q\\!_''*U1<<?ALLQRO-TTU 3qss7D 
TQA	
TQAQ1a FVRA|4D
Bt*B	Q,r2rl8R3
@B &&r2.MR !!#J>>S   >>S   >>SXXa[--bnn.>????

/
4Cjjl2w%)### JNNZ^^3b87BCviGXXXX 3r7
DBc!BCGCK#%&s2w.CGCK#%&s2w.0D !1"c"gckC'(R01"c"gckC'(R0!E !A#c"g+qs#c!B$iQ&6q&881SW9S=;LQ;NN!!!SWSW_S(,s2wsAv~=>!QqSR[!A#&QrT36)9!);;aBimC>OPQ>QQR1"c"gc"goc)#-BQ>?AE $eU)CT\`aDAqAr1aAq!Q7O ,,,WS[!Q1s2whR!D"gXc\1aAs2wR!DA1aAquQw?,1aBsFAq13 4E 2JE 66/"e+++66/"e+++ 166/*//QtAwY?@JJLQRTUPVVVV !!!5J$eU)C(,t   =DAq66/"e+++66/"e+++s )A
 $<> 1 HDs   a-a1%a&(a(a
c                     [        S5      n [        S5      n[        SS5      n[        S5      u  p4nSn[        S5      n[        S5      nUR	                  US5        UR                  S	S
XR                  /5      n	U	R                  XqUR                  -  5        UR                  SX9R                  -  5      n
U
R                  XU	5        [        SX5      n[        X!-
  /5      nXF-  UR                  -  n[        Xp/U/US9nUR                  U/X4/5      u  nnUR                  SSS9u  n	nnU	[        SS/S[!        U 5      -  U-  S//5      :X  d   eU[        / 5      :X  d   eg )NrV   rf   r/   L, m, t皙#@r   N*r   r   axisr!   pP)q_indu_indr%   T)r.   r   皙#)r   r   r   r   r?   r6   r7   r<   r:   r8   r@   r   r   r   rD   rO   r   )rV   rf   r^   LrS   rQ   rT   r   pNr   r!   r   kdeRr   r   frstarr   inp_vecs                      r   $test_linearize_pendulum_kane_minimalr      s`   		B		B
q
!Ci GA!A 	sA	tBJJq! 	
C"cc+AMM!V 	S!CC% AMM"	$	B #(
C 	
ACCA 
Qd2$s	;B%%rdaVH5LR LLL=MAq'Ac"ga 345555r
??    c                  &   [        S5      u  p[        SSS9u  p#[        S5      u  pE[        SSS9u  pg[        S5      u  pn
Sn[        S5      n[        S5      nUR	                  US	5        [        X-  5      nUR                  S
SXR                  /5      nUR                  SXR                  -  XR                  -  -   5      n[        SUU	5      n[        X$-
  X5-
  /5      n[        UX#/5      nUR	                  UUR                  U5      R                  U5      R!                  U5      5        [        UR                  U5      R#                  5       U-
  /5      n[        UR%                  U5      R'                  U5      R)                  UR                  5      /5      nUR+                  5         UR-                  U
5      nUR+                  5         X-  UR                  -  n[/        X/U/U /U/UUUUS9	nUR1                  U/UU4/5      u  nnXUS	0nUS	US	0nUS	US	0nUR3                  UUU/SSS9u  nnnUR5                  5       [        S	S/SU-  S	//5      :X  d   eU[        / 5      :X  d   eUR3                  UUU/SSSS9u  nnnUR5                  5       [        S	S/SU-  S	//5      :X  d   eU[        / 5      :X  d   eUR3                  UUU/SSS S9u  nnnUR5                  5       [        S	S/SU-  S	//5      :X  d   eU[        / 5      :X  d   eg )Nq1:3r/   levelzu1:3r   r   r   r   r   r   r   P1r   )r   r   r&   r(   r'   r)   acceleration_constraintsr%   Tr,   r   r3   )r-   r.   r   r4   c                 $    U R                  U5      $ NLUsolver   bs     r   <lambda>9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>   s    AIIaLr   )r   r   r   r   r?   r   r6   r7   r:   r8   r9   r   r   r   r=   r>   rB   	magnituderA   expressr   r   r5   r   rD   rO   expand) rV   rW   r^   r_   rf   rg   rm   rn   r   rS   rQ   rT   r   r   theta1r   r!   r   r   dq_dictrF   rG   rH   r   r   r   r   r   r   r   r   r   s                                    r   'test_linearize_pendulum_kane_nonminimalr      sC    F#FBfA.HCF#FBfA.HCi GA!A 	sA	tBJJq! "%[F	C&##/A 	T2cc6BssF?+A	$1	B #(( CC#$G IIaB""1%**734 !**R.**,q01
2C !%%(""1%))!##./
0CLLN ((1+CLLN 	
ACCA 
Qd2$RD!$s3
PB %%rdaVH5LR 2q>D2q>D!S!ELL4u*=t*. ! 0MAq' 88:!Q$q&! 56666r
?? LL4u*=t)-T ! CMAq' 88:!Q$q&! 56666r
??LL4u*=)-*./H ! JMAq'
 88:!Q$q&! 56666r
??r   c                  ^   [        S5      n [        SS5      n[        S5      u  p#nSn[        S5      n[        S5      nUR	                  US5        UR                  SS	XR                  /5      nUR                  XaUR                  -  5        UR                  S
X(R                  -  5      n	U	R                  XvU5        [        SX5      n
[        Xj5      n[        X/XU-  UR                  -  4/US9nUR                  5         UR                  U /U/SS9u  pn[!        U5      [#        SS/S[%        U 5      -  U-  S//5      :X  d   eU[#        / 5      :X  d   eUR                  U /U/SSS9u  pn[!        U5      [#        SS/S[%        U 5      -  U-  S//5      :X  d   eU[#        / 5      :X  d   eg )NrV   r/   r   r   r   r   r   r   r   r!   r   )	forcelistframeT)r.   r   r3   )r.   r4   )r   r   r   r   r?   r6   r7   r<   r:   r8   r@   r   r   r   form_lagranges_equationsrO   r   r   r   )rV   r^   r   rS   rQ   rT   r   r   r   r!   r   LagLMr   r   s                  r   (test_linearize_pendulum_lagrange_minimalr     s   		B
q
!Ci GA!A 	sA	tBJJq! 	
C"cc+AMM!W 	S!CC% AMM"	$	B Q
C	dQ3qss7|nA	FB! LL"udL;MA'A;&1a&4B<>1*=!>????r
?? LL"ud$LOMA'A;&1a&4B<>1*=!>????r
??r   c                  P   [        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        [        X-  5      n
UR                  S	S
XR                  /5      n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/XXW-  UR                  -  4/US9nUR#                  5         XUSUSUSUR%                  U5      SUR%                  U5      S0nUR'                  US9nUR)                  U5        UR+                  U/U/U /U/USS9u  nnn[-        U5      [        SS/SU-  S//5      :X  d   eU[        / 5      :X  d   eUR+                  U/U/U /U/USS S9u  nnn[-        U5      [        SS/SU-  S//5      :X  d   eU[        / 5      :X  d   eg )Nr   r/   r   r   r   r   r   r   r   r   r   r   r"   )
hol_coneqsr   r   )r-   T)r-   r.   r   c                 $    U R                  U5      $ r   r   r   s     r   r   =test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>L  s    !"1r   )r-   r.   r4   )r   r   r   r   r?   r   r6   r7   r:   r8   r9   r=   r>   r   r   r   r   r   r5   solve_multipliersupdaterO   r   )rV   rW   r^   r_   r   rS   rQ   rT   r   r   r   r   r!   r   rF   r   r   r-   lam_opr   r   s                        r   +test_linearize_pendulum_lagrange_nonminimalr   )  s9   F#FBfA.HCi GA!AsA	tBJJq!"%[F	C&##/A
T2cc6BssF?+AIIaB""1%&	$	B
"a%"a%-!Q$&'
(C
Q
C	2h3qs133w<.XY	ZB!r1c1c1chhqk1chhqk1MH!!8!4FOOFLL"urdSEt ! -MAq'A;&1a&461+!67777r
?? LL"urdSEH)- >. ! /MAq' A;&1a&461+!67777r
??r   c                     [        S5      =u  pp#[        SS5      =u  pEpg[        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                  SSX-R                  /5      n[        S	5      nUR                  US
5        UR                  SXR                  -  5      nUR                  XU5        [        XS-  US-  -  U	S-  US-  -  U	S-  US-  -  5      n[        SUXUU45      nU	* U
-  U-  [        U5      -  Ul        [        UU5      n[!        UU5      nUR#                  5         U S
US
US
US
US
UR%                  5       S
UR%                  5       S
UR%                  5       S
0nUR'                  X7USS9S
   n[)        / SQ/ SQ/ SQS
S
S
S
SU-  S
/S
SU
-  SU-  -  S
SU-  S-  S
S
// SQ/5      nUU:X  d   eg )Nzq1 q2 q3r/   zr m gr   Yr   r   r   r   r   Dmcr#   r"   BodyDT)r   qd_indr-   r.   )r   r   r   r/   r   r   )r   r   r   r   r/   r   )r   r   r   r   r   r/   ir*   r+   )r   r   r   r   r   r   )r   r   r   r6   r7   r8   r9   r   r?   r:   r@   r   r   r   potential_energyr   r   r   r5   rO   r   )rV   rW   rX   r\   r^   r_   r`   rd   rR   rS   rT   r   r   r   r   r   r   rz   r   r   lr-   r   r   s                           r   $test_linearize_rolling_disc_lagranger   Q  s   #J//JBB'
A66MCcgGA!sA	C"cc+A	C"cc+A	C"cc+Ac
AIIaO
++eQW
%COOA!q51a4<QAq1uq!t|<AgsA1c(3E S1Wq[3r72E
Q
CQA  Ar1b!QQ
Asxxz1chhj!=H 	
!4HKA
$$$aAr#vq)bdAaCj!QsU1Wa3$& 'C 8O8r   N)"sympyr   r   r   r   r   r   r	   sympy.core.sympifyr
   sympy.simplify.simplifyr   sympy.solvers.solversr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r   r   r   r   r   r    r   r   <module>r      se    A A A & , '    & , ,B#JNb#L&P#r   